這一節(jié)將會(huì)學(xué)習(xí)怎么使用visualization_msgs/Marker message發(fā)送一個(gè)基本形狀到rviz

1 介紹

不像別的顯示組件， Marker Display能讓你直接可視化數(shù)據(jù)而不需要rviz知道任何東西贞远。我們這一節(jié)就要學(xué)習(xí)怎么發(fā)送幾個(gè)基本幾何形狀。

2 創(chuàng)建一個(gè)package

在開(kāi)始之前活合，先創(chuàng)建一個(gè)package：
catkin_create_pkg using_markers roscpp visualization_msgs
我在此之前重新創(chuàng)建了一個(gè)工作區(qū)棵癣，然后才創(chuàng)建包的上祈，可以看一下之前的教程。

3 發(fā)送Markers

3.1 源碼

打開(kāi)using_markers package：
一定要在工作空間并且source 了setup.bash
roscd using_markers
新建：
vim src/basic_shapes.cpp
把下面的代碼粘貼到src/basic_shapes.cpp:

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

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

  // Set our initial shape type to be a cube
  uint32_t shape = visualization_msgs::Marker::CUBE;

  while (ros::ok())
  {
    visualization_msgs::Marker marker;
    // Set the frame ID and timestamp.  See the TF tutorials for information on these.
    marker.header.frame_id = "/my_frame";
    marker.header.stamp = ros::Time::now();

    // Set the namespace and id for this marker.  This serves to create a unique ID
    // Any marker sent with the same namespace and id will overwrite the old one
    marker.ns = "basic_shapes";
    marker.id = 0;

    // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
    marker.type = shape;

    // Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
    marker.action = visualization_msgs::Marker::ADD;

    // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
    marker.pose.position.x = 0;
    marker.pose.position.y = 0;
    marker.pose.position.z = 0;
    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;

    // Set the scale of the marker -- 1x1x1 here means 1m on a side
    marker.scale.x = 1.0;
    marker.scale.y = 1.0;
    marker.scale.z = 1.0;

    // Set the color -- be sure to set alpha to something non-zero!
    marker.color.r = 0.0f;
    marker.color.g = 1.0f;
    marker.color.b = 0.0f;
    marker.color.a = 1.0;

    marker.lifetime = ros::Duration();

    // Publish the marker
    while (marker_pub.getNumSubscribers() < 1)
    {
      if (!ros::ok())
      {
        return 0;
      }
      ROS_WARN_ONCE("Please create a subscriber to the marker");
      sleep(1);
    }
    marker_pub.publish(marker);

    // Cycle between different shapes
    switch (shape)
    {
    case visualization_msgs::Marker::CUBE:
      shape = visualization_msgs::Marker::SPHERE;
      break;
    case visualization_msgs::Marker::SPHERE:
      shape = visualization_msgs::Marker::ARROW;
      break;
    case visualization_msgs::Marker::ARROW:
      shape = visualization_msgs::Marker::CYLINDER;
      break;
    case visualization_msgs::Marker::CYLINDER:
      shape = visualization_msgs::Marker::CUBE;
      break;
    }

    r.sleep();
  }
}

現(xiàn)在編輯 using_markers package里面的CMakeLists.txt 文件腿箩，增加下面的內(nèi)容在最后面：

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

3.2 代碼解析

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

我們將ROS包含了進(jìn)來(lái)豪直，還包含了visualization_msgs/Marker 消息的定義。

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

我們初始化了ROS珠移，然后在visualization_marker topic上面創(chuàng)建了一個(gè)publisher弓乙。

uint32_t shape = visualization_msgs::Marker::CUBE;

這里我們創(chuàng)建了一個(gè)整型來(lái)追蹤我們將要發(fā)布什么形狀。因?yàn)槲覀儗⒁l(fā)布的四種形狀都是以相同的形式發(fā)送的钧惧，所以我們只需要切換形狀就好了暇韧。

 while (ros::ok())
  {
    visualization_msgs::Marker marker;
    // Set the frame ID and timestamp.  See the TF tutorials for information on these.
    marker.header.frame_id = "/my_frame";
    marker.header.stamp = ros::Time::now();

這里開(kāi)始了程序的實(shí)質(zhì)的部分。首先我們創(chuàng)建了一個(gè)visualization_msgs/Marker浓瞪，然后開(kāi)始填充它懈玻。在這里我們的frame_id設(shè)置為/my_frame,這里只是作為一個(gè)例子，所以才這么取名字的乾颁，在一個(gè)運(yùn)行的系統(tǒng)中涂乌，這個(gè)應(yīng)該是你想要的坐標(biāo)系相對(duì)于這個(gè)的解釋，反正就是你要搞個(gè)有意義點(diǎn)的名字英岭。

 marker.ns = "basic_shapes";
 marker.id = 0;

命名空間（ns）和id是用來(lái)給這個(gè)marker創(chuàng)建一個(gè)唯一的名字的湾盒。如果接收到一個(gè)相同命名空間和id的marker，那新的就會(huì)把老的替換掉诅妹。

marker.type = shape;

這個(gè)就是指定我們會(huì)發(fā)送哪種形狀過(guò)去罚勾。我們把它設(shè)定成shape，這樣我們改變shape就可以改變發(fā)送的形狀吭狡。

marker.action = visualization_msgs::Marker::ADD;

這個(gè)是用來(lái)指定我們要對(duì)marker做什么的尖殃，有ADD和DELETE兩個(gè)選項(xiàng)，ADD其實(shí)是創(chuàng)建或者修改的意思划煮，看注釋還增加了一個(gè)新的選項(xiàng)——DELETALL送丰。

    marker.pose.position.x = 0;
    marker.pose.position.y = 0;
    marker.pose.position.z = 0;
    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;

這里我們?cè)O(shè)置了marker的姿態(tài)。geometry_msgs/Pose message包含了一個(gè)geometry_msgs/Vector3來(lái)指定位置弛秋，還有一個(gè)geometry_msgs/Quaternion來(lái)指定方向蚪战。這里我們把方向設(shè)置為身份方向（w=1.0）

    marker.scale.x = 1.0;
    marker.scale.y = 1.0;
    marker.scale.z = 1.0;

