詳解如何在RVIZ中用Marker顯示機器人運動路徑

寫在前面

最近有道作業題需要將機器人的歷史路徑顯示出來，但是網上很多相關的教程都是搬運了官網的鏈接，並沒有詳細的操作流程。。。因此我又花費了很多時間去ros官網上學習marker的用法，學習怎么寫publisher和subscriber，最終成功將路徑顯示了出來。這篇文章是對這個過程的詳細的介紹，原理和代碼實踐部分會分開，因此如果你趕時間只要一個結果的話，把詳細的講解跳過即可。不過我還是推薦看完，畢竟我花了這么多精力來寫知其然也要知其所以然。

（我是ROS新手，從作業這一點也應該看出來了XD，有不對的地方歡迎指正）

你應該已經完成了的准備工作

• 已經配置好了ROS工作環境。這一點不用多說，參考ROS wiki。
• 這里我用的是Gazebo仿真環境和RVIZ可視化工具，這兩個東西是安裝ros時用官網上那個最全的安裝包里自帶（這里展示的是kinetic版本的安裝命令，其他的版本到上面的鏈接里面找）：

$ sudo apt install ros-kinetic-desktop-full

• 已經跟着ROS wiki建立了catkin工作區，我這里的工作區就是wiki上的~/catkin_ws，如果你的不是在用戶根目錄下記得在后面執行命令的時候改掉。

• 工作區目錄下的src目錄在系統的ROS_PACKAGE_PATH環境變量中，不在的話可以這樣設置：

$ export ROS_PACKAGE_PATH=${ROS_PACKAEG_PATH}:${HOME}/catkin_ws/src

寫路徑顯示的執行結點

首先要創建一個package，名為show_path。如果用python倒是不需要，可我這用的是C++：

$ cd ~/catkin_ws/src
$ catkin_create_pkg show_path std_msgs roscpp visualization_msgs tf

源代碼

將下面的代碼放進一個cpp文件，放在剛建好的package下的src目錄下。即~/catkin_ws/src/show_path/src/

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <visualization_msgs/Marker.h>

int main( int argc, char** argv )
{
  ros::init(argc, argv, "showpath");
  ros::NodeHandle n;
  ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);
  ros::Rate r(10);
  tf::TransformListener listener;

  while (!ros::ok()){
    r.sleep();
  }

  visualization_msgs::Marker points, line_strip;
  points.header.frame_id = line_strip.header.frame_id = "/map";
  points.header.stamp = line_strip.header.stamp = ros::Time::now();
  points.ns = line_strip.ns = "showpath";
  points.action = line_strip.action = visualization_msgs::Marker::ADD;
  points.pose.orientation.w = line_strip.pose.orientation.w = 1.0;

  points.id = 0;
  line_strip.id = 1;

  points.type = visualization_msgs::Marker::POINTS;
  line_strip.type = visualization_msgs::Marker::LINE_STRIP;

  line_strip.scale.x = 0.05;

  line_strip.color.b = 1.0;
  line_strip.color.a = 1.0;

  float x(0), y(0);
  int cnt(0);

  while (ros::ok())
  {
    tf::StampedTransform transform;
    try{
      listener.lookupTransform("/map", "/base_footprint",
                               ros::Time(0), transform);
    }
    catch (tf::TransformException ex){
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
    }
    x = transform.getOrigin().x();
    y = transform.getOrigin().y();

    geometry_msgs::Point p;
    p.x = x;
    p.y = y;
    p.z = 0.1;

    if (cnt > 1) {line_strip.points.push_back(p);}
    else {cnt++;}

    marker_pub.publish(line_strip);

    r.sleep();

  }
}

源代碼詳解

下面我分塊講解以下本例中的源代碼，方便大家根據需要更改。

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <visualization_msgs/Marker.h>

引入必要的頭文件。ros.h是C++的api；transform_listener.h的引入是為了獲取機器人的坐標；Marker.h則是ros的message，本方法的主角。關於Marker的詳細介紹參照ROS的Marker參考頁。

  ros::init(argc, argv, "showpath");
  ros::NodeHandle n;
  ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);
  ros::Rate r(10);
  tf::TransformListener listener;

