使用ROS读取tum格式轨迹文件并在RVIZ显示的实现过程

内容

有一个轨迹文本文件，使用ROS系统读取该文件，并将轨迹在RVIZ界面绘制。

先用EVO工具将tum格式轨迹文件转为bag格式；

再创建ROS功能包，订阅bag文件中的话题数据，处理再发布新的轨迹话题，同时新建线程发布TF。

最后编译、运行，多个终端发布开启roscore、启动功能包、打开rviz、播放bag。

实现过程

tum转bag

使用EVO工具将tum格式轨迹文件转为bag格式：

evo_traj tum traj_1.txt traj_2.txt --save_as_bag

需要同时安装有 EVO和ROS。

完成转换后得到.bag格式文件，文件里面的话题类型为<geometry_msgs::PoseStamped>。

创建功能包

进入ROS工作空间的源代码文件夹：

cd ~/catkin_ws/src

 在src目录下创建一个新的包，例如showpath：

catkin_create_pkg showpath

写入代码 

进入新创建的包目录：

cd showpath

 创建一个src文件夹，并进入该文件夹

mkdir src
cd src

创建一个C++源文件，例如read_tum_path.cpp，并使用文本编辑器打开它： 

touch read_tum_path.cpp
gedit read_tum_path.cpp

将以下代码复制粘贴到read_tum_path.cpp中：

#include <ros/ros.h>
#include <rosbag/bag.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
#include <thread>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
using namespace std;
ros::Publisher path_pub; 
nav_msgs::Path path;void gt_path_callback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{// 创建并填充nav_msgs::Path消息path.header = msg->header;path.header.frame_id = "camera_link";path.poses.push_back(*msg);cout<<"The program is reading data.stamp:"<<msg->header.stamp<<endl;// 发布nav_msgs::Path消息path_pub.publish(path);
}void publishTf()
{ros::NodeHandle nh;tf2_ros::TransformBroadcaster broadcaster;while (ros::ok()){// 创建并设置tf2变换消息geometry_msgs::TransformStamped transformStamped;transformStamped.header.frame_id = "base_link";transformStamped.child_frame_id = "camera_link";transformStamped.transform.translation.x = 0;transformStamped.transform.translation.y = 0.0;transformStamped.transform.translation.z = 0;transformStamped.transform.rotation.x = 0.0;transformStamped.transform.rotation.y = 0.0;transformStamped.transform.rotation.z = 0.0;transformStamped.transform.rotation.w = 1.0;ros::Rate rate(10.0);  // 发布频率为10HztransformStamped.header.stamp = ros::Time::now();broadcaster.sendTransform(transformStamped);// 发布频率适当调整ros::Rate(10).sleep();}
}int main(int argc, char** argv)
{ros::init(argc, argv, "tf_and_dataset_publisher");ros::NodeHandle nh;
cout<<"Program startup completed"<<endl;
ros::Subscriber sub_lidar = nh.subscribe("/gt_traj", 100,gt_path_callback);// 创建一个发布器，用于发布nav_msgs::Path消息path_pub = nh.advertise<nav_msgs::Path>("path_topic", 10);// 创建两个线程，分别用于发布tf变换和播放数据集// 创建线程，用于发布tf变换std::thread tf_thread(&publishTf);ros::spin();
cout<<"Program startup completed"<<endl;// 等待线程结束tf_thread.join();  return 0;
}

上述程序中，新建节点tf_and_dataset_publisher，接收/gt_traj话题，<geometry_msgs::PoseStamped>格式数据被回调函数处理后发布<nav_msgs::Path>类型的/path_topic话题。同时用std::thread新建线程发布tf变换，tf变换根据需要设定。

回到showpath包目录，并创建一个CMakeLists.txt文件，使用文本编辑器打开它： 

cd ..
touch CMakeLists.txt
gedit CMakeLists.txt

创建功能包时候已经创建有 CMakeLists.txt文件，删除新建就行。

将以下内容复制粘贴到CMakeLists.txt中：

cmake_minimum_required(VERSION 2.8.3)
set(CMAKE_CXX_STANDARD 11)project(showpath)find_package(catkin REQUIRED COMPONENTSroscpptf2_rosnav_msgsgeometry_msgs
)catkin_package(
)include_directories(${catkin_INCLUDE_DIRS}
)add_executable(read_tum_path src/read_tum_path.cpp)
target_link_libraries(read_tum_path ${catkin_LIBRARIES})

 保存并关闭CMakeLists.txt文件。

编译构建

返回到ROS工作空间的根目录，并构建该包： 

cd ~/catkin_ws
catkin_make

如果有报错，根据报错内容修改。

运行程序

 打开多个终端，分别运行ROS核心、showpath功能包、RVIZ界面、播放数据集。

roscore

rosrun showpath read_tum_path

rviz

rosbag play --pause xxx.bag

在rviz中添加一个Path显示器，并将其订阅到/path_topic话题，或者在“topic”中直接添加/path_topic话题，以显示轨迹的可视化效果。

 效果