1.利用nav_msgs/Path消息实现轨迹显示
- 主函数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 = 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;
}
- 编辑CMakeLists.txt
add_executable(showpath src/showpath.cpp)
target_link_libraries(showpath ${catkin_LIBRARIES}) - rviz显示
在globel option的Fixed Fram输入odom
左边点击add
选中path
在path的topic选项中选
/trajectory
2.利用visualization_msgs/Marker消息类型显示轨迹
如名字所示,就是画出可视化的标志物。利用Marker有两种方法可以实现画出轨迹。相对于后面的方法来说,使用Marker可以有丰富的形状选择。首先看这个类包含的成员:
//各种标志物类型的定义,每一个的具体介绍和形状可以到这里查看:http://wiki.ros.org/rviz/DisplayTypes/Marker
uint8 ARROW=0//箭头
uint8 CUBE=1//立方体
uint8 SPHERE=2//球
uint8 CYLINDER=3//圆柱体
uint8 LINE_STRIP=4//线条(点的连线)
uint8 LINE_LIST=5//线条序列
uint8 CUBE_LIST=6//立方体序列
uint8 SPHERE_LIST=7//球序列
uint8 POINTS=8//点集
uint8 TEXT_VIEW_FACING=9//显示3D的文字
uint8 MESH_RESOURCE=10//网格?
uint8 TRIANGLE_LIST=11//三角形序列
//对标记的操作
uint8 ADD=0
uint8 MODIFY=0
uint8 DELETE=2
uint8 DELETEALL=3
Header header
string ns //命名空间namespace,就是你理解的那样
int32 id //与命名空间联合起来,形成唯一的id,这个唯一的id可以将各个标志物区分开来,使得程序可以对指定的标志物进行操作
int32 type //类型
int32 action //操作,是添加还是修改还是删除
geometry_msgs/Pose pose # Pose of the object
geometry_msgs/Vector3 scale # Scale of the object 1,1,1 means default (usually 1 meter square)
std_msgs/ColorRGBA color # Color [0.0-1.0]
duration lifetime # How long the object should last before being automatically deleted. 0 means forever
bool frame_locked # If this marker should be frame-locked, i.e. retransformed into its frame every timestep
#Only used if the type specified has some use for them (eg. POINTS, LINE_STRIP, ...)
geometry_msgs/Point[] points//这个是在序列、点集中才会用到,指明序列中每个点的位置
#Only used if the type specified has some use for them (eg. POINTS, LINE_STRIP, ...)
#number of colors must either be 0 or equal to the number of points
#NOTE: alpha is not yet used
std_msgs/ColorRGBA[] colors
# NOTE: only used for text markers
string text
# NOTE: only used for MESH_RESOURCE markers
string mesh_resource
bool mesh_use_embedded_materials
参考:https://blog.csdn.net/u013834525/article/details/80447931