初始化結點，命名為“showpath”，發布話題為“visualization_marker”，頻率為10。同時聲明tf坐標聽者對象listener。

  while (!ros::ok()){
    r.sleep();
  }

等待ros連接

  visualization_msgs::Marker points, line_strip;
  points.header.frame_id = line_strip.header.frame_id = "/map";
  points.header.stamp = line_strip.header.stamp = ros::Time::now();
  points.ns = line_strip.ns = "showpath";
  points.action = line_strip.action = visualization_msgs::Marker::ADD;
  points.pose.orientation.w = line_strip.pose.orientation.w = 1.0;

定義了兩個對象並且將它們初始化：

• points是點，並不顯示，但是是組成線的基本單元。
• line_strip線，用於標示歷史路徑。

  points.id = 0;
  line_strip.id = 1;

前面points和line_strip共用同一個namespace即showpath，這里把兩者的id作出區別以免發生沖突。

  points.type = visualization_msgs::Marker::POINTS;
  line_strip.type = visualization_msgs::Marker::LINE_STRIP;

設置兩種Marker的種類。

  line_strip.scale.x = 0.05;

  line_strip.color.b = 1.0;
  line_strip.color.a = 1.0;

設置軌跡的寬度(scale)和顏色(color)。這里設置寬度為0.05， 顏色為藍色不透明。

tf::StampedTransform transform;
    try{
      listener.lookupTransform("/map", "/base_footprint",
                               ros::Time(0), transform);
    }
    catch (tf::TransformException ex){
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
    }

在連接的時間內，持續獲取/map到/base_footprint的坐標，也就是機器人在地圖上的坐標，返回到listener對象中。

    x = transform.getOrigin().x();
    y = transform.getOrigin().y();

    geometry_msgs::Point p;
    p.x = x;
    p.y = y;
    p.z = 0.1;

取出機器人在地圖上的坐標，並且記錄在點p內。

    if (cnt > 1) {line_strip.points.push_back(p);}
    else {cnt++;}

將點p加入line_strip內的點隊列內。排除掉第一個點，第一個點有可能會初始化到原點，使路徑中出現一段不該有的直線。當然，只是我個人遇到的問題，不敢說一定會這這樣。

    marker_pub.publish(line_strip);

    r.sleep();

發布話題以及必要的sleep

后續工作

在剛建立的package里的CMakeList.txt中加入以下兩行：

add_executable(showpath src/showpath.cpp)
target_link_libraries(showpath ${catkin_LIBRARIES})

之后就可以開始愉快地make啦。

$ cd ~/catkin_ws
$ catkin_make

之后

$ source devel/setup.bash

顯示路徑

首先還是將turtlebot放進我們的仿真環境。這里就用默認的做個范例：

$ roslaunch turtlebot_gazebo turtlebot_world.launch

之后，啟動自動導航，調出地圖。

$ roslaunch turtlebot_gazebo amcl_demo.launch
$ roslaunch turtlebot_rviz_launchers view_navigation.launch

在rviz里訂閱Marker話題：點擊左下角的“add”，在彈出的對話框中選擇Marker。

獲取坐標需要一定的歷史信息，所以可以先讓機器人運動一下，再啟動showpath節點：

$ rosrun show_path showpath

可能會有這樣的報錯信息，忽略就好，不影響最終效果（如果有知道為什么請賜教，在此謝過）

給機器人定個目標位姿，導航過去就可以看到路徑用藍色線條標出來啦。

PS:雖然這里是用導航來移動機器人的，但是我們標記歷史路徑的程序並沒有用到導航信息。事實上這里用別的方式去移動機器人，也是可以畫出路徑的，只是，沒有地圖的話，這個路徑的意義就不是那么明顯了。

參考鏈接

• Marker 基礎形狀教程
• Marker 點和線教程
• Marker參考頁