用ROS在RVIZ中同时显示RTK轨迹（GT）和SLAM轨迹（以视觉VINS为例）

0、前期准备

首先我们需要安装ROS、并用小乌龟测试是否能正常运行。此外，还需要测试RVIZ是否能正常工作。

1、RTK轨迹（GT）显示

首先，创建工作空间showTrack_ws

mkdir -p showTrack_ws/src
cd showTrack_ws
catkin_make

进入工作空间后，可以用两种方法创建功能包。这里推荐方法二，因为后续都是通过Vscode完成的，更加直观。

方法一：直接创建（showtrack是功能包名字，roscpp、rospy、std_msgs是编译依赖）

catkin_create_pkg showtrack roscpp rospy std_msgs

方法二：进入Vscode创建功能包，在工作空间下执行命令

code .

选择创建功能包

创建功能包后会先设置功能包名称，再给定功能包依赖。名称依赖项如方法一所示。

名称：

依赖：

创建完成后左侧应该是这样子的

我们在showtrack功能包的src下创建showtrack.cpp，并把以下内容复制在cpp文件里：

#include<stdio.h>
#include<ros/ros.h>
#include<nav_msgs/Path.h>
#include<nav_msgs/Odometry.h>
#include <iostream>
#include <vector>
#include <algorithm>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <cmath>

using namespace std;
using namespace Eigen;

ros::Publisher path_pub;    //发布path的节点
ros::Subscriber odom_sub;   //接收odometry的节点

nav_msgs::Path path;        //发布的路径
bool init_flag=1;             //path初始化标记位

void odomCallback(const nav_msgs::Odometry::ConstPtr& odom){
    if(init_flag){
        path.header=odom->header;
        init_flag=0;
    }

    geometry_msgs::PoseStamped pose_stamped_this;
    pose_stamped_this.header=odom->header;
    pose_stamped_this.header.frame_id="world";
    // 坐标转换xyz，GPS到坐标原点
    // 旋转变化（弧度制）
    double PI = 3.141593;
    double yaw = 5 * PI / 4, pitch = 0, roll =  7 * PI / 4;
    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitZ());
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitX());
 
    Eigen::Quaterniond q = rollAngle * yawAngle * pitchAngle;
    Eigen::Matrix3d R = q.matrix();
    cout << "R = " << R << endl;

    pose_stamped_this.pose.position.x = odom->pose.pose.position.x + 0.2;
    pose_stamped_this.pose.position.y = odom->pose.pose.position.y + 8.6;
    pose_stamped_this.pose.position.z = odom->pose.pose.position.z + 18.8;

    Vector3d pose = {pose_stamped_this.pose.position.x, pose_stamped_this.pose.position.y, pose_stamped_this.pose.position.x};
    pose = R * pose;

    pose_stamped_this.pose.position.x = pose[0];
    pose_stamped_this.pose.position.y = pose[1];
    pose_stamped_this.pose.position.z = pose[2];
    
    path.header.frame_id = "world";

    path.poses.push_back(pose_stamped_this);
    path_pub.publish(path);
}


int main (int argc, char **argv)
{
    ros::init (argc, argv, "odometry2path");
    ros::NodeHandle nh;
    
 //       记得在此处更改odometry消息topic名称     //
    odom_sub  = nh.subscribe<nav_msgs::Odometry>("/USV2/NED_odometry", 10, odomCallback);
    path_pub = nh.advertise<nav_msgs::Path>("/USV2/NED_path",10, true);
 

    
    ros::spin();
    // ros::Rate loop_rate(50);
    // loop_rate.sleep();
    return 0;
}

复制完成后需要修改功能包路径下的CMakeList文件，

之后在直接ctrl+shift+B快捷键调用catkin_make命令编译即可（如果提示需要选择生成的任务类型，选择catkin_make:build即可）。之后在工作空间下执行以下命令运行ros节点：

source ./devel/setup.bash
rosrun showtrack showtrack

2、RVIZ轨迹显示

运行SLAM系统，并运行RVIZ，在RVIZ中根据话题添加可视化组件

运行SLAM系统即数据集之后即可显示对应的轨迹，效果如下：

如果需要修改参数，只需要在showtrack.cpp文件中修改对应的地方即可