1. imu的移植

同之前的类似我们，

• 新增sensor_msg的头文件

#include <sensor_msgs/msg/imu.hpp>
#iinclude <sensor_msgs/msg/magnetic_field.hpp>

• 修改定义imu和mag的pub定义以及msg定义
  ros::Publisher ----> rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr
ros::Publisher ----> rclcpp::Publisher<sensor_msgs::msg::MagneticField>::SharedPtr

• 添加依赖
  在CMakeLists.txt新增sensor_msg的依赖

...
find_package(sensor_msgs REQUIRED)
...
ament_target_dependencies(pibot_driver 
                      rclcpp
                      ...
                      sensor_msgs)
...

• launch文件新增相应的配置参数

            parameters=[
            ...
                {"imu/accelerometer_bias": [0.005436, 0.014684, -0.395418]},
                {"imu/gyroscope_bias": [0.005436, 0.014684, -0.395418]},
                {"imu/use_accelerometer": True},
                {"imu/use_gyroscope": True},
                {"imu/use_magnetometer": True},
                {"imu/perform_calibration": True},
            ]

这里有与之前稍有不同的是accelerometer_bias和gyroscope_bias其接受的是x y z的三个值
ROS1中这样定义

        <rosparam if="$(arg use_imu)">
            imu/accelerometer_bias: {x: 0.005436, y: 0.014684, z: -0.395418}
            imu/gyroscope_bias: {x: -0.035592, y: 0.080670, z: 0.001216}
        </rosparam>`

ROS2的参数还不支持map格式，故稍作修改修改为list(c++中是vector)

• 实现pub,分别发布imu和mag

void BaseDriver::update_imu() {
...
    auto now = this->get_clock()->now();
    if (use_accelerometer_ || use_gyroscope_) {
      imu_msg_.header.stamp = now;

      imu_msg_.angular_velocity.x = DataHolder::get()->imu_data[3] - gyroscope_bias_[0];
      imu_msg_.angular_velocity.y = DataHolder::get()->imu_data[4] - gyroscope_bias_[1];
      imu_msg_.angular_velocity.z = DataHolder::get()->imu_data[5] - gyroscope_bias_[2];

      imu_msg_.linear_acceleration.x = DataHolder::get()->imu_data[0] - acceleration_bias_[0];
      imu_msg_.linear_acceleration.y = DataHolder::get()->imu_data[1] - acceleration_bias_[1];
      imu_msg_.linear_acceleration.z = DataHolder::get()->imu_data[2] - acceleration_bias_[2];

      imu_pub_->publish(imu_msg_);
    }

    if (use_magnetometer_) {
      if (use_mag_msg_) {
        mag_msg_.header.stamp = now;

        mag_msg_.magnetic_field.x = (DataHolder::get()->imu_data[6] * MILIGAUSS_TO_TESLA_SCALE - (mag_x_max_ - mag_x_min_) / 2 - mag_x_min_);
        mag_msg_.magnetic_field.y = (DataHolder::get()->imu_data[7] * MILIGAUSS_TO_TESLA_SCALE - (mag_y_max_ - mag_y_min_) / 2 - mag_y_min_);
        mag_msg_.magnetic_field.z = (DataHolder::get()->imu_data[8] * MILIGAUSS_TO_TESLA_SCALE - (mag_z_max_ - mag_z_min_) / 2 - mag_z_min_);

        mag_pub_->publish(mag_msg_);
      }
    }
...
}

2. 校准的服务移植

ROS1中定义service来实现对imu校准的服务,ROS2中同样我们实现一个service

类型我们使用同样的Empty

• 添加头文件以及相应的依赖

#include <std_srvs/srv/empty.hpp>

...
find_package(std_srvs REQUIRED)
...
ament_target_dependencies(pibot_driver 
...
                      std_srvs)

• 注册服务，传入相应的回调
  advertiseService--->create_service
  参考https://gitee.com/pibot/ros2_tutorials/tree/master/src/cpp_srvcli/src

create_service<std_srvs::srv::Empty>("imu/calibrate_imu",std::bind(&BaseDriver::calibrateCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));

• 回调实现,回调中置相应的变量

void BaseDriver::calibrateCallback(const std::shared_ptr<rmw_request_id_t> __attribute__((unused)) request_header,
                                   const std::shared_ptr<std_srvs::srv::Empty::Request> __attribute__((unused)) request,
                                   std::shared_ptr<std_srvs::srv::Empty::Response> __attribute__((unused)) response) {
  RCLCPP_INFO(this->get_logger(), "Calibrating accelerometer and gyroscope, make sure robot is stationary and level.");
  perform_calibration_ = true;
}

3. 测试

ros2 launch  pibot_bringup bringup_launch.py

➜  ros2 topic list         
/cmd_vel
/imu/data_raw
/imu/mag
/odom
/parameter_events
/rosout
/tf
➜  ros2 topic info /imu/data_raw
Type: sensor_msgs/msg/Imu
Publisher count: 1
Subscription count: 0
➜  ros2 topic info /imu/mag     
Type: sensor_msgs/msg/MagneticField
Publisher count: 1
Subscription count: 0
➜  ros2 service list       
/imu/calibrate_imu
/pibot_driver/describe_parameters
➜  ros2 service type /imu/calibrate_imu 
std_srvs/srv/Empty

调用服务,类似ROS1中的rosserivce call,ROS2中使用ros2 service call

➜  ros2 service call /imu/calibrate_imu std_srvs/srv/Empty {}
requester: making request: std_srvs.srv.Empty_Request()

response:
std_srvs.srv.Empty_Response()

本文代码https://gitee.com/pibot/pibot_bringup/tree/5e7e0c961a35f2704bb92a1f69fb4af3640d4093