Rviz教程-Marker：发送基本的形状（C++）

描述：显示如何使用 visualization_msgs/Marker 消息发送基本形状（立方体，球体，圆柱体，矢量）到rviz。

1. 介绍

不像其他的显示，Maker Display让你能够在不知道数据的任何解释的情况下观察数据。反而，基本形状通过 visualization_msgs/Marker消息显示，例如箭头，盒子，球和线。
这个教程显示如何发送四个基本形状（立方体，圆球，圆柱体和箭头）。将会创建一个程序，每秒发送一个新的marker，用不同的形状替代上一个。

2. 创建一个包

在开始之前，创建一个名为using_markers的包，在包路径下的某处：

catkin_create_pkg using_markers roscpp visualization_msgs

3. 发送Markers

3.1 代码
粘贴下面代码到src/basic_shapes.cpp：

#include 
#include 

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

现在编辑包中的Cmakelist.txt文件，添加;

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

3.2 代码解释

一点点拆解代码：

#include 
#include 

rose被包括进来了，也看到消息信息也被包含。

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_marker", 1);

看起来很熟悉，初始化rose，在话题 visualization_marker上创建一个ros::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，并且开始填充它。这里的header是roslib/Header。设置frame_id为/my_frame。在运行着的系统中，这个坐标系和需要的marker的坐标系被插入坐标系相关联。

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

命名空间ns和id被用作创建这个marker的唯一的名字。如果一个marker消息被相同的ns和id的节点接受的话，那么新的marker将会取代之前的那个marker。

    marker.type = shape;

type域指定发送的marker的种类。可用的type被集合在visualization_msgs/Marker中。这里设置type到shape变量，这个每次循环都会改变。

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

action域指定marker将会做什么。选项是 visualization_msgs::Marker::ADD
和visualization_msgs::Marker::DELETE. ADD有时会出错，真实的意思是创建或者改变。
在indigo中新的部分是：一个新的action被添加，用作删除特定Rviz显示中的所有marker，不管ID或者名称空间。值是3并且在未来的ROS版本中消息将会改变值为visualization_msgs::Marker::DELETEALL。

    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的位姿。

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

现在指定marker的尺寸。

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

指定marker的颜色。每个成员应该在0和1之间。alpha值为0意味着完全透明，1意味着完全不透明。

marker.lifetime = ros::Duration();

lifetime域指定marker在被自动删除之前会坚持多久。ros::Duration()意味着从不自动删除。
如果一个新的marker消息在lifetime之前被接收，lifetime将会在新的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);

我们会等待marker的订阅，并且之后发布marker。注意你也能使用latched publisher作为替代代码。

    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

4. 观察marker