结合imu与odom运行robot_pose_ekf

EKF融合运行记录

泰裤辣！！！
虚拟机空间不够，在复制bag包时直接崩溃，再也打不开，只好说拜拜，从头开始吧

step1 克隆robot_pose_ekf包

mkdir -p ekf_ws/src
cd ekf_ws
catkin_make
cd src
git clone https://github.com/udacity/robot_pose_ekf

step2 编译

cd ..
catkin_make

缺少 orocos-bfl 功能包

sudo apt-get install ros-melodic-bfl
catkin_make

step3 从头开始中~~~，这里省略十万八千字

step4 下载bag包

这里参考可峰科技的1-R2000数据（某建筑楼内）

step5 编写节点绘制两个odom数据的轨迹

为了更好的比较融合前后位姿的差异，编写一个节点，订阅原始odom话题与融合后的odom_combined话题数据，在回调函数中解析出当前位姿，并发布各自的轨迹。（注意：这两个odom话题的数据类型并不一致）

实现如下：

cd src
catkin_create_pkg odom_trajectory roscpp nav_msgs std_msgs geometry_msgs tf

编写节点，并更改此功能包中的cmakelists.txt文件，如下

add_executable(trajectory src/trajectory.cpp)

target_link_libraries(trajectory
  ${catkin_LIBRARIES}
)

代码参考了古月居中这篇文章

#include <ros/ros.h>
#include <ros/console.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>
nav_msgs::Path  path, combine_path;
ros::Publisher  path_pub, combine_path_pub;
 
void odomCallback(const nav_msgs::Odometry::ConstPtr& odom)
{
    geometry_msgs::PoseStamped this_pose_stamped;
    this_pose_stamped.pose.position.x = odom -> pose.pose.position.x;
    this_pose_stamped.pose.position.y = odom -> pose.pose.position.y;
    this_pose_stamped.pose.position.z = odom -> pose.pose.position.z;
    this_pose_stamped.pose.orientation = odom -> pose.pose.orientation;
 
    this_pose_stamped.header.stamp = ros::Time::now();
    this_pose_stamped.header.frame_id = "odom";
 
    path.poses.push_back(this_pose_stamped);
 
    path.header.stamp = ros::Time::now();
    path.header.frame_id = "odom";
    path_pub.publish(path);

}
 
 void combine_odomCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& odom)
{
    geometry_msgs::PoseStamped this_pose_stamped;
    this_pose_stamped.pose.position.x = odom -> pose.pose.position.x;
    this_pose_stamped.pose.position.y = odom -> pose.pose.position.y;
    this_pose_stamped.pose.position.z = odom -> pose.pose.position.z;
    this_pose_stamped.pose.orientation = odom -> pose.pose.orientation;
 
    this_pose_stamped.header.stamp = ros::Time::now();
    this_pose_stamped.header.frame_id = "odom";
 
    combine_path.poses.push_back(this_pose_stamped);
 
    combine_path.header.stamp = ros::Time::now();
    combine_path.header.frame_id = "odom";
    combine_path_pub.publish(combine_path);

}

int main (int argc, char **argv)
{
    ros::init (argc, argv, "showpath_odom");
    ros::NodeHandle ph;
 
    path_pub = ph.advertise<nav_msgs::Path>("trajectory_odom", 10, true);
    combine_path_pub = ph.advertise<nav_msgs::Path>("combine_trajectory_odom", 10, true);
    ros::Subscriber odomSub = ph.subscribe<nav_msgs::Odometry>("/odom", 10, odomCallback);  //订阅里程计话题信息
    ros::Subscriber combine_odomSub = ph.subscribe<geometry_msgs::PoseWithCovarianceStamped>("/robot_pose_ekf/odom_combined", 10, combine_odomCallback);  //订阅ekf里程计话题信息
    ros::Rate loop_rate(50);
    while(ros::ok())
    {
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

step6 运行

新建3个终端，分别运行
.bag包

cd ekf_ws
rosbag play 2019-10-26-20-19-78.bag 

robot_pose_ekf包
(需要根据自己的bag包发布的话题名更改launch文件）
我所用的launch文件如下：

<launch>

<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
  <param name="output_frame" value="odom_combined"/>
  <param name="base_footprint_frame" value="base_footprint"/>
  <param name="freq" value="30.0"/>
  <param name="sensor_timeout" value="1.0"/>  
  <param name="odom_used" value="true"/>
  <param name="imu_used" value="true"/>
  <param name="vo_used" value="false"/>

  <remap from="odom" to="/odom" />
</node>

</launch>

cd ekf_ws
source ./devel/setup.bash
roslaunch robot_pose_ekf robot_pose_ekf.launch

trajectory节点

cd ekf_ws
source ./devel/setup.bash
rosrun odom_trajectory trajectory

打开RVIZ,最终结果如下：

这里用到的bag包是在建筑楼内录制的，可见融合imu后的路径更加准确（红色轨迹）。

总结

除重新装了个虚拟机外，最大的问题是找到一个合适的数据集，之前在kitti上找到的数据集运行后没有传感器的协方差矩阵。历时10h成功跑通，目前处在崩溃当中。