（九）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 <ros/ros.h>
#include <ros/console.h>
#include <nav_msgs/Path.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>

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

ros::NodeHandle ph;
ros::Publisher path_pub = ph.advertise<nav_msgs::Path>("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 = (current_time - last_time).toSec();
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