為了更加直觀的在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 }