任务描述：实现在Rviz中对机械臂末端轨迹的绘制
实现方法：先将路径点存在事先定义好的路径变量中，以话题的形式发布出去。在RVIZ中添加可视化组件“path”，订阅路径话题。
1.包含头文件
#include
2.以话题发布的形式发布
int main(int argc, char *argv[])
{
ros::init(argc,argv,"showpath");
ros::NodeHandle nh;
ros::Publisher path_pub = nh.advertise("Path",1,true);
double position[5][3] = {{0,0,0},{0.5,0.10,0.5},{0.7,0.6,0.15},{0.20,0.16,0.30},{0.10,0.20,0.30}};
nav_msgs::Path path;
path.header.frame_id = "world";
for(int i=0;i<5;i++){
geometry_msgs::PoseStamped point;
point.pose.position.x = position[i][0];
point.pose.position.y = position[i][1];
point.pose.position.z = position[i][2];
path.poses.push_back(point);
}
while(ros::ok()){
path_pub.publish(path);
ros::spinOnce();
}
return 0；
}
3.添加可视化组件
4.订阅话题
————————————————————分割线————————————————————
上面的方法虽然可以实现路径的显示，但我个人感觉大多数时候还是用在实时路径规划上比较方便，毕竟有时间戳的存在。然而，对于RRT这种离线搜索，并且在储存结构上离散的路径绘制来说很困难。下面推荐一种利用Marker的路径绘制方法
RRT算法原理很简单，参考这篇：RRT算法(快速拓展随机树)的Python实现
另外，虽然本文使用python实现的，想用C++实现，步骤基本一致，只需把一些消息定义进行修改
流程概述：
- 定义存储随机树节点的变量(RRT_marker)
- 创建话题的发布者(rrtMarker_pub)用来发布RRT_marker
- 在RRT算法中增加节点push算法
1.导入Marker消息类型
from visualization_msgs.msg import Marker //树的数据类型 from geometry_msgs.msg import Point //节点的数据类型
2.创建并初始化树
rrtMarker = Marker() rrtMarker.header.frame_id = "world" //参考坐标系 rrtMarker.type = Marker.LINE_LIST //标记样式，有序点集 rrtMarker.color.g = 1.0 //颜色RGB值 rrtMarker.color.a = 1.0 //透明度 rrtMarker.scale.x = 0.001 //标记大小 rrtMarker.pose.orientation.w = 1.0 //方向。如果是箭头，圆柱等样式的标记可能会用到 rrtMarker.lifetime = rospy.Duration() //(没用明白)
博主在想能不能利用lifetime属性摆脱不得不使用循环发布话题的做法，但是没成功，如果有人成功可以交流交流。
关于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 uint8 MESH_RESOURCE=10 uint8 TRIANGLE_LIST=11 uint8 ADD=0 uint8 MODIFY=0 uint8 DELETE=2 uint8 DELETEALL=3 Header header # 时间和坐标系信息 string ns # 这些markers的名称空间。这给marker加上一个唯一的id int32 id # 分配给marker的唯一的id。 int32 type # maker的类型。可用的类型在消息定义中有说明 int32 action # 0=add/modify an object, 1=(deprecated), 2=deletes an object, 3=deletes all objects geometry_msgs/Pose pose # marker的位姿，内容是3维坐标和一组方向四元数 geometry_msgs/Vector3 scale # marker的尺寸，在位姿/方向之前应用，[1,1,1]意思是尺寸是1m*1m*1m std_msgs/ColorRGBA color # 物体的颜色，由r/g/b/a指定，取值范围是[0,1]。不要忘记设置a否则它会默认是0，导致marker不可见。 duration lifetime # 一个区间值，在这段时间之后自动删除marker，这个向下计数器被重置，如果另一个同样namespace/id 的marker被接收到时。0的意思是不会删除这个marker bool frame_locked # 告知rviz每次更新循环重新发布变换到指定坐标系的当前位置。 #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 #仅限使用点成员的marker，指定每个定点颜色。 # NOTE: 仅限于text markers string text #文本字符串 # NOTE: only used for MESH_RESOURCE markers string mesh_resource #MESH_RESOURCE markers的源位置。任何rviz支持的mesh类型都可以使用。格式是被resource_retriever使用的URI-form。 bool mesh_use_embedded_materials
来源：rviz–显示类型-Marker
3.2 创建话题发布者rrtMarker_pub = rospy.Publisher("RRT_planning_markerpath",Marker)
输入参数：
- 发布话题名称
- 数据类型
- 队列长度 (queue_size=)。这里不设置会报警，不过影响不大所以没设置。
直接上代码
marker_new_point = Point() marker_nearest_point = Point() marker_new_point.x = new_node.x marker_new_point.y = new_node.y marker_new_point.z = new_node.z rrtMarker.points.append(marker_new_point) marker_nearest_point.x = nearest_node.x marker_nearest_point.y = nearest_node.y marker_nearest_point.z = nearest_node.z rrtMarker.points.append(marker_nearest_point)
因为在对rrtMarker初始化时，标记类型设定为有序列表。所以在push节点时必须成对添加。
4. 主函数循环while not rospy.is_shutdown():
rrtMarker_pub.publish(rrtMarker)
–PS：第一次正儿八经用python写代码，还挺有意思，但是用着还是感觉不太舒服，和C++比少了些什么的感觉。
此外，附上C++的参考
ROS(indigo)RRT路径规划
github案例
