ROS之rviz顯示歷史運動軌跡、路徑的各種方法（visualization_msgs/Marker、nav_msgs/Path）

0. 寫在最前面

希望大家收藏：

本文持續更新地址：https://haoqchen.site/2018/05/25/ROS-show-trajectory/

左側專欄還在更新其他ROS實用技巧哦，關注一波？

    在使用ROS進行機器人控制的時候，有時候會希望能夠顯示並且保留機器人的歷史運動軌跡，比如最近在做行人跟蹤，就希望能夠保留多個行人的軌跡以及機器人的運動。本文將會介紹網上搜羅到的各種方法，並粗略比較一些優劣。

github地址：https://github.com/HaoQChen/show_trajectory

喜歡的幫我github點個贊，點個關注唄～～～～

1. 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

1.1 使用lifetime實現

    每次獲取到機器人的位姿后，就在對應點發布一個標志，然后將lifetime設為0，也就是無限久地保存～～但是需要注意一點，讓ns或者id變量每次都不一樣，否則ns和id一直一樣的話，后面的操作會覆蓋前面的操作，也就一直只能看到最新的了。建議每次讓id+=1。代碼見marker.cpp，這個是稍微修改了ROS官網Markers: Sending Basic Shapes (C++)的代碼。大家不要被顏色形狀什么的干擾～～～我只是懶得調。

1.2 使用標志序列或者點集實現

    這個沒啥好說的，程序都是copy官網的教程。但這個有個不好的是，如果時間比較久，需要保存一個很長的數組來存儲歷史軌跡的坐標。見marker_list.cpp

 

 

2. nav_msgs/Path

    這個功能是利用rviz中的Path類型實現的，只需要發布nav_msgs/Path類型的消息，然后在rviz上訂閱該消息就可以顯示軌跡路徑。而nav_msgs/Path類型的數據很簡單，就是一個位姿的集合。navigation功能包集中顯示規划路徑用的就是這個東西。

    這個有點像Marker中的序列和點集一樣，需要維護一個數組。在rviz中可以對路徑的顏色、寬度、透明度等進行設置，另外可以設置顯示箭頭和坐標。最終顯示的結果根Marker差不多，個人感覺是簡單方便，不用想這么多。代碼見path.cpp

//nav_msgs/Path數據類型  
Header header  
geometry_msgs/PoseStamped[] poses  
//類型擴展：  
Header header  
    uint32 seq  
    time stamp  
    string frame_id  
geometry_msgs/PoseStamped[] poses  
    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

效果圖：

 

3. 未完待續

看到github上一個開源項目，上面有更多可視化的圖形～～～～有興趣可以取看看：

github上的rviz_visual_tools

nav_msgs/Odometry也可以～～只需要發布好里程計數據（一般從輪子運動模型或者IMU或者視覺里程計，激光雷達里程計等發布）。然后在rviz中添加這個odometry就可以了。keep選多一點，這個是rviz會幫你緩存的數據量。length即向量的長度，選擇合適的即可。注意要將你的Fixed Frame選擇為odom的坐標系才會顯示。

參考

http://wiki.ros.org/rviz/Tutorials/Markers%3A%20Basic%20Shapes

http://wiki.ros.org/rviz/Tutorials/Markers%3A%20Points%20and%20Lines

https://blog.csdn.net/ktigerhero3/article/details/70256437