内容
有一个轨迹文本文件，使用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); // 发布频率为10Hz
transformStamped.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 COMPONENTS
roscpp
tf2_ros
nav_msgs
geometry_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
rvizrosbag play --pause xxx.bag在rviz中添加一个Path显示器，并将其订阅到/path_topic话题，或者在“topic”中直接添加/path_topic话题，以显示轨迹的可视化效果。
效果
