在工作空间中创建包

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;
}