沉默杀手HomeArchivesTagsCategories
使用Marker在rviz上标记机器人的轨迹
|ROSROS Kinetic知识|
Word count: 1.2k|Reading time: 5 min

在rviz中标记机器人的轨迹需要用到类visualization_msgs::Marker，其重要成员如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
//各种标志物类型的定义，也是Marker的枚举值，所有的具体介绍和形状：http://wiki.ros.org/rviz/DisplayTypes/Marker
uint8 ARROW=0//箭头
uint8 CUBE=1//立方体
uint8 SPHERE=2//球
uint8 CYLINDER=3//圆柱体
uint8 LINE_STRIP=4//线条（点的连线）
uint8 LINE_LIST=5//线条序列
uint8 CUBE_LIST=6//立方体序列
uint8 SPHERE_LIST=7//球序列
uint8 POINTS=8//点集
uint8 TEXT_VIEW_FACING=9//显示3D的文字
uint8 MESH_RESOURCE=10//网格？
uint8 TRIANGLE_LIST=11//三角形序列


//对标记的操作，也是枚举值
uint8 ADD=0
uint8 MODIFY=0
uint8 DELETE=2
uint8 DELETEALL=3

// 重要成员变量
Header header
string ns   //命名空间namespace
int32 id    //与命名空间联合起来，形成唯一的id，这个唯一的id可以将各个标志物区分开来，使得程序可以对指定的标志物进行操作，如果再次执行相同id，会删除上一次的标记
int32 type  	//类型，指哪种形状
int32 action    //操作，是添加还是修改还是删除
geometry_msgs/Pose pose       # 位姿
geometry_msgs/Vector3 scale   # 默认1，指1米大小。一般要小于1
std_msgs/ColorRGBA color      # Color [0.0-1.0]，其实是普通颜色值除以255
duration lifetime             # 在自动删除前维持多久，0表示永久。注意类型为duration，所以要用ros::Duration(1)初始化
bool frame_locked             # If this marker should be frame-locked, i.e. retransformed into its frame every timestep

geometry_msgs/Point[] points//这个是在序列、点集中才会用到，指明序列中每个点的位置

# 数量必须是0或与点数相同，alpha还不能用
std_msgs/ColorRGBA[] colors

#只用于文本标识
string text

string mesh_resource
bool mesh_use_embedded_materials

ns和id变量每次不能同时一样，否则后面的操作会覆盖前面的操作，也就一直只能看到最新的了。建议每次让id+=1或者从命令行输入，

对Marker的pose和scale必须赋值， 否则发布会报警，不能正常显示。

1
2
3
4
5
6
7
8
9
10
11
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;
// 缺一不可，不能有0
marker.scale.x = 0.3;
marker.scale.y = 0.3;
marker.scale.z = 0.3;

如果报警Non-empty points array is ignored.可以忽略

代码

我写了代码如下，在rviz上标记机器人当前位置，形状为球体，id从命令行输入，id为0时清除所有标识

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <geometry_msgs/Pose.h>

int main( int argc, char** argv )
{
    ros::init(argc, argv, "basic_shapes");
    ros::NodeHandle nh;
    if(argc !=2)
    {
        ROS_ERROR("no marker id !");
        return -1;
    }

    visualization_msgs::Marker marker;

    ros::Publisher marker_pub = nh.advertise<visualization_msgs::Marker>("visualization_marker", 1);

    uint32_t shape = visualization_msgs::Marker::SPHERE;

    // 获取当前位姿，阻塞至获取成功
    boost::shared_ptr<geometry_msgs::Pose const> edge = ros::topic::waitForMessage<geometry_msgs::Pose>("robot_pose",nh);

    if( atoi(argv[1])==0 )
    {
        marker.action = visualization_msgs::Marker::DELETEALL;
        ROS_WARN("All marker in rviz will be removed !");
    }
    else
        marker.action = visualization_msgs::Marker::ADD;

    // Set the frame ID and timestamp.  See the TF tutorials for information on these.
    marker.header.frame_id = "/map";
    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 = atoi(argv[1]);

    marker.type = shape;

    // 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 = edge->position.x;
    marker.pose.position.y = edge->position.y;
    marker.pose.position.z = edge->position.z;
    marker.pose.orientation.x = edge->orientation.x;
    marker.pose.orientation.y = edge->orientation.y;
    marker.pose.orientation.z = edge->orientation.z;
    marker.pose.orientation.w = edge->orientation.w;

    // Set the scale of the marker -- 1x1x1 here means 1m on a side
    marker.scale.x = 0.1;
    marker.scale.y = 0.1;
    marker.scale.z = 0.1;

    // Set the color -- set alpha to something non-zero!
    marker.color.r = 0.0f;
    marker.color.g = 0.0f;
    marker.color.b = 1.0f;
    // 不要是0，否则报警 Marker is fully transparent (color.a is 0.0).
    marker.color.a = 1.0;
    // 不加参数就是0，标识生命期为永久
    marker.lifetime = ros::Duration();

    // 如果rviz里没有订阅话题visualization_marker，等待2秒
    while (marker_pub.getNumSubscribers() < 1)
    {
        if (!ros::ok())
        {
            return 0;
        }
        ROS_WARN_ONCE("Please create a subscriber to the marker");
        sleep(2);
    }
    // 这样比较好，有了订阅者再发布
    marker_pub.publish(marker);
    return 0;
}

打开rviz，后添加Marker，设置话题:

机器人在导航状态，所以Fixed Frame是/map

在终端运行rosrun add_marker marker 1，会添加标识成功

运行rosrun map_server map_saver不会保存标识，因为它和地图无关，只是rviz里的显示，否则就成了地图上的障碍物了。

因为发布了Marker消息，在rviz里订阅后观测到，所以重启rviz后就没有标识了

参考：
官方marker教程
ROS之rviz显示历史运动轨迹、路径的各种方法
github: rviz_visual_tools