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" />