為了更加直觀的在rviz中顯示機器人的軌跡（為了做感測器融合前後里程計精度的比對），準備寫一個節點訂閱odom話題轉為path後釋出出去。

　　$mkdir -p showpath/src;　　$cd src;　　$catkin_create_pkg showpath roscpp rospy sensor_msgs std_msgs nav_msgs tf;　　$catkin_make。以後都採用該方式建ros包，比較省心。

　　查詢資料,在ros answer中搜publish a path。需要把所有的odom話題中的pose訊息賦給一個geometry_msgs話題，再把新話題push_back到path話題中，最後再發布出去。一開始編譯通過並執行後，在rviz中訂閱成功卻不顯示。$rostopic echo /trajectory 後發現訊息不是在path.poses[]容器中，而是一個個單獨釋出的。這樣只能飛快地顯示一個個pose點。而nav_msgs/path須定義為一個全域性的變數，通過push_back新增新資料，因此解決方法是在類的private下定義一個nav_msgs::Path path。

 1 #include <ros/ros.h>
 2 #include <ros/console.h>
 3 #include <nav_msgs/Path.h>
 4 #include <nav_msgs/Odometry.h>
 5 #include <std_msgs/String.h>
 6 #include <geometry_msgs/Quaternion.h>
 7 #include <geometry_msgs/PoseStamped.h>
 8 #include <tf/transform_broadcaster.h>
 9
 
 #include <tf/tf.h>
10 class SubscribeAndPublish
11 {
12 public:
13     SubscribeAndPublish()
14     {
15         pub_=n_.advertise<nav_msgs::Path>("my_trajectory",1, true);
16         sub_=n_.subscribe("/odom2",1,&SubscribeAndPublish::callback,this);
17     }
18     void callback(const
 
 nav_msgs::Odometry input)
19     {    
20         geometry_msgs::PoseStamped pose;
21         
22         pose.header.stamp=input.header.stamp;
23         pose.header.frame_id="odom";
24         pose.pose.position.x=input.pose.pose.position.x;
25         pose.pose.position.y=input.pose.pose.position.y;
26         pose.pose.position.z=input.pose.pose.position.z;
27         pose.pose.orientation.x=input.pose.pose.orientation.x;
28         pose.pose.orientation.y=input.pose.pose.orientation.y;
29         pose.pose.orientation.z=input.pose.pose.orientation.z;
30         pose.pose.orientation.w=input.pose.pose.orientation.w;
31 
32         path.header.stamp=input.header.stamp;
33         path.header.frame_id="odom";
34         path.poses.push_back(pose);
35         pub_.publish(path);
36     }
37 private:
38     ros::NodeHandle n_;
39     ros::Publisher pub_;
40     ros::Subscriber sub_;
41     ros::Time time;
42     nav_msgs::Path path;
43 
44 };    
45 main (int argc, char **argv)
46 {
47     ros::init (argc, argv, "showpath");
48     SubscribeAndPublish SAP;
49     ros::spin();
50     return 0;
51 }