ROS IMU 數據發佈

1，參照 http://wiki.ros.org/rviz_imu_plugin 安裝rviz_imu_plugin,有助於調試查看。

https://github.com/ccny-ros-pkg/imu_tools

參照Readme編譯安裝：

source /opt/ros/kinetic/setup.bash

mkdir ws/src -p; cd ws/src

git clone -b <distro> https://github.com/ccny-ros-pkg/imu_tools.git

catkin_init_workspace    //該命令會產生CMakeLists.txt -> /opt/ros/kinetic/share/catkin/cmake/toplevel.cmake 連接文件。

rosdep install imu_tools

cd ..

catkin_make

catkin_make install -DCMAKE_INSTALL_PREFIX=/opt/ros/kinetic

 

2，廣播imu數據。

void RobotRos::publishInertia()
{
  if (ros::ok())
  {
    if (imu_data_publisher.getNumSubscribers() > 0)
    {
      // Publish as shared pointer to leverage the nodelets' zero-copy pub/sub feature
      sensor_msgs::ImuPtr msg(new sensor_msgs::Imu);
      ThreeAxisGyro::Data data = getRawInertiaData();
      
      msg->header.frame_id = "gyro_link";
      msg->header.stamp = ros::Time::now();

      //geometry_msgs::Quaternion createQuaternionMsgFromRollPitchYaw(double roll,double pitch,double yaw)
      msg->orientation = tf::createQuaternionMsgFromRollPitchYaw(getRoll(),getPitch(), getHeading());

      // set a non-zero covariance on unused dimensions (pitch and roll); this is a requirement of robot_pose_ekf
      // set yaw covariance as very low, to make it dominate over the odometry heading when combined
      // 1: fill once, as its always the same;  2: using an invented value; cannot we get a realistic estimation?
      //msg->orientation_covariance[0] = DBL_MAX;
      //msg->orientation_covariance[4] = DBL_MAX;
      //msg->orientation_covariance[8] = 0.05;

      // fill angular velocity; we ignore acceleration for now
      msg->angular_velocity.x = 0.0;
      msg->angular_velocity.y = 0.0;
      msg->angular_velocity.z = kobuki.getAngularVelocity();

      // angular velocity covariance; useless by now, but robot_pose_ekf's
      // roadmap claims that it will compute velocities in the future
      //msg->angular_velocity_covariance[0] = DBL_MAX;
      //msg->angular_velocity_covariance[4] = DBL_MAX;
      //msg->angular_velocity_covariance[8] = 0.05;

      // line acc
      msg->linear_acceleration.x = (data.x_acc*0.001);
      msg->linear_acceleration.y = (data.y_acc*0.001);
      msg->linear_acceleration.z = (data.z_acc*0.001);
      
      //msg->linear_acceleration_covariance[0] = DBL_MAX;
      //msg->linear_acceleration_covariance[4] = DBL_MAX;
      //msg->linear_acceleration_covariance[8] = 0.05;
      
      imu_data_publisher.publish(msg);
    }
  }
}

3，顯示效果如下：