這里指定了標(biāo)記的規(guī)模牵现，對(duì)于基本形狀铐懊，1表示在這邊長(zhǎng)度是1米邀桑。

    marker.color.r = 0.0f;
    marker.color.g = 1.0f;
    marker.color.b = 0.0f;
    marker.color.a = 1.0;

marker的顏色就像指定 std_msgs/ColorRGBA。每個(gè)數(shù)字介于0-1之間科乎。最后一個(gè)a(alpha)表示透明度壁畸，0表示完全透明。

marker.lifetime = ros::Duration();

這個(gè)是指定marker在被自動(dòng)清除前可以逗留多長(zhǎng)時(shí)間茅茂。這里 ros::Duration()表示不自動(dòng)刪除捏萍。
如果在lifetime結(jié)束之前有新內(nèi)容到達(dá)了，它就會(huì)被重置空闲。

while (marker_pub.getNumSubscribers() < 1)
    {
      if (!ros::ok())
      {
        return 0;
      }
      ROS_WARN_ONCE("Please create a subscriber to the marker");
      sleep(1);
    }
    marker_pub.publish(marker);

這里是等待訂閱者出現(xiàn)令杈。你也可以在這個(gè)地方啟動(dòng)訂閱者。

switch (shape)
    {
    case visualization_msgs::Marker::CUBE:
      shape = visualization_msgs::Marker::SPHERE;
      break;
    case visualization_msgs::Marker::SPHERE:
      shape = visualization_msgs::Marker::ARROW;
      break;
    case visualization_msgs::Marker::ARROW:
      shape = visualization_msgs::Marker::CYLINDER;
      break;
    case visualization_msgs::Marker::CYLINDER:
      shape = visualization_msgs::Marker::CUBE;
      break;
    }

這個(gè)就是確定要發(fā)送那個(gè)marker的碴倾。根據(jù)上一句發(fā)送的內(nèi)容切換到下一個(gè)

 r.sleep();
  }

睡眠逗噩，然后跳回循環(huán)頂部。

3.3 編譯源碼

$ cd %TOP_DIR_YOUR_CATKIN_WORKSPACE%
$ catkin_make

3.4 運(yùn)行代碼

rosrun using_markers basic_shapes

出現(xiàn)錯(cuò)誤可能是你沒(méi)有創(chuàng)建工作空間的原因跌榔，參考前面的教程創(chuàng)建异雁。
這個(gè)時(shí)候會(huì)有一句黃色的Warning提醒你沒(méi)有訂閱者，使我們程序里輸出的提醒僧须。

4 查看Marker

新窗口中編譯rviz：
rosmake
運(yùn)行rviz：
rosrun rviz rviz
如果沒(méi)有出現(xiàn)任何東西纲刀，可能是我前面跳過(guò)了一些內(nèi)容的原因，因?yàn)榻坛讨惺褂玫能浖姹靖业牟灰粯拥Ｆ剑晕姨^(guò)了那部分內(nèi)容示绊，想看可以點(diǎn)這里。
我找到了一個(gè)解決辦法可以用：

5 沒(méi)有顯示解決辦法

可能會(huì)看到這樣一個(gè)錯(cuò)誤暂论，并且沒(méi)有顯示圖像：

No tf data.  Actual error: Fixed Frame [map] does not exist

首先安裝basic_shapes：
$ catkin_make install
然后新窗口運(yùn)行ROS:
roscore
在打開(kāi)一個(gè)窗口運(yùn)行發(fā)送程序：
$ rosrun using_markers basic_shapes
然后新窗口輸入：

$ rosrun tf static_transform_publisher 0.0 0.0 0.0 0.0 0.0 0.0 map my_frame 100 

我覺(jué)得真正對(duì)我的問(wèn)題起作用的是這個(gè)面褐。
然后再運(yùn)行rviz：

$ rosrun rviz rviz

然后新建一個(gè)Marker，就可以看到了空另。
Marker的topic一欄跟我們程序里定義的topic一樣盆耽。

問(wèn)題

上面的

$ rosrun tf static_transform_publisher 0.0 0.0 0.0 0.0 0.0 0.0 map my_frame 100 

能夠讓左邊的Global Status變成正常的，不過(guò)我發(fā)現(xiàn)一個(gè)方法也可以解決收不到數(shù)據(jù)的問(wèn)題扼菠。
在我們的程序里摄杂，定義了 marker.header.frame_id = "/my_frame";這樣的話，應(yīng)該把global option里面的Fixed Frame改成/my_frame與之對(duì)應(yīng)循榆，這樣就可以用了析恢。