ROS中级：Marker 向rviz发送基本形状

一、说明 

描述：展示如何使用visualization_msgs/Marker 消息将基本形状（立方体、球体、圆柱体、箭头）发送到rviz。

二、介绍

与其他显示不同，标记显示可让您在 rviz 中可视化数据，而 rviz 无需了解有关解释该数据的任何内容。相反，原始对象通过visualization_msgs/Marker 消息发送到显示器，让您可以显示箭头、框、球体和线条等内容。

本教程将向您展示如何发送四种基本形状（长方体、球体、圆柱体和箭头）。我们将创建一个程序，每秒发送一个新标记，用不同的形状替换最后一个。

注意：rviz_visual_tools 包为 C++ 用户提供了便利的功能。

三、创建一个包

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

catkin_create_pkg using_markers roscpp visualization_msgs

四、发送Marker代码 4.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();
  }
}

4.2、编辑CMake脚本

现在编辑 using_markers 包中的 CMakeLists.txt 文件，并添加：

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

五、程序分析 

1）头文件包含

#include 
#include 

您现在应该已经看到了 ROS 包括在内。我们还包括了visualization_msgs/Marker 消息定义。

2）主函数

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

这应该看起来很熟悉。我们初始化ROS，并在visualization_marker主题上创建一个ros::Publisher。

  uint32_t shape = visualization_msgs::Marker::CUBE;

在这里，我们创建一个整数来跟踪我们要发布的形状。我们将在这里使用的四种类型都以相同的方式使用visualization_msgs/Marker 消息，因此我们可以简单地切换形状类型来演示四种不同的形状。

  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，如果你学过 tf 教程应该很熟悉。我们将 frame_id 成员设置为 /my_frame 作为示例。在正在运行的系统中，这应该是您希望解释标记姿势的相对帧。

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

命名空间 (ns) 和 id 用于为此标记创建唯一名称。如果收到具有相同 ns 和 id 的标记消息，则新标记将替换旧标记。

  marker.type = shape;

此类型字段指定我们发送的标记类型。可用类型在visualization_msgs/Marker 消息中枚举。在这里，我们将类型设置为我们的形状变量，每次循环都会改变。

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

action 字段指定如何处理标记。选项是visualization_msgs::Marker::ADD 和visualization_msgs::Marker::DELETE。 ADD 有点用词不当，它的真正意思是“创建或修改”。

Indigo 中的新增功能 添加了新 *** 作以删除特定 Rviz 显示中的所有标记，无论 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的位姿。 geometry_msgs/Pose 消息由一个 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;

 标记的颜色被指定为 std_msgs/ColorRGBA。每个成员应介于 0 和 1 之间。alpha (a) 值为 0 表示完全透明（不可见），1 表示完全不透明。

    marker.lifetime = ros::Duration();

生命周期字段指定此标记在被自动删除之前应该保留多长时间。 ros::Duration() 的值意味着永远不会自动删除。

如果在达到生命周期之前收到新的标记消息，则生命周期将重置为新标记消息中的值。

    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.header.frame_id = "/my_frame";

睡眠并循环回到顶部。

六、 构建代码

您应该能够使用以下方式构建代码：

$ cd %TOP_DIR_YOUR_CATKIN_WORKSPACE%
$ catkin_make

七、运行代码

您应该能够运行代码：

rosrun using_markers basic_shapes

八、查看标记

现在您正在发布标记，您需要设置 rviz 以查看它们。首先，确保 rviz 已构建：

rosmake rviz

rosrun using_markers basic_shapes
rosrun rviz rviz

如果您以前从未使用过 rviz，请参阅用户指南以帮助您入门。

因为我们没有任何 tf 变换设置，所以要做的第一件事就是将 Fixed Frames 设置为我们在上面设置标记的帧 /my_frame。为此，请将固定帧字段设置为“/my_frame”。
 

接下来添加一个标记显示。请注意，指定的默认主题 visual_marker 与正在发布的主题相同。

您现在应该在原点看到一个标记，该标记每秒都会改变形状：

九、错误和改出

[ WARN] [1651908826.073714980]: Invalid argument passed to canTransform  argument source_frame in tf2 frame_ids cannot start with a '/' like:

是因为有些版本tf2不支持/my_frame这个表达，需要将程序中

marker.header.frame_id = "/my_frame";

改为

marker.header.frame_id = "my_frame";