在path话题中存储odom消息并发布

　　为了更加直观的在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。

#include <ros/ros.h>
#include <ros/console.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.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>
class SubscribeAndPublish
{
public:
    SubscribeAndPublish()
    {
        pub_=n_.advertise<nav_msgs::Path>("my_trajectory",1, true);
        sub_=n_.subscribe("/odom2",1,&SubscribeAndPublish::callback,this);
    }
    void callback(const nav_msgs::Odometry input)
    {    
        geometry_msgs::PoseStamped pose;
        
        pose.header.stamp=input.header.stamp;
        pose.header.frame_id="odom";
        pose.pose.position.x=input.pose.pose.position.x;
        pose.pose.position.y=input.pose.pose.position.y;
        pose.pose.position.z=input.pose.pose.position.z;
        pose.pose.orientation.x=input.pose.pose.orientation.x;
        pose.pose.orientation.y=input.pose.pose.orientation.y;
        pose.pose.orientation.z=input.pose.pose.orientation.z;
        pose.pose.orientation.w=input.pose.pose.orientation.w;

        path.header.stamp=input.header.stamp;
        path.header.frame_id="odom";
        path.poses.push_back(pose);
        pub_.publish(path);
    }
private:
    ros::NodeHandle n_;
    ros::Publisher pub_;
    ros::Subscriber sub_;
    ros::Time time;
    nav_msgs::Path path;

};    
main (int argc, char **argv)
{
    ros::init (argc, argv, "showpath");
    SubscribeAndPublish SAP;
    ros::spin();
    return 0;
}