（九）ROS在rviz中实时显示轨迹（nav_msgs/Path消息的使用）

如何在rviz中如何实时显示轨迹呢？
本文分析nav_msgs/Path结构，实现在rviz中画出圆形轨迹。

1.使用参考代码画轨迹效果展示（hector_slam）

google rviz中如何实时显示轨迹?会有以下重要资料。
参考资料
参考http://answers.ros.org/question/209224/show-robot-trajectory-in-rviz-real-time/
hector_slam中实现了画轨迹。
网址http://wiki.ros.org/hector_slam/Tutorials/MappingUsingLoggedData
那么先用bag数据来跑一跑。
步骤如下：

（1）启动包

roslaunch hector_slam_launch tutorial.launch

（2）回放bag
rosbag play Team_Hector_MappingBox_RoboCup_2011_Rescue_Arena.bag --clock
（3）效果

（4）查看发送的topic
rostopic echo /trajectory

2.实现在rviz中画出圆形轨迹

以上的包太复杂，本文实现最简单的画轨迹功能。
##2.1分析nav_msgs/Path.msg结构
nav_msgs/Path.msg结构

std_msgs/Header header
geometry_msgs/PoseStamped[] poses

将以上结构展开如下：
std_msgs/Header结构

uint32 seq
time stamp
string frame_id

geometry_msgs/PoseStamped[]结构

std_msgs/Header header
geometry_msgs/Pose pose

geometry_msgs/Pose pose结构

# This represents an orientation in free space in quaternion form.
float64 x
float64 y
float64 z
float64 w

2.2实现画出圆形轨迹

（1）新建工程

mkdir -p showpath/src
cd src
catkin_create_pkg showpath roscpp rospy sensor_msgs std_msgs nav_msgs tf
cd ..
catkin_make 

（2）编辑主函数showpath.cpp

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

main (int argc, char **argv)
{
    ros::init (argc, argv, "showpath");

    ros::NodeHandle ph;
    ros::Publisher path_pub = ph.advertise("trajectory",1, true);

    ros::Time current_time, last_time;
    current_time = ros::Time::now();
    last_time = ros::Time::now();

    nav_msgs::Path path;
    //nav_msgs::Path path;
    path.header.stamp=current_time;
    path.header.frame_id="odom";


    double x = 0.0;
    double y = 0.0;
    double th = 0.0;
    double vx = 0.1;
    double vy = -0.1;
    double vth = 0.1;

    ros::Rate loop_rate(1);
    while (ros::ok())
    {

        current_time = ros::Time::now();
        //compute odometry in a typical way given the velocities of the robot
        double dt = 0.1;
        double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
        double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
        double delta_th = vth * dt;

        x += delta_x;
        y += delta_y;
        th += delta_th;


        geometry_msgs::PoseStamped this_pose_stamped;
        this_pose_stamped.pose.position.x = x;
        this_pose_stamped.pose.position.y = y;

        geometry_msgs::Quaternion goal_quat = tf::createQuaternionMsgFromYaw(th);
        this_pose_stamped.pose.orientation.x = goal_quat.x;
        this_pose_stamped.pose.orientation.y = goal_quat.y;
        this_pose_stamped.pose.orientation.z = goal_quat.z;
        this_pose_stamped.pose.orientation.w = goal_quat.w;

        this_pose_stamped.header.stamp=current_time;
        this_pose_stamped.header.frame_id="odom";
        path.poses.push_back(this_pose_stamped);

        path_pub.publish(path);
        ros::spinOnce();               // check for incoming messages

        last_time = current_time;
        loop_rate.sleep();
    }

    return 0;
}

（3）编辑CMakeLists.txt
在最后加入

add_executable(showpath src/showpath.cpp)
target_link_libraries(showpath ${catkin_LIBRARIES})

（4）编译和运行

编译

cd /home/topeet/workspace/showpath
catkin_make

运行

source ./devel/setup.bash
rosrun showpath showpath

查看/trajectory 信息

rostopic echo /trajectory

#3.rviz实验结果
运行

rosrun rviz rviz

在globel option的Fixed Fram输入odom
左边点击add
选中path
在path的topic选项中选
/trajectory