四驱板使用fishbot四驱代码，无法正常发送odom和imu话题
-
使用四驱板并烧录fishbot的四驱板的代码，仅更改了fishbot_config.h中的
CONFIG_DEFAULT_MOTOR_PARAM_PULSE_RATION将其值改为了180000（GMR编码器）和2340（霍尔编码器）和将CONFIG_DEFAULT_TRANSPORT_MODE改为了seral，但是我发现其不能正常发布odom话题，无论如何移动，odom话题一直不变，除了sec，其余都为0。一开始我认为是我的编码器出现了问题，但是我不确定。不过我还是换了电机。我将gmr编码器换成了普通的霍尔编码器，结果依然不行。尝试过使用串口debug,但是921600的波特率好像不支持我的串口助手，导致出现乱码。将波特率改为115200却出现了数据溢出，于是放弃了。
由于pid控制电机似乎仍然管用，我决定使用imu加激光雷达传感器作为里程计，尝试使用microROS发布imu话题。可似乎仍然不能正常发布。IMU发布话题的代码与odom话题的代码封装在一个函数中，可在topic list中，却只有odom话题，而没有imu话题。我认为是imu.isEnable()没有返回1值，但不知道为什么。以下是我依据鱼总的教程添加的代码，由于能编译成功，和ImuDriver.cpp和ImuDriver.h与二驱的代码一致，我就不过多赘述。以下放出我在fishbot.cpp中更改的代码:
定义变量和对象rcl_publisher_t imu_publisher; // 用于发布机器人的IMU信息（Imu） sensor_msgs__msg__Imu imu_msg; MPU6050 mpu(Wire); // 初始化MPU6050对象 ImuDriver imu(mpu); // 初始化Imu对象 imu_t imu_data; // IMU 数据对象在
bool setup_fishbot()中加入imu初始化imu.begin(18, 19);//imu初始化在
bool create_fishbot_transport()的改动(以下所有注释部分为添加或改动的代码)bool create_fishbot_transport() { String nodename = config.ros2_nodename(); String ros2namespace = config.ros2_namespace(); String twist_topic = config.ros2_twist_topic_name(); String odom_topic = config.ros2_odom_topic_name(); String odom_frameid_str = config.ros2_odom_frameid(); //改动 String odom_child_frameid_str = config.ros2_odom_child_frameid(); odom_msg.header.frame_id = micro_ros_string_utilities_set(odom_msg.header.frame_id, odom_frameid_str.c_str()); odom_msg.child_frame_id = micro_ros_string_utilities_set(odom_msg.child_frame_id, odom_child_frameid_str.c_str()); imu_msg.header.frame_id = micro_ros_string_utilities_set(imu_msg.header.frame_id, "imu"); // odom_msg.header.frame_id = micro_ros_string_utilities_set(odom_msg.header.frame_id, odom_frameid_str.c_str()); const unsigned int timer_timeout = config.odom_publish_period(); delay(500); allocator = rcl_get_default_allocator(); // 调用 rclc_timer_init_default 函数初始化 ROS 2 定时器，传入支持库、定时器周期和回调函数 RCSOFTCHECK(rclc_publisher_init_best_effort( &imu_publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Imu), "imu")); RCSOFTCHECK(rclc_support_init(&support, 0, NULL, &allocator)); RCSOFTCHECK(rclc_node_init_default(&node, nodename.c_str(), ros2namespace.c_str(), &support)); RCSOFTCHECK(rclc_publisher_init_best_effort( &odom_publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, Odometry), odom_topic.c_str())); RCSOFTCHECK(rclc_subscription_init_default( &twist_subscriber, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist), twist_topic.c_str())); RCSOFTCHECK(rclc_service_init_default( &config_service, &node, ROSIDL_GET_SRV_TYPE_SUPPORT(fishbot_interfaces, srv, FishBotConfig), "/fishbot_config")); RCSOFTCHECK(rclc_timer_init_default(&timer, &support, RCL_MS_TO_NS(timer_timeout), callback_sensor_publisher_timer_)); //RCSOFTCHECK(rclc_timer_init_default(&timer, &support, RCL_MS_TO_NS(timer_timeout), callback_odom_publisher_timer_)); RCSOFTCHECK(rclc_executor_init(&executor, &support.context, 3, &allocator)); RCSOFTCHECK(rclc_executor_add_subscription(&executor, &twist_subscriber, &twist_msg, &callback_twist_subscription_, ON_NEW_DATA)); RCSOFTCHECK(rclc_executor_add_timer(&executor, &timer)); RCSOFTCHECK(rclc_executor_add_service(&executor, &config_service, &config_req, &config_res, callback_config_service_)); return true; }在
void loop_fishbot_control()中加入imu.update();bool microros_setup_transport_serial_()的更改bool microros_setup_transport_serial_() { String nodename = config.ros2_nodename(); String ros2namespace = config.ros2_namespace(); String twist_topic = config.ros2_twist_topic_name(); String odom_topic = config.ros2_odom_topic_name(); String odom_frameid_str = config.ros2_odom_frameid(); // 用于将odom_frameid_str的C字符串值设置为odom_msg的帧ID。 // 该函数会将odom_msg中的帧ID指针所指向的字符串缓冲区中的数据替换为odom_frameid_str的C字符串值，并返回指向新字符串的指针。 // 这样可以确保odom_msg的帧ID与配置中的值一致。 odom_msg.header.frame_id = micro_ros_string_utilities_set(odom_msg.header.frame_id, odom_frameid_str.c_str()); odom_msg.child_frame_id = micro_ros_string_utilities_set(odom_msg.child_frame_id, odom_frameid_str.c_str()); imu_msg.header.frame_id = micro_ros_string_utilities_set(imu_msg.header.frame_id, "imu"); const unsigned int timer_timeout = config.odom_publish_period(); // 初始化数据接收配置 config_req.key = micro_ros_string_utilities_init_with_size(keyvalue_capacity); config_req.value = micro_ros_string_utilities_init_with_size(keyvalue_capacity); config_res.key = micro_ros_string_utilities_init_with_size(keyvalue_capacity); config_res.value = micro_ros_string_utilities_init_with_size(keyvalue_capacity); uint32_t serial_baudraate = config.serial_baudrate(); Serial.updateBaudRate(serial_baudraate); if (!set_microros_serial_transports(Serial)) { return false; } return true; }加入了以下函数
void callback_sensor_publisher_timer_(rcl_timer_t *timer, int64_t last_call_time) { RCLC_UNUSED(last_call_time); if (timer != NULL) { // 用于获取当前的时间戳，并将其存储在消息的头部中 int64_t stamp = rmw_uros_epoch_millis(); // 获取机器人的位置和速度信息，并将其存储在一个ROS消息（odom_msg）中 odom_t odom = kinematics.odom(); odom_msg.header.stamp.sec = static_cast<int32_t>(stamp / 1000); // 秒部分 odom_msg.header.stamp.nanosec = static_cast<uint32_t>((stamp % 1000) * 1e6); // 纳秒部分 odom_msg.pose.pose.position.x = odom.x; odom_msg.pose.pose.position.y = odom.y; odom_msg.pose.pose.orientation.w = odom.quaternion.w; odom_msg.pose.pose.orientation.x = odom.quaternion.x; odom_msg.pose.pose.orientation.y = odom.quaternion.y; odom_msg.pose.pose.orientation.z = odom.quaternion.z; odom_msg.twist.twist.angular.z = odom.angular_speed; odom_msg.twist.twist.linear.x = odom.linear_x_speed; odom_msg.twist.twist.linear.y = odom.linear_y_speed; display.updateBotAngular(odom.angular_speed); display.updateBotLinear(odom.linear_x_speed); display.updateBotLinear(odom.linear_y_speed); RCSOFTCHECK(rcl_publish(&odom_publisher, &odom_msg, NULL)); if (imu.isEnable()) { imu.getImuDriverData(imu_data); imu_msg.header.stamp.sec = static_cast<int32_t>(stamp / 1000); // 秒部分 imu_msg.header.stamp.nanosec = static_cast<uint32_t>((stamp % 1000) * 1e6); // 纳秒部分 imu_msg.angular_velocity.x = imu_data.angular_velocity.x; imu_msg.angular_velocity.x = imu_data.angular_velocity.x; imu_msg.angular_velocity.x = imu_data.angular_velocity.x; imu_msg.linear_acceleration.x = imu_data.linear_acceleration.x; imu_msg.linear_acceleration.y = imu_data.linear_acceleration.y; imu_msg.linear_acceleration.z = imu_data.linear_acceleration.z; imu_msg.orientation.x = imu_data.orientation.x; imu_msg.orientation.y = imu_data.orientation.y; imu_msg.orientation.z = imu_data.orientation.z; imu_msg.orientation.w = imu_data.orientation.w; RCSOFTCHECK(rcl_publish(&imu_publisher, &imu_msg, NULL)); } } }将
void callback_odom_publisher_timer_(rcl_timer_t *timer, int64_t last_call_time)注释了。为了防止占用大量篇幅，以上是我代码的主要改动部分。如有需要我会张贴完整的代码。
@小鱼 -
- 编码器配置从默认值改成180000，肯定会导致数据类型溢出，导致速度计算和积分异常，你需要从头整理下代码，了解代码思想，找到数据溢出部分修改。另外可以参考fishbot的配套电机参数，这套代码在我们的小车上是调通了的。
2.关于添加IMU传感器，这个我这边可以代为完成，这个功能本身在待完成Lists中。另外你如果担心IMU问题，可以下载单独的IMU程序进行测试。
3.关于通信模式和参数的更改，最好使用配置助手，直接更待宏定义，可能会更改不成功，因为参数会被写入到nvs中，二次启动将直接从nvs中读取，读取失败才会使用给定的配置默认值。
-
@小鱼 编码器的问题我解决了，是我的线接错了[笑哭]，但是imu还是不行，我直接读取imu数据发送至串口可以正常发送的，说明imu没有问题，应该是代码问题。之前看到imu和屏幕似乎使用了同一个引脚，我认为有冲突于是把显示器的代码注释了，可依然不行，然后我发现以下代码中
imu_msg.angular_velocity.x出现重复，也进行了修改，但是imu话题依然没有正确发布。之后，我把if (imu.isEnable())进行了注释，依然不发布imu话题。if (imu.isEnable()) { imu.getImuDriverData(imu_data); imu_msg.header.stamp.sec = static_cast<int32_t>(stamp / 1000); // 秒部分 imu_msg.header.stamp.nanosec = static_cast<uint32_t>((stamp % 1000) * 1e6); // 纳秒部分 imu_msg.angular_velocity.x = imu_data.angular_velocity.x; imu_msg.angular_velocity.x = imu_data.angular_velocity.x; imu_msg.angular_velocity.x = imu_data.angular_velocity.x; imu_msg.linear_acceleration.x = imu_data.linear_acceleration.x; imu_msg.linear_acceleration.y = imu_data.linear_acceleration.y; imu_msg.linear_acceleration.z = imu_data.linear_acceleration.z; imu_msg.orientation.x = imu_data.orientation.x; imu_msg.orientation.y = imu_data.orientation.y; imu_msg.orientation.z = imu_data.orientation.z; imu_msg.orientation.w = imu_data.orientation.w; RCSOFTCHECK(rcl_publish(&imu_publisher, &imu_msg, NULL)); }之后，我把
if (imu.isEnable())进行了注释，依然不发布imu话题。我认为其他文件没有出错，因为imu相关的文件应该只有ImuDriver.h和ImuDriver.cpp还有MPU6050_light.h，而这些文件是直接copy github的，应该不会出错，所以问题大概率为fishbot.cpp的问题，以下是我的全部代码(说实话，对照着二驱板的开源来来回回看了很多次了，不知道问题在哪，编译也不报错。)/** * @file fishbot.cpp * @author fishros (fishros@foxmail.com) * @brief 核心文件，硬件控制以及通讯控制 * @version V1.0.0 * @date 2023-01-04 * * @copyright Copyright (c) fishros.com & fishros.org.cn 2023 * */ #include "fishbot.h" /*==================MicroROS消息============================*/ sensor_msgs__msg__Imu imu_msg; geometry_msgs__msg__Twist twist_msg; nav_msgs__msg__Odometry odom_msg; char odom_frame_id[16]; fishbot_interfaces__srv__FishBotConfig_Response config_res; fishbot_interfaces__srv__FishBotConfig_Request config_req; static micro_ros_utilities_memory_conf_t conf = {0}; static int keyvalue_capacity = 100; /*==================MicroROS订阅发布者服务========================*/ rcl_publisher_t odom_publisher; rcl_publisher_t imu_publisher; // 用于发布机器人的IMU信息（Imu） rcl_subscription_t twist_subscriber; rcl_service_t config_service; rcl_wait_set_t wait_set; /*==================MicroROS相关执行器&节点===================*/ rclc_executor_t executor; rclc_support_t support; rcl_allocator_t allocator; rcl_node_t node; rcl_timer_t timer; enum states { WAITING_AGENT, AGENT_AVAILABLE, AGENT_CONNECTED, AGENT_DISCONNECTED } state; /*========================FishBot控制相关====================*/ PidController pid_controller[4]; Esp32PcntEncoder encoders[4]; Esp32McpwmMotor motor; Kinematics kinematics; FishBotConfig config; //FishBotDisplay display; BluetoothSerial SerialBT; float battery_voltage; OneButton button(0, true); MPU6050 mpu(Wire); // 初始化MPU6050对象 ImuDriver imu(mpu); // 初始化Imu对象 imu_t imu_data; // IMU 数据对象 void WiFiEventCB(WiFiEvent_t event) { Serial.println("WIFI EVENT!"); switch (event) { case SYSTEM_EVENT_STA_GOT_IP: //display.updateWIFIIp(WiFi.localIP().toString()); break; case SYSTEM_EVENT_STA_LOST_IP: //display.updateWIFIIp("wait connect!"); break; }; } void doubleClick() { fishlog_debug("key", "doubleClick() detected."); if (config.microros_transport_mode() == CONFIG_TRANSPORT_MODE_WIFI_UDP_CLIENT) { config.config("microros_mode", "serial"); } else { config.config("microros_mode", "udp_client"); } esp_restart(); } bool setup_fishbot() { // 1.初始化 Serial.begin(115200); fishlog_set_target(Serial); config.init(CONFIG_NAME_NAMESPACE); Serial.println(FIRST_START_TIP); Serial.println(config.config_str()); //display.init(); //display.updateTransMode(config.microros_transport_mode()); //display.updateStartupInfo(); button.attachDoubleClick(doubleClick); // 2.设置IO 电机&编码器 motor.attachMotor(0, 12, 22); motor.attachMotor(1, 16, 17); motor.attachMotor(2, 25, 33); motor.attachMotor(3, 26, 27); encoders[0].init(0, 14, 23); encoders[1].init(1, 15, 13); encoders[2].init(2, 36, 39); encoders[3].init(3, 32, 35); imu.begin(18, 19);//imu初始化 // 3.设置PID for (int i = 0; i < 4; i++) { pid_controller[i].update_target(0.0); pid_controller[i].update_pid(config.kinematics_pid_kp(), config.kinematics_pid_ki(), config.kinematics_pid_kd()); pid_controller[i].out_limit(-config.kinematics_pid_out_limit(), config.kinematics_pid_out_limit()); kinematics.set_motor_param(i, config.kinematics_reducation_ration(), config.kinematics_pulse_ration(), config.kinematics_wheel_diameter()); } // 4.设置运动学参数 kinematics.set_kinematic_param(config.kinematics_wheel_distance()); // 7.设置电压测量引脚 pinMode(34, INPUT); analogReadResolution(12); analogSetAttenuation(ADC_11db); battery_voltage = 5.02 * ((float)analogReadMilliVolts(34) * 1e-3); return true; } static void deal_command(char key[32], char value[32]) { if (strcmp(key, "command") == 0) { if (strcmp(value, "restart") == 0) { esp_restart(); } else if (strcmp(value, "read_config") == 0) { Serial.print(config.config_str()); } return; } else { String recv_key(key); String recv_value(value); config.config(recv_key, recv_value); Serial.print("$result=ok\n"); } } bool setup_fishbot_transport() { bool setup_success = true; if (config.microros_transport_mode() == CONFIG_TRANSPORT_MODE_WIFI_UDP_CLIENT) { fishlog_set_target(Serial); WiFi.onEvent(WiFiEventCB); setup_success = microros_setup_transport_udp_client_(); //display.updateTransMode("udp_client"); } if (config.microros_transport_mode() == CONFIG_TRANSPORT_MODE_SERIAL) { SerialBT.begin(config.board_name()); fishlog_set_target(SerialBT); microros_setup_transport_serial_(); //display.updateTransMode("serial"); } return true; } bool create_fishbot_transport() { String nodename = config.ros2_nodename(); String ros2namespace = config.ros2_namespace(); String twist_topic = config.ros2_twist_topic_name(); String odom_topic = config.ros2_odom_topic_name(); String odom_frameid_str = config.ros2_odom_frameid(); String odom_child_frameid_str = config.ros2_odom_child_frameid(); odom_msg.header.frame_id = micro_ros_string_utilities_set(odom_msg.header.frame_id, odom_frameid_str.c_str()); odom_msg.child_frame_id = micro_ros_string_utilities_set(odom_msg.child_frame_id, odom_child_frameid_str.c_str()); imu_msg.header.frame_id = micro_ros_string_utilities_set(imu_msg.header.frame_id, "imu"); odom_msg.header.frame_id = micro_ros_string_utilities_set(odom_msg.header.frame_id, odom_frameid_str.c_str()); const unsigned int timer_timeout = config.odom_publish_period(); delay(500); allocator = rcl_get_default_allocator(); // 调用 rclc_timer_init_default 函数初始化 ROS 2 定时器，传入支持库、定时器周期和回调函数 RCSOFTCHECK(rclc_publisher_init_best_effort( &imu_publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Imu), "imu")); RCSOFTCHECK(rclc_support_init(&support, 0, NULL, &allocator)); RCSOFTCHECK(rclc_node_init_default(&node, nodename.c_str(), ros2namespace.c_str(), &support)); RCSOFTCHECK(rclc_publisher_init_best_effort( &odom_publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, Odometry), odom_topic.c_str())); RCSOFTCHECK(rclc_subscription_init_default( &twist_subscriber, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist), twist_topic.c_str())); RCSOFTCHECK(rclc_service_init_default( &config_service, &node, ROSIDL_GET_SRV_TYPE_SUPPORT(fishbot_interfaces, srv, FishBotConfig), "/fishbot_config")); RCSOFTCHECK(rclc_timer_init_default(&timer, &support, RCL_MS_TO_NS(timer_timeout), callback_sensor_publisher_timer_)); //RCSOFTCHECK(rclc_timer_init_default(&timer, &support, RCL_MS_TO_NS(timer_timeout), callback_odom_publisher_timer_)); RCSOFTCHECK(rclc_executor_init(&executor, &support.context, 3, &allocator)); RCSOFTCHECK(rclc_executor_add_subscription(&executor, &twist_subscriber, &twist_msg, &callback_twist_subscription_, ON_NEW_DATA)); RCSOFTCHECK(rclc_executor_add_timer(&executor, &timer)); RCSOFTCHECK(rclc_executor_add_service(&executor, &config_service, &config_req, &config_res, callback_config_service_)); return true; } bool destory_fishbot_transport() { rmw_context_t *rmw_context = rcl_context_get_rmw_context(&support.context); (void)rmw_uros_set_context_entity_destroy_session_timeout(rmw_context, 0); RCSOFTCHECK(rcl_publisher_fini(&odom_publisher, &node)); RCSOFTCHECK(rcl_publisher_fini(&imu_publisher, &node)); RCSOFTCHECK(rcl_subscription_fini(&twist_subscriber, &node)); RCSOFTCHECK(rcl_service_fini(&config_service, &node)); RCSOFTCHECK(rcl_timer_fini(&timer)); RCSOFTCHECK(rclc_executor_fini(&executor)); RCSOFTCHECK(rcl_node_fini(&node)); rclc_support_fini(&support); return true; } void loop_fishbot_control() { static float out_motor_speed[4]; static uint64_t last_update_info_time = millis(); static uint8_t index = 0; kinematics.update_motor_ticks(micros(), encoders[0].getTicks(), encoders[1].getTicks(), encoders[2].getTicks(), encoders[3].getTicks()); for (index = 0; index < 4; index++) { // 目标速度为0时停止控制，解决 #https://fishros.org.cn/forum/topic/1372 问题 if (pid_controller[index].target_ == 0) { out_motor_speed[index] = 0; } else { // 使用 pid_controller 控制器对电机速度进行 PID 控制 out_motor_speed[index] = pid_controller[index].update(kinematics.motor_speed(index)); } // 将 PID 控制器的输出值作为电机的目标速度进行控制 motor.updateMotorSpeed(index, out_motor_speed[index]); // fishlog_debug("pid", "index:%d target:%f current:%f out=%f", index, pid_controller[index].target_, kinematics.motor_speed(index), out_motor_speed[index]); } // 电量信息 if (out_motor_speed[0] == 0 && out_motor_speed[1] == 0) { battery_voltage = 5.02 * ((float)analogReadMilliVolts(34) * 1e-3); //display.updateBatteryInfo(battery_voltage); } // 更新系统信息 //display.updateCurrentTime(rmw_uros_epoch_millis()); //display.updateDisplay(); button.tick(); imu.update(); } void loop_fishbot_transport() { static char result[10][32]; static int config_result; while (Serial.available()) { int c = Serial.read(); config_result = config.loop_config_uart(c, result); if (config_result == CONFIG_PARSE_OK) { deal_command(result[0], result[1]); } else if (config_result == CONFIG_PARSE_ERROR) { Serial.print("$result=error parse\n"); } } switch (state) { case WAITING_AGENT: EXECUTE_EVERY_N_MS(500, state = (RMW_RET_OK == rmw_uros_ping_agent(100, 1)) ? AGENT_AVAILABLE : WAITING_AGENT;); break; case AGENT_AVAILABLE: state = (true == create_fishbot_transport()) ? AGENT_CONNECTED : WAITING_AGENT; if (state == WAITING_AGENT) { destory_fishbot_transport(); }; break; case AGENT_CONNECTED: EXECUTE_EVERY_N_MS(200, state = (RMW_RET_OK == rmw_uros_ping_agent(100, 1)) ? AGENT_CONNECTED : AGENT_DISCONNECTED;); if (state == AGENT_CONNECTED) { if (!rmw_uros_epoch_synchronized()) { RCSOFTCHECK(rmw_uros_sync_session(1000)); if (rmw_uros_epoch_synchronized()) { setTime(rmw_uros_epoch_millis() / 1000 + SECS_PER_HOUR * 8); fishlog_debug("fishbot", "current_time:%ld", rmw_uros_epoch_millis()); } delay(10); return; } RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100))); } break; case AGENT_DISCONNECTED: destory_fishbot_transport(); state = WAITING_AGENT; break; default: break; } if (state != AGENT_CONNECTED) { delay(10); } } bool microros_setup_transport_udp_client_() { String ssid = config.wifi_sta_ssid(); String ip = config.microros_uclient_server_ip(); String password = config.wifi_sta_pswd(); uint32_t agent_port = config.microros_uclient_server_port(); // 初始化数据接收配置 config_req.key = micro_ros_string_utilities_init_with_size(keyvalue_capacity); config_req.value = micro_ros_string_utilities_init_with_size(keyvalue_capacity); config_res.key = micro_ros_string_utilities_init_with_size(keyvalue_capacity); config_res.value = micro_ros_string_utilities_init_with_size(keyvalue_capacity); IPAddress agent_ip; agent_ip.fromString(ip); if (!set_microros_wifi_transports((char *)ssid.c_str(), (char *)password.c_str(), agent_ip, agent_port, config.board_name())) { return false; } return true; } bool microros_setup_transport_serial_() { String nodename = config.ros2_nodename(); String ros2namespace = config.ros2_namespace(); String twist_topic = config.ros2_twist_topic_name(); String odom_topic = config.ros2_odom_topic_name(); String odom_frameid_str = config.ros2_odom_frameid(); // 用于将odom_frameid_str的C字符串值设置为odom_msg的帧ID。 // 该函数会将odom_msg中的帧ID指针所指向的字符串缓冲区中的数据替换为odom_frameid_str的C字符串值，并返回指向新字符串的指针。 // 这样可以确保odom_msg的帧ID与配置中的值一致。 odom_msg.header.frame_id = micro_ros_string_utilities_set(odom_msg.header.frame_id, odom_frameid_str.c_str()); odom_msg.child_frame_id = micro_ros_string_utilities_set(odom_msg.child_frame_id, odom_frameid_str.c_str()); imu_msg.header.frame_id = micro_ros_string_utilities_set(imu_msg.header.frame_id, "imu"); const unsigned int timer_timeout = config.odom_publish_period(); // 初始化数据接收配置 config_req.key = micro_ros_string_utilities_init_with_size(keyvalue_capacity); config_req.value = micro_ros_string_utilities_init_with_size(keyvalue_capacity); config_res.key = micro_ros_string_utilities_init_with_size(keyvalue_capacity); config_res.value = micro_ros_string_utilities_init_with_size(keyvalue_capacity); uint32_t serial_baudraate = config.serial_baudrate(); Serial.updateBaudRate(serial_baudraate); if (!set_microros_serial_transports(Serial)) { return false; } return true; } // void callback_odom_publisher_timer_(rcl_timer_t *timer, int64_t last_call_time) // { // RCLC_UNUSED(last_call_time); // if (timer != NULL) // { // odom_t odom = kinematics.odom(); // int64_t stamp = rmw_uros_epoch_millis(); // odom_msg.header.stamp.sec = static_cast<int32_t>(stamp / 1000); // 秒部分 // odom_msg.header.stamp.nanosec = static_cast<uint32_t>((stamp % 1000) * 1e6); // 纳秒部分 // odom_msg.pose.pose.position.x = odom.x; // odom_msg.pose.pose.position.y = odom.y; // odom_msg.pose.pose.orientation.w = odom.quaternion.w; // odom_msg.pose.pose.orientation.x = odom.quaternion.x; // odom_msg.pose.pose.orientation.y = odom.quaternion.y; // odom_msg.pose.pose.orientation.z = odom.quaternion.z; // odom_msg.twist.twist.angular.z = odom.angular_speed; // odom_msg.twist.twist.linear.x = odom.linear_x_speed; // odom_msg.twist.twist.linear.y = odom.linear_y_speed; // display.updateBotAngular(odom.angular_speed); // display.updateBotLinear(odom.linear_x_speed); // RCSOFTCHECK(rcl_publish(&odom_publisher, &odom_msg, NULL)); // if (imu.isEnable()) // { // imu.getImuDriverData(imu_data); // imu_msg.header.stamp.sec = static_cast<int32_t>(stamp / 1000); // 秒部分 // imu_msg.header.stamp.nanosec = static_cast<uint32_t>((stamp % 1000) * 1e6); // 纳秒部分 // imu_msg.angular_velocity.x = imu_data.angular_velocity.x; // imu_msg.angular_velocity.x = imu_data.angular_velocity.x; // imu_msg.angular_velocity.x = imu_data.angular_velocity.x; // imu_msg.linear_acceleration.x = imu_data.linear_acceleration.x; // imu_msg.linear_acceleration.y = imu_data.linear_acceleration.y; // imu_msg.linear_acceleration.z = imu_data.linear_acceleration.z; // imu_msg.orientation.x = imu_data.orientation.x; // imu_msg.orientation.y = imu_data.orientation.y; // imu_msg.orientation.z = imu_data.orientation.z; // imu_msg.orientation.w = imu_data.orientation.w; // RCSOFTCHECK(rcl_publish(&imu_publisher, &imu_msg, NULL)); // } // } // } void callback_twist_subscription_(const void *msgin) { const geometry_msgs__msg__Twist *msg = (const geometry_msgs__msg__Twist *)msgin; static float target_motor_speed1, target_motor_speed2, target_motor_speed3, target_motor_speed4; kinematics.kinematic_inverse(msg->linear.x * 1000, msg->linear.y * 1000, msg->angular.z, target_motor_speed1, target_motor_speed2, target_motor_speed3, target_motor_speed4); pid_controller[0].update_target(target_motor_speed1); pid_controller[1].update_target(target_motor_speed2); pid_controller[2].update_target(target_motor_speed3); pid_controller[3].update_target(target_motor_speed4); } void callback_config_service_(const void *req, void *res) { fishbot_interfaces__srv__FishBotConfig_Request *req_in = (fishbot_interfaces__srv__FishBotConfig_Request *)req; fishbot_interfaces__srv__FishBotConfig_Response *res_in = (fishbot_interfaces__srv__FishBotConfig_Response *)res; String recv_key(req_in->key.data); String recv_value(req_in->value.data); fishlog_debug("fishbot", "recv service key=%s,value=%s\n", req_in->key.data, req_in->value.data); config.config(recv_key, recv_value); // PID相关配置 if (recv_key.startsWith("pid_")) { pid_controller[0].update_pid(config.kinematics_pid_kp(), config.kinematics_pid_ki(), config.kinematics_pid_kd()); pid_controller[1].update_pid(config.kinematics_pid_kp(), config.kinematics_pid_ki(), config.kinematics_pid_kd()); pid_controller[0].out_limit(-config.kinematics_pid_out_limit(), config.kinematics_pid_out_limit()); pid_controller[1].out_limit(-config.kinematics_pid_out_limit(), config.kinematics_pid_out_limit()); } // 系统相关配置 if (recv_key.equalsIgnoreCase("system")) { if (recv_value.equalsIgnoreCase("restart")) { xTaskCreate([](void *param) {for(int i=0;i<3;i++)delay(1000);esp_restart(); }, "restart_task", 1024, NULL, 1, NULL); } } fishlog_debug("fishbot", "current_str:%s", config.config_str().c_str()); sprintf(res_in->key.data, "%s", req_in->key.data); sprintf(res_in->value.data, "%s", req_in->key.data); } void callback_sensor_publisher_timer_(rcl_timer_t *timer, int64_t last_call_time) { RCLC_UNUSED(last_call_time); if (timer != NULL) { // 用于获取当前的时间戳，并将其存储在消息的头部中 int64_t stamp = rmw_uros_epoch_millis(); // 获取机器人的位置和速度信息，并将其存储在一个ROS消息（odom_msg）中 odom_t odom = kinematics.odom(); odom_msg.header.stamp.sec = static_cast<int32_t>(stamp / 1000); // 秒部分 odom_msg.header.stamp.nanosec = static_cast<uint32_t>((stamp % 1000) * 1e6); // 纳秒部分 odom_msg.pose.pose.position.x = odom.x; odom_msg.pose.pose.position.y = odom.y; odom_msg.pose.pose.orientation.w = odom.quaternion.w; odom_msg.pose.pose.orientation.x = odom.quaternion.x; odom_msg.pose.pose.orientation.y = odom.quaternion.y; odom_msg.pose.pose.orientation.z = odom.quaternion.z; odom_msg.twist.twist.angular.z = odom.angular_speed; odom_msg.twist.twist.linear.x = odom.linear_x_speed; odom_msg.twist.twist.linear.y = odom.linear_y_speed; //display.updateBotAngular(odom.angular_speed); //display.updateBotLinear(odom.linear_x_speed); //display.updateBotLinear(odom.linear_y_speed); RCSOFTCHECK(rcl_publish(&odom_publisher, &odom_msg, NULL)); //if (imu.isEnable()) //{ imu.getImuDriverData(imu_data); imu_msg.header.stamp.sec = static_cast<int32_t>(stamp / 1000); // 秒部分 imu_msg.header.stamp.nanosec = static_cast<uint32_t>((stamp % 1000) * 1e6); // 纳秒部分 imu_msg.angular_velocity.x = imu_data.angular_velocity.x; imu_msg.angular_velocity.y = imu_data.angular_velocity.y; imu_msg.angular_velocity.z = imu_data.angular_velocity.z; imu_msg.linear_acceleration.x = imu_data.linear_acceleration.x; imu_msg.linear_acceleration.y = imu_data.linear_acceleration.y; imu_msg.linear_acceleration.z = imu_data.linear_acceleration.z; imu_msg.orientation.x = imu_data.orientation.x; imu_msg.orientation.y = imu_data.orientation.y; imu_msg.orientation.z = imu_data.orientation.z; imu_msg.orientation.w = imu_data.orientation.w; RCSOFTCHECK(rcl_publish(&imu_publisher, &imu_msg, NULL)); //} } } ```@小鱼 -
@2429148505 我也没有发现问题，暂时看不出来，后续我抽时间将开源的代码在四驱版上测试修改，添加IMU后再来回帖。
-
@小鱼 在 四驱板使用fishbot四驱代码，无法正常发送odom和imu话题 中说：
@2429148505 我也没有发现问题，暂时看不出来，后续我抽时间将开源的代码在四驱版上测试修改，添加IMU后再来回帖。
催催，我自己也试了很久，还是解决不了。尝试过在fishros那个框架下，只发布imu话题，一直成功不了。您这边有什么进展了吗。有没有发现以上的代码的问题。
-
@小鱼 以下是我只测试imu能否正常发布的fishbot.cpp文件，main函数与imu,microROS库都与您在github上的开源相同，仍然无法正常发布imu话题。您能帮我看看问题，或者教教我debug的步骤吗。这个问题困扰我很久了
#include "fishbot.h" #include "TimeLib.h" #define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}} #define SECS_PER_HOUR ((time_t)(3600UL)) sensor_msgs__msg__Imu imu_msg; rcl_publisher_t imu_publisher; // 用于发布机器人的IMU信息（Imu） fishbot_interfaces__srv__FishBotConfig_Response config_res; fishbot_interfaces__srv__FishBotConfig_Request config_req; MPU6050 mpu(Wire); // 初始化MPU6050对象 ImuDriver imu(mpu); // 初始化Imu对象 imu_t imu_data; // IMU 数据对象 rclc_executor_t executor; rclc_support_t support; rcl_allocator_t allocator; rcl_node_t node; rcl_timer_t timer; const unsigned int timer_timeout = 50; enum states { WAITING_AGENT, AGENT_AVAILABLE, AGENT_CONNECTED, AGENT_DISCONNECTED } state; int8_t split_str(const char *line, char result[][32]) { if (line[0] != '$') return 1; uint16_t index = 0; uint16_t count = 0; uint16_t temp_index = 0; for (index = 1; line[index] != '\0'; index++) { if (line[index] == '=') { result[count++][temp_index++] = '\0'; temp_index = 0; continue; } result[count][temp_index++] = line[index]; } result[count][temp_index++] = '\0'; if (count != 1) { return 1; } return 1; } int8_t loop_config_uart(int c, char result[][32]) { static char line[512]; static int index = 0; if (c == '\n') { line[index] = '\0'; index = 0; return split_str(line, result); } else if (c > 0 && c < 127) { line[index] = c; ++index; } return 0; } bool setup_fishbot() { Serial.begin(115200); //fishlog_set_target(Serial); //config.init("fishbot"); Serial.println("FIRST_START_TIP"); //Serial.println(config.config_str()); imu.begin(18, 19);//imu初始化 return 1; } static void deal_command(char key[32], char value[32]) { if (strcmp(key, "command") == 0) { if (strcmp(value, "restart") == 0) { esp_restart(); } else if (strcmp(value, "read_config") == 0) { Serial.print("Ok"); } return; } else { String recv_key(key); String recv_value(value); //config.config(recv_key, recv_value); Serial.print("$result=ok\n"); } } bool setup_fishbot_transport() { bool setup_success = true; //SerialBT.begin("config.board_name()"); //fishlog_set_target(SerialBT); microros_setup_transport_serial_(); //display.updateTransMode("serial"); return true; } bool create_fishbot_transport() { // String odom_child_frameid_str = //config.ros2_odom_child_frameid(); // odom_msg.header.frame_id = micro_ros_string_utilities_set(odom_msg.header.frame_id, odom_frameid_str.c_str()); // odom_msg.child_frame_id = micro_ros_string_utilities_set(odom_msg.child_frame_id, odom_child_frameid_str.c_str()); imu_msg.header.frame_id = micro_ros_string_utilities_set(imu_msg.header.frame_id, "imu"); RCSOFTCHECK(rclc_publisher_init_best_effort( &imu_publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Imu), "imu")); RCSOFTCHECK(rclc_timer_init_default(&timer, &support, RCL_MS_TO_NS(timer_timeout), callback_sensor_publisher_timer_)); RCSOFTCHECK(rclc_executor_add_timer(&executor, &timer)); return true; } bool destory_fishbot_transport() { rmw_context_t *rmw_context = rcl_context_get_rmw_context(&support.context); (void)rmw_uros_set_context_entity_destroy_session_timeout(rmw_context, 0); RCSOFTCHECK(rcl_publisher_fini(&imu_publisher, &node)); return 1; } void loop_fishbot_control() { imu.update(); } void loop_fishbot_transport() { static char result[10][32]; static int config_result; while (Serial.available()) { int c = Serial.read(); config_result = loop_config_uart(c, result); if (config_result == 1) { deal_command(result[0], result[1]); } else if (config_result == -1) { Serial.print("$result=error parse\n"); } } switch (state) { case WAITING_AGENT: EXECUTE_EVERY_N_MS(500, state = (RMW_RET_OK == rmw_uros_ping_agent(100, 1)) ? AGENT_AVAILABLE : WAITING_AGENT;); break; case AGENT_AVAILABLE: state = (true == create_fishbot_transport()) ? AGENT_CONNECTED : WAITING_AGENT; if (state == WAITING_AGENT) { destory_fishbot_transport(); }; break; case AGENT_CONNECTED: EXECUTE_EVERY_N_MS(200, state = (RMW_RET_OK == rmw_uros_ping_agent(100, 1)) ? AGENT_CONNECTED : AGENT_DISCONNECTED;); if (state == AGENT_CONNECTED) { if (!rmw_uros_epoch_synchronized()) { RCSOFTCHECK(rmw_uros_sync_session(1000)); if (rmw_uros_epoch_synchronized()) { setTime(rmw_uros_epoch_millis() / 1000 + SECS_PER_HOUR * 8); //fishlog_debug("fishbot", "current_time:%ld", rmw_uros_epoch_millis()); } delay(10); return; } RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100))); } break; case AGENT_DISCONNECTED: destory_fishbot_transport(); state = WAITING_AGENT; break; default: break; } if (state != AGENT_CONNECTED) { delay(10); } } bool microros_setup_transport_serial_(){ const unsigned int timer_timeout = 50; imu_msg.header.frame_id = micro_ros_string_utilities_set(imu_msg.header.frame_id, "imu"); if (!set_microros_serial_transports(Serial)) { return false; } return true; } void callback_sensor_publisher_timer_(rcl_timer_t *timer, int64_t last_call_time) { int64_t stamp = rmw_uros_epoch_millis(); imu.getImuDriverData(imu_data); imu_msg.header.stamp.sec = static_cast<int32_t>(stamp / 1000); // 秒部分 imu_msg.header.stamp.nanosec = static_cast<uint32_t>((stamp % 1000) * 1e6); // 纳秒部分 imu_msg.angular_velocity.x = imu_data.angular_velocity.x; imu_msg.angular_velocity.y = imu_data.angular_velocity.y; imu_msg.angular_velocity.z = imu_data.angular_velocity.z; imu_msg.linear_acceleration.x = imu_data.linear_acceleration.x; imu_msg.linear_acceleration.y = imu_data.linear_acceleration.y; imu_msg.linear_acceleration.z = imu_data.linear_acceleration.z; imu_msg.orientation.x = imu_data.orientation.x; imu_msg.orientation.y = imu_data.orientation.y; imu_msg.orientation.z = imu_data.orientation.z; imu_msg.orientation.w = imu_data.orientation.w; RCSOFTCHECK(rcl_publish(&imu_publisher, &imu_msg, NULL)); } -
@小鱼 我整理好上传github了，速速审核
-
@2429148505 收到
-
@2429148505 已合并，但四驱版本不久前分割出新的仓库了： https://github.com/fishros/fishbot_motion_control_microros_four_driver
如果有空可以顺便同步一下，万分感谢！
-
@小鱼 OK,有空我上传一下。顺便问问，我想搞麦轮利用imu的姿态矫正，有什么推荐的资料吗
-
@2429148505 在ROS 2中，融合IMU和里程计数据可以使用多种方式和工具包。常用的方法是通过机器人定位和导航工具包（如
robot_localization），它能够处理多个传感器输入并提供融合后的定位信息。以下是一个基本的步骤指南，帮助你在ROS 2中实现IMU和里程计数据的融合：
1. 安装ROS 2和依赖包
确保你已经安装了ROS 2和相关的依赖包。可以参考ROS 2的官方安装指南来安装ROS 2。
2. 安装
robot_localization包robot_localization是一个非常强大的工具包，用于融合多个传感器数据，包括IMU和里程计。sudo apt-get update sudo apt-get install ros-<your-ros2-distro>-robot-localization请将
<your-ros2-distro>替换为你所使用的ROS 2发行版名称，如foxy、galactic或humble。3. 配置IMU和里程计数据发布
确保你的IMU和里程计数据分别发布到正确的ROS主题上。通常情况下：
- IMU数据发布到
/imu/data主题 - 里程计数据发布到
/odom主题
你可以使用ROS 2的发布者和订阅者来发布这些数据。
4. 创建配置文件
创建一个YAML配置文件，用于配置
robot_localization节点的参数。以下是一个示例配置文件ekf.yaml：ekf_filter_node: ros__parameters: frequency: 30.0 sensor_timeout: 0.1 two_d_mode: true publish_tf: true map_frame: map odom_frame: odom base_link_frame: base_link world_frame: odom odom0: /odom odom0_config: [true, true, false, false, false, true, false, false, false, false, false, false, false, false, false] odom0_queue_size: 10 imu0: /imu/data imu0_config: [false, false, false, true, true, false, false, false, false, false, false, true, false, false, false] imu0_queue_size: 10 imu0_remove_gravitational_acceleration: true5. 启动
robot_localization节点创建一个启动文件，使用
launch来启动robot_localization节点，并加载你的配置文件。以下是一个示例启动文件ekf_launch.py：from launch import LaunchDescription from launch_ros.actions import Node def generate_launch_description(): return LaunchDescription([ Node( package='robot_localization', executable='ekf_node', name='ekf_filter_node', output='screen', parameters=['/path/to/your/ekf.yaml'], ) ])将
/path/to/your/ekf.yaml替换为你的ekf.yaml文件的实际路径。6. 运行你的ROS 2应用
使用
ros2 launch命令启动你的应用：ros2 launch <your_package_name> ekf_launch.py将
<your_package_name>替换为包含启动文件的实际包名。7. 验证融合结果
你可以使用
rviz2或其他可视化工具来查看融合后的定位结果。确保定位结果是预期的，并根据需要调整配置文件中的参数。通过这些步骤，你应该能够在ROS 2中成功融合IMU和里程计数据，以实现更精确的机器人定位和导航。
- IMU数据发布到
-
@小鱼 我的意思是，我现在的麦轮在控制层面会出现右偏的情况，不能正常地直走。想在控制算法层面上改进。不过在定位算法上，我没有实践过。定位的更准并持续地更新路径，能改善这个问题吗。
-
@小鱼 不过在定位方面我最近在考虑LIO-SAM算法，到时候看看效果怎么样。
-
-
@小鱼 在 四驱板使用fishbot四驱代码，无法正常发送odom和imu话题 中说：
@2429148505 在 四驱板使用fishbot四驱代码，无法正常发送odom和imu话题 中说：
在的麦轮在控制层面会出现右偏的情况
校准轮距和轮子的编码器数值到距离的转换因子就好
没有听懂，如果是编码器到距离没有校准的问题，那直走应该不会右偏吧。我现在直走也不正常。还是说我理解的不对，有相关的资料吗
-
@2429148505 和二驱差不多，就是速度计算那里，每个轮子因为出场原因，减速比并不一样造成的误差
-
@小鱼 ok,我试试，确实没搞校准
-
@小鱼 在 四驱板使用fishbot四驱代码，无法正常发送odom和imu话题 中说：
@2429148505 和二驱差不多，就是速度计算那里，每个轮子因为出场原因，减速比并不一样造成的误差
帮你加上设置矫正脉冲的功能了，有空的时候上传
-
@2429148505 ok
-
@小鱼 最近测试四驱板，发现我的odom方向与实际方向相反。(我说之前导航怎么老是出问题，我还以为是打滑的原因(虽然还是走不直，以后打算加一个imu纠偏))使用的是fishbot四驱板的代码，以下是现象:当cmd_vel只给x速度时，小车会向前走，而且四个轮子输出都是正的，可以排除是安装错误的问题，但是轮子向前转动时，输出的脉冲为负值，而且返回的速度也都为负值。而且不是编码器读数应该没问题，因为把编码器的引脚改反，会发生错误，轮子会因为pid原地振动。感觉是因为正解算时解算反了，但是我找不到代码错误。不知道你有没有出现这个现象。
