Rviz教程（二）：Markers: Sending Basic Shapes (C++)

Markers: Sending Basic Shapes (C++)

Description:  Shows how to use  visualization_msgs/Marker  messages to send basic shapes (cube,  sphere (范围) ,  cylinder (圆筒) , arrow) to rviz.

Tutorial Level:  BEGINNER

Next Tutorial:   Markers: Points and Lines  

目录

1. Intro
2. Create a Package
3. Sending Markers
   1. The Code
   2. The Code Explained
   3. Building the Code
   4. Running the Code
4. Viewing the Markers
5. More Information

electric fuerte groovy hydro indigo jade kinetic lunar

catkin rosbuild

Intro

Unlike other displays, the Marker Display lets you visualize(形象) data in rviz without rviz knowing anything about interpreting(说明) that data. Instead, primitive(原始的) objects are sent to the display through visualization_msgs/Markermessages, which let you show things like arrows, boxes, spheres(范围) and lines.

This tutorial(辅导的) will show you how to send the four basic shapes (boxes, spheres(范围), cylinders(圆筒), and arrows). We'll create a program that sends out a new marker every second, replacing the last one with a different shape.

Create a Package

Before getting started, let's create a package called using_markers, somewhere in your package path:

catkin_create_pkg using_markers roscpp visualization_msgs

Sending Markers

The Code

Paste(张贴) the following into src/basic_shapes.cpp:

https://raw.github.com/ros-visualization/visualization(形象化)_tutorials/indigo-devel/visualization_marker_tutorials/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();
  }
}

Now edit the CMakeLists.txt file in your using_markers package, and add:

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

to the bottom.

The Code Explained

Ok, let's break down the code piece by piece:

切换行号显示

  30 #include <ros/ros.h>
  31 #include <visualization_msgs/Marker.h>
  32 

You should have seen the ROS include by now. We also include the visualization_msgs/Marker message definition.

切换行号显示

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

This should look familiar. We initialize(初始化) ROS, and create a ros::Publisher on the visualization_marker topic.

切换行号显示

  41   uint32_t shape = visualization_msgs::Marker::CUBE;

Here we create an integer(整数) to keep track of what shape we're going to publish. The four types we'll be using here all use the visualization_msgs/Marker message in the same way, so we can simply switch(转换) out the shape type to demonstrate(证明) the four different shapes.

切换行号显示

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

This begins the meat of the program. First we create a visualization_msgs/Marker, and begin filling it out. The header here is a roslib/Header, which should be familiar if you've done the tf tutorials. We set the frame_id member to /my_frame as an example. In a running system this should be the frame(有木架的) relative to which you want the marker's pose(造成) to be interpreted(说明).

切换行号显示

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

The namespace (ns) and id are used to create a unique(独特的) name for this marker. If a marker message is received with the same ns and id, the new marker will replace the old one.

切换行号显示

  56     marker.type = shape;

This type field is what specifies(指定) the kind of marker we're sending. The available types are enumerated(枚举) in the visualization_msgs/Marker message. Here we set the type to our shape variable(变量), which will change every time through the loop(环).

切换行号显示

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

The action field is what specifies what to do with the marker. The options are visualization(形象化)_msgs::Marker::ADD and visualization_msgs::Marker::DELETE. ADD is something of a misnomer(用词不当), it really means "create or modify(修改)".

New in Indigo A new action has been added to delete all markers in the particular Rviz display, regardless(不管) of ID or namespace. The value is 3 and in future ROS version the message will change to have value visualization(形象化)_msgs::Marker::DELETEALL.

切换行号显示

  62     marker.pose.position.x = 0;
  63     marker.pose.position.y = 0;
  64     marker.pose.position.z = 0;
  65     marker.pose.orientation.x = 0.0;
  66     marker.pose.orientation.y = 0.0;
  67     marker.pose.orientation.z = 0.0;
  68     marker.pose.orientation.w = 1.0;

Here we set the pose(姿势) of the marker. The geometry_msgs/Pose message consists of a geometry_msgs/Vector3 to specify(指定) the position and a geometry_msgs/Quaternion to specify the orientation(方向). Here we set the position to the origin, and the orientation to the identity(身份) orientation (note the 1.0 for w).

切换行号显示

  71     marker.scale.x = 1.0;
  72     marker.scale.y = 1.0;
  73     marker.scale.z = 1.0;

Now we specify the scale(规模) of the marker. For the basic shapes, a scale of 1 in all directions means 1 meter on a side.

切换行号显示

  76     marker.color.r = 0.0f;
  77     marker.color.g = 1.0f;
  78     marker.color.b = 0.0f;
  79     marker.color.a = 1.0;

The color of the marker is specified(指定) as a std_msgs/ColorRGBA. Each member should be between 0 and 1. An alpha (a) value of 0 means completely transparent(透明的) (invisible(无形的)), and 1 is completely opaque(不透明的).

切换行号显示

  81     marker.lifetime = ros::Duration();

The lifetime field specifies how long this marker should stick around before being automatically(自动地) deleted. A value of ros::Duration() means never to auto-delete.

If a new marker message is received before the lifetime has been reached, the lifetime will be reset(重置) to the value in the new marker message.

切换行号显示

  84     while (marker_pub.getNumSubscribers() < 1)
  85     {
  86       if (!ros::ok())
  87       {
  88         return 0;
  89       }
  90       ROS_WARN_ONCE("Please create a subscriber to the marker");
  91       sleep(1);
  92     }
  93     marker_pub.publish(marker);

We wait for the marker to have a subscriber(订户) and we then publish the marker. Note that you can also use a latched publisher as an alternative(二中择一) to this code.

切换行号显示

  96     switch (shape)
  97     {
  98     case visualization_msgs::Marker::CUBE:
  99       shape = visualization_msgs::Marker::SPHERE;
 100       break;
 101     case visualization_msgs::Marker::SPHERE:
 102       shape = visualization_msgs::Marker::ARROW;
 103       break;
 104     case visualization_msgs::Marker::ARROW:
 105       shape = visualization_msgs::Marker::CYLINDER;
 106       break;
 107     case visualization_msgs::Marker::CYLINDER:
 108       shape = visualization_msgs::Marker::CUBE;
 109       break;
 110     }

This code lets us show all four shapes while just publishing the one marker message. Based on the current shape, we set what the next shape to publish will be.

切换行号显示

 112     r.sleep();
 113   }

Sleep and loop(打环) back to the top.

Building the Code

You should be able to build the code with:

$ cd %TOP_DIR_YOUR_CATKIN_WORKSPACE%
$ catkin_make

Running the Code

You should be able to run the code with:

rosrun using_markers basic_shapes

Viewing the Markers

Now that you're publishing markers, you need to set rviz up to view them. First, make sure rviz is built:

rosmake rviz

Now, run rviz:

rosrun using_markers basic_shapes
rosrun rviz rviz

If you've never used rviz before, please see the User's Guide to get you started.

The first thing to do, because we don't have any tf transforms(改变) setup(设置), is to set the Fixed Frames to the frame(框架) we set the marker to above, /my_frame. In order to do so, set the Fixed Frame(框架) field to "/my_frame".

Next add a Markers display. Notice that the default topic specified(指定), visualization_marker, is the same as the one being published.

You should now see a marker at the origin that changes shape every second: 

More Information

For more information on the different types of markers beyond the ones shown here, please see the Markers Display Page