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

内容

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

实现过程

创建功能包

进入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 <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <fstream>

nav_msgs::Path readTumPath(const std::string& file_path) {
    nav_msgs::Path path;
    path.header.frame_id = "map";

    std::ifstream file(file_path.c_str());
    if (!file) {
        ROS_ERROR_STREAM("Failed to open the file: " << file_path);
        return path;
    }

    std::string line;
    while (std::getline(file, line)) {
        std::istringstream iss(line);
        double timestamp, x, y, z;
        if (!(iss >> timestamp >> x >> y >> z)) {
            ROS_WARN_STREAM("Invalid line format: " << line);
            continue;
        }

        geometry_msgs::PoseStamped pose;
        pose.header.stamp = ros::Time(timestamp);
        pose.pose.position.x = x;
        pose.pose.position.y = y;
        pose.pose.position.z = z;

        path.poses.push_back(pose);
    }

    return path;
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "tum_path_publisher");
    ros::NodeHandle nh;

    std::string file_path = "path_to_a.txt";  // 替换为实际的轨迹文件路径
    nav_msgs::Path path = readTumPath(file_path);

    ros::Publisher path_pub = nh.advertise<nav_msgs::Path>("tum_path", 10);
    ros::Rate rate(10);  // 发布频率为10Hz

    while (ros::ok()) {
        path_pub.publish(path);
        rate.sleep();
    }

    return 0;
}

在上面的代码中，请将"path_to_a.txt"替换为实际轨迹文件路径。

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

cd ..
touch CMakeLists.txt
gedit CMakeLists.txt

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

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

cmake_minimum_required(VERSION 2.8.3)
project(showpath)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  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

rviz

轨迹数据将以ROS路径消息的形式发布到tum_path主题上。

最后，在rviz中添加一个Path显示器，并将其订阅到tum_path主题以显示轨迹的可视化效果。

请确保将"path_to_a.txt"替换为实际轨迹文件的路径，并且在构建时没有出现任何错误。

 效果