重要提示
提问前必看的发帖注意事项—— 提问前必看!不符合要求的问题拒绝回答!!
社区使用指南—如何添加标签修改密码
四驱板使用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话题,一直成功不了。您这边有什么进展了吗。有没有发现以上的代码的问题。