RViz学习笔记(二) - Markers: 发送基本的形状

这一节将会学习怎么使用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与之对应,这样就可以用了。

推荐阅读更多精彩内容