ROS融合IMU筆記

ROS官網有一個叫robot_pose_ekf的包，是專門處理傳感器融合的包，具體介紹：http://wiki.ros.org/robot_pose_ekf

其中主要功能是訂閱主題包括odom（里程計）、imu_data（姿態傳感器）、vo（視覺里程計）輸入，三者或是其中兩者融合后，輸出合成的里程計主題odom_combined並發布出去。第二是提供一個TF變化。

 

配置文件如下和注釋如下：

<launch>
 <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">  <param name="output_frame" value="odom_combined"/> #表示輸出的主題：odom_combined  <param name="freq" value="30.0"/> #表示發布主題的頻率 30hz  <param name="sensor_timeout" value="1.0"/> #表示超時時間，1.0表示1秒，如果1秒內沒有傳感器輸入，會報錯。  <param name="odom_used" value="true"/> #表示里程計使能  <param name="imu_used" value="true"/> #表示imu使能  <param name="vo_used" value="false"/> #視覺里程計  <param name="debug" value="true"/>  <param name="self_diagnose" value="false"/>  </node>  </launch>

odom (nav_msgs/Odometry) 
imu_data (sensor_msgs/Imu)
vo (nav_msgs/Odometry)

其中imu發布函數為
sensor_msgs::Imu imu;
imu.header.stamp = measurement_time;
imu.header.frame_id = imu_frame_id;
quaternionTFToMsg(differential_rotation, imu.orientation);
// i do not know the orientation covariance
imu.orientation_covariance[0] = 1000000;
imu.orientation_covariance[1] = 0;
imu.orientation_covariance[2] = 0;
imu.orientation_covariance[3] = 0;
imu.orientation_covariance[4] = 1000000;
imu.orientation_covariance[5] = 0;
imu.orientation_covariance[6] = 0;
imu.orientation_covariance[7] = 0;
imu.orientation_covariance[8] = 0.000001;
// angular velocity is not provided
// imu.angular_velocity_covariance[0] = 10000000;

imu.angular_velocity.x = 0.0;
imu.angular_velocity.y = 0.0;
imu.angular_velocity.z = (double)-1*(angleRate*3.14/(180*100));

imu.linear_acceleration.x = (double)(-1*((acc_x+10) * 9.80665/1000.f));
imu.linear_acceleration.y = (double)(-1*((acc_y+24) * 9.80665/1000.f));
imu.linear_acceleration.z = (double)(-1*((acc_z-1070) * 9.80665/1000.f));

// imu.linear_acceleration_covariance[0] = -1 ;
imu_pub.publish(imu);

其中TF變換：odom_combined → base_footprint
static tf::TransformBroadcaster br;
tf::Transform transform;
transform.setRotation(differential_rotation);
br.sendTransform(tf::StampedTransform(transform, measurement_time, tf_parent_frame_id, tf_frame_id));
加入TF樹：
<node pkg="tf" type="static_transform_publisher" name="imu_broadcaster" args="0 0 0 0 0 0 base_footprint imu_base 100" respawn="true" />