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

Posted 2021-12-21 hyhy904

消息结构说明
nav_msgs/Path.msg结构
#An array of poses that represents a Path for a robot to follow
Header header
geometry_msgs/PoseStamped[] poses
1
2
3
geometry_msgs/PoseStamped.msg结构
# A Pose with reference coordinate frame and timestamp
Header header
Pose pose
1
2
3
geometry_msgs/Pose.msg结构
# A representation of pose in free space, composed of position and orientation.
Point position
Quaternion orientation
1
2
3
geometry_msgs/Point.msg结构
# This contains the position of a point in free space
float64 x
float64 y
float64 z
1
2
3
4
geometry_msgs/Quaternion.msg结构
# This represents an orientation in free space in quaternion form.

float64 x
float64 y
float64 z
float64 w
1
2
3
4
5
6
实现代码：
#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;

---------------------