一、显示自定义圆形轨迹

参考：（九）ROS在rviz中实时显示轨迹（nav_msgs/Path消息的使用）_nav_msgs/path.h_火星机器人life的博客-CSDN博客

默认代码的画圆速度很慢，改变ROS周期可以调整 //ros::Rate loop_rate(100);

二、显示GNSS/INS轨迹

在接收到GNSS/INS位置和姿态信息后，由回调函数绘制轨迹path

2.1代码show_path.cpp

订阅GNSS/INS位置姿态信息，发布运动轨迹path

#include <ros/ros.h>
#include <ros/console.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/NavSatFix.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>

#include "eul2qua.h"
#include "lla2utm.h"

nav_msgs::Path  path;
ros::Publisher  path_pub;

void odomCallback(const geometry_msgs::PoseStampedConstPtr& odom)
{
    geometry_msgs::PoseStamped this_pose_stamped;
    double pre_longitude = 116.17487972 , pre_latitude = 40.13351039 , pre_height = 37.43;
    double LL2UTM_E0 = LonLat2UTM(pre_longitude, pre_latitude).first;
    double LL2UTM_N0 = LonLat2UTM(pre_longitude, pre_latitude).second;
    // cout << "original: "<< LL2UTM_E0 << endl;
    // cout << "original: "<< LL2UTM_N0 << endl;

    double new_longitude = odom->pose.position.x;
    double new_latitude  = odom->pose.position.y;
    double LL2UTM_E_new = LonLat2UTM(new_longitude, new_latitude).first;
    double LL2UTM_N_new = LonLat2UTM(new_longitude, new_latitude).second;

    this_pose_stamped.pose.position.x = LL2UTM_E_new - LL2UTM_E0;
    this_pose_stamped.pose.position.y = LL2UTM_N_new - LL2UTM_N0;
    this_pose_stamped.pose.position.z = odom->pose.position.z - pre_height;

    this_pose_stamped.pose.orientation = odom -> pose.orientation;
 
    this_pose_stamped.header.stamp = ros::Time::now();
    this_pose_stamped.header.frame_id = "map";
 
    path.poses.push_back(this_pose_stamped);
    path.header.stamp = ros::Time::now();
    path.header.frame_id = "map";
    
    path_pub.publish(path);
    cout << "odom: "<<this_pose_stamped.pose.position.x << "  " << this_pose_stamped.pose.position.y << "  " << this_pose_stamped.pose.position.z << endl;
 
}
 
int main (int argc, char **argv)
{
    ros::init (argc, argv, "showpath");
    ros::NodeHandle ph;
    
    path_pub = ph.advertise<nav_msgs::Path>("trajectory_odom", 10, true);
    ros::Subscriber odomSub = ph.subscribe<geometry_msgs::PoseStamped>("/lla_qua", 10, odomCallback);  //订阅里程计话题信息

    ros::spin();
    return 0;
}

2.2CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(show_path)

set(CMAKE_CXX_FLAGS "-std=c++11")
set(CMAKE_CXX_STANDARD 14)  


find_package(catkin REQUIRED COMPONENTS
  nav_msgs
  roscpp
  rospy
  # sensor_msgs
  std_msgs
  # tf
)
find_package(Eigen3 REQUIRED)
find_package(PCL REQUIRED)

catkin_package(
  CATKIN_DEPENDS roscpp std_msgs
  DEPENDS EIGEN3 PCL 
  INCLUDE_DIRS include
)

include_directories(
  include
  ${catkin_INCLUDE_DIRS}
  ${PCL_INCLUDE_DIRS}
)

aux_source_directory(./src SRCS)
add_executable(show_path ${SRCS})
target_link_libraries(show_path ${catkin_LIBRARIES})