[FishBot二次开发]驱动并通过MicroROS发布IMU话题

[小鱼]

需求描述

驱动IMU模块，并通过MicroROS发布，以便后续进行里程计和IMU模块。

前置教程

• IMU驱动开发:https://fishros.com/d2lros2/#/humble/chapt13/advance/2.使用开源库驱动IMU
• MicroROS话题发布：https://fishros.com/d2lros2/#/humble/chapt14/get_started/2.MicroROS-话题发布实现

开始写代码

直接基于FishBot开源驱动代码进行编写：https://github.com/fishros/fishbot_motion_control_microros

初始化硬件

MPU6050 mpu(Wire);               // 初始化MPU6050对象
ImuDriver imu(mpu);              // 初始化Imu对象
imu_t imu_data;                  // IMU 数据对象

创建发布者：

需要注意 imu_msg.header.frame_id 要和URDF中定义的部件名称相同。

sensor_msgs__msg__Imu imu_msg;
rcl_publisher_t imu_publisher;       // 用于发布机器人的IMU信息（Imu）
imu_t imu_data;                  // IMU 数据对象
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"));

发布数据

        if (imu.isEnable())
        {
            imu.getImuDriverData(imu_data);

            imu_msg.header.stamp.sec = stamp * 1e-3;
            imu_msg.header.stamp.nanosec = stamp - imu_msg.header.stamp.sec * 1000;

            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));
        }

测试结果

完整代码：https://github.com/fishros/fishbot_motion_control_microros

测试平台：FishBot二驱机器人

重新构建运行代码，topic echo 数据

 ros2 topic echo  imu
---
header:
  stamp:
    sec: 1686806795
    nanosec: 362
  frame_id: imu
orientation:
  x: -0.0004342478350736201
  y: 0.004601224325597286
  z: 0.9932646751403809
  w: 0.1157754585146904
orientation_covariance:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
angular_velocity:
  x: 0.09578633308410645
  y: 0.0
  z: 0.0
angular_velocity_covariance:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
linear_acceleration:
  x: -0.001542968675494194
  y: 0.011208496056497097
  z: 0.9988227486610413
linear_acceleration_covariance:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0