这一节将会学习怎么使用visualization_msgs/Marker message发送一个基本形状到rviz

1 介绍

不像别的显示组件， Marker Display能让你直接可视化数据而不需要rviz知道任何东西。我们这一节就要学习怎么发送几个基本几何形状。

2 创建一个package

在开始之前，先创建一个package：
catkin_create_pkg using_markers roscpp visualization_msgs
我在此之前重新创建了一个工作区，然后才创建包的，可以看一下之前的教程。

3 发送Markers

3.1 源码

打开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();
  }
}

现在编辑 using_markers package里面的CMakeLists.txt 文件，增加下面的内容在最后面：

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包含了进来，还包含了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上面创建了一个publisher。

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();

这里开始了程序的实质的部分。首先我们创建了一个visualization_msgs/Marker，然后开始填充它。在这里我们的frame_id设置为/my_frame,这里只是作为一个例子，所以才这么取名字的，在一个运行的系统中，这个应该是你想要的坐标系相对于这个的解释，反正就是你要搞个有意义点的名字。

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

命名空间（ns）和id是用来给这个marker创建一个唯一的名字的。如果接收到一个相同命名空间和id的marker，那新的就会把老的替换掉。

marker.type = shape;

这个就是指定我们会发送哪种形状过去。我们把它设定成shape，这样我们改变shape就可以改变发送的形状。

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

这个是用来指定我们要对marker做什么的，有ADD和DELETE两个选项，ADD其实是创建或者修改的意思，看注释还增加了一个新的选项——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;

这里我们设置了marker的姿态。geometry_msgs/Pose message包含了一个geometry_msgs/Vector3来指定位置，还有一个geometry_msgs/Quaternion来指定方向。这里我们把方向设置为身份方向（w=1.0）

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

这里指定了标记的规模，对于基本形状，1表示在这边长度是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。每个数字介于0-1之间。最后一个a(alpha)表示透明度，0表示完全透明。

marker.lifetime = ros::Duration();

这个是指定marker在被自动清除前可以逗留多长时间。这里 ros::Duration()表示不自动删除。
如果在lifetime结束之前有新内容到达了，它就会被重置。

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);

这里是等待订阅者出现。你也可以在这个地方启动订阅者。

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;
    }

这个就是确定要发送那个marker的。根据上一句发送的内容切换到下一个

 r.sleep();
  }

睡眠，然后跳回循环顶部。

3.3 编译源码

$ cd %TOP_DIR_YOUR_CATKIN_WORKSPACE%
$ catkin_make

3.4 运行代码

rosrun using_markers basic_shapes

出现错误可能是你没有创建工作空间的原因，参考前面的教程创建。
这个时候会有一句黄色的Warning提醒你没有订阅者，使我们程序里输出的提醒。

4 查看Marker

新窗口中编译rviz：
rosmake
运行rviz：
rosrun rviz rviz
如果没有出现任何东西，可能是我前面跳过了一些内容的原因，因为教程中使用的软件版本跟我的不一样，所以我跳过了那部分内容，想看可以点这里。
我找到了一个解决办法可以用：

5 没有显示解决办法

可能会看到这样一个错误，并且没有显示图像：

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

首先安装basic_shapes：
$ catkin_make install
然后新窗口运行ROS:
roscore
在打开一个窗口运行发送程序：
$ 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 

我觉得真正对我的问题起作用的是这个。
然后再运行rviz：

$ rosrun rviz rviz

然后新建一个Marker，就可以看到了。
Marker的topic一栏跟我们程序里定义的topic一样。

问题

上面的

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

能够让左边的Global Status变成正常的，不过我发现一个方法也可以解决收不到数据的问题。
在我们的程序里，定义了 marker.header.frame_id = "/my_frame";这样的话，应该把global option里面的Fixed Frame改成/my_frame与之对应，这样就可以用了。