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

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

1.1分析nav_msgs/Path.msg结构
nav_msgs/Path.msg总的结构

std_msgs/Header header
	uint32 seq
	time stamp
	string frame_id
geometry_msgs/PoseStamped[] poses
	std_msgs/Header header
		uint32 seq
		time stamp
		string frame_id
	geometry_msgs/Pose pose
		geometry_msgs/Point position
			float64 x
			float64 y
			float64 z
		geometry_msgs/Quaternion orientation
			float64 x
			float64 y
			float64 z		
			float64 w

以下为分解的结构：

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

1.2实现画出圆形轨迹
（1）新建工程

mkdir -p catkin_ws/src
cd src
catkin_create_pkg showpath roscpp  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="world"; 

	double x = 0.0; 
	double y = 0.0; 
	double th = 0.0; 
	double vx = 0.1; 
	double vy = -0.1; 
	double vth = 0.1; //2运动的三自由度量

	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="world"; 
		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 ~/catkin_ws/
catkin_make

运行

source ./devel/setup.bash
rosrun showpath showpath

查看/trajectory 信息

rostopic echo /trajector

2.rviz实验结果
运行

rosrun rviz rviz

在globel option的Fixed Fram输入odom
左边点击add
选中path
在path的topic选项中选
/trajectory
改下代码（将X方向的初始速度分量设为0）：

double x = 0.0; 
double y = 0.0;   //初始的xy起点
double th = 0.0;  //初始的角度
double vx = 0;    //初始的x轴向速度
double vy = 0.1; //初始的y轴向速度
double vth = 0.1;//初始的角速度