在rviz中实时显示车辆轨迹

在工作空间中创建包

cd ~/catkin_ws/src
catkin_create_pkg trajectory_display_example roscpp nav_msgs sensor_msgs

在src文件夹下创建一个C++源文件

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/Point.h>
#include <visualization_msgs/Marker.h>class TrajectoryDisplay
{
public:TrajectoryDisplay(){ros::NodeHandle nh;odom_sub = nh.subscribe("/raw_odom", 10, &TrajectoryDisplay::odomCallback, this);imu_sub = nh.subscribe("imu", 10, &TrajectoryDisplay::imuCallback, this);path_pub = nh.advertise<nav_msgs::Path>("/trajectory_odom", 10);}void odomCallback(const nav_msgs::Odometry::ConstPtr& odom){geometry_msgs::PoseStamped this_pose_stamped;this_pose_stamped.pose.position.x = odom->pose.pose.position.x;this_pose_stamped.pose.position.y = odom->pose.pose.position.y;this_pose_stamped.pose.position.z = odom->pose.pose.position.z;this_pose_stamped.pose.orientation = odom->pose.pose.orientation;this_pose_stamped.header.stamp = ros::Time::now();this_pose_stamped.header.frame_id = "odom";path.poses.push_back(this_pose_stamped);path.header.stamp = ros::Time::now();path.header.frame_id = "odom";path_pub.publish(path);ROS_INFO("odom : %f, %f, %f", odom->pose.pose.position.x, odom->pose.pose.position.y, odom->pose.pose.position.z);}void imuCallback(const sensor_msgs::Imu::ConstPtr& msg){// Do something with IMU data if needed}private:ros::Subscriber odom_sub;ros::Subscriber imu_sub;ros::Publisher path_pub;nav_msgs::Path path;
};int main(int argc, char** argv)
{ros::init(argc, argv, "trajectory_display_node");TrajectoryDisplay trajectory_display;ros::spin();return 0;
}