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