搞点云3D目标检测少不了要将检测结果展示出来秀一把,这时候rviz就可以发挥重要作用了。借助于rviz的Marker,允许通过发送visualization_msgs/Marker或者visualization_msgs/MarkerArray消息以可编程的形式添加各种基本形状在rviz中进行3D展示。说到底,marker它就是一个用来标记的东西。如果我们给定marker一个position和orientation(位置和姿态),rviz就会在画图区域的指定位置生成一个指定方向的marker,这个marker的形状我们可以在程序中自由选择,可以是立方体,箭头等。如何给marker一个orientation和position那自然就是一方发布消息,由marker来接收消息,消息里面包含了marker的位置和姿态等信息。
这篇小文章就演示一下我们常用的一个功能,在rviz上画3D检测框。要求不高,我们也就先画一个孤零零的3D框。 Marker是通过接受消息进而在rviz上展示,这基本上就归纳了我们要做的事情:1).创建消息发布者;2).创建消息;3).发布与接收消息。
#include "ros/ros.h"
#include "visualization_msgs/Marker.h"
class MarkerPublisher{
public:
MarkerPublisher(ros::NodeHandle& nh){
pub_marker_ = nh.advertise<visualization_msgs::Marker>("visualization_marker", 10);//initialize marker publisher
set_marker_fixed_property();
};
void Publish(){
while (pub_marker_.getNumSubscribers() < 1) {
sleep(1);
}
marker_.header.stamp = ros::Time();
pub_marker_.publish(marker_);
};
void set_marker_fixed_property(){
/*decide from which view we can see the marker*/
marker_.header.frame_id = "livox_frame";
marker_.ns = "my_namespace";
marker_.id = 0;
//set marker type
marker_.type = visualization_msgs::Marker::CUBE;
//set marker position
marker_.pose.position.x = 0;
marker_.pose.position.y = 0;
marker_.pose.position.z = 0;
//set marker scale
marker_.scale.x = 2.0; //m
marker_.scale.y = 2.0;
marker_.scale.z = 2.0;
//decide the color of the marker
marker_.color.a = 1.0; // Don't forget to set the alpha!
marker_.color.r = 0.0;
marker_.color.g = 1.0;
marker_.color.b = 0.0;
//set marker action
marker_.action = visualization_msgs::Marker::ADD;
marker_.lifetime = ros::Duration(); //(sec,nsec),0 forever
};
private:
ros::Publisher pub_marker_;
visualization_msgs::Marker marker_;
};
int main(int argc, char **argv){
ros::init(argc, argv, "marker_worker");
ros::NodeHandle nh;
MarkerPublisher mp(nh);
//ros::Rate rate(50);
//while (ros::ok()) {
mp.Publish();
// rate.sleep();
//}
ros::spin();
return 0;
}
我们首先是创建了一个消息发布者,要发布的消息类型为visualization_msgs::Marker,消息topic为"visualization_marker"。
pub_marker_ = nh.advertise<visualization_msgs::Marker>("visualization_marker", 10);//initialize marker publisher
然后是填充消息各个字段的内容。
marker.header.frame_id = "livox_frame"
用于表示和此数据关联的帧,在坐标系变化中可以理解为数据所在的坐标系名称。
marker.ns = "my_namespace";
marker.id = 0;
id和命名空间关联起来,形成唯一的id,用于将各个标志物区分开来。我这里只显示一个标志物,id为0就可以了。
marker_.type = visualization_msgs::Marker::CUBE;
标志物的形状类型,可供选择的类型有很多,譬如:
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//三角形序列
//set marker position
marker_.pose.position.x = 0;
marker_.pose.position.y = 0;
marker_.pose.position.z = 0;
标志物在坐标系中的位置。
//set marker scale
marker_.scale.x = 2.0; //m
marker_.scale.y = 2.0;
marker_.scale.z = 2.0;
标志物在各个方向上的尺寸,这里以m为单位。
//decide the color of the marker
marker_.color.a = 1.0; // Don't forget to set the alpha!
marker_.color.r = 0.0;
marker_.color.g = 1.0;
marker_.color.b = 0.0;
设置颜色,但是要注意这里还有一个透明度(marker_.color.a)的设置。
//set marker action
marker_.action = visualization_msgs::Marker::ADD;
对标志物的动作,有一下几种:
uint8 ADD=0
uint8 MODIFY=0
uint8 DELETE=2
uint8 DELETEALL=3
marker_.lifetime = ros::Duration(); //(sec,nsec),0 forever
标志物显示持续的时间,roscpp
里有两种时间表示方法:时刻 (ros::Time)
和时长(ros::Duration)
。其中Duration可以是负数。
ros::Duration::Duration(uint32_t _sec, uint32_t _nsec)
ros::Duration::Duration(double t)
//_sec是秒,_nsec是纳秒
定义完了消息,我们就可以发布消息了。这里我仅仅发布了一次消息,让标志物永久显示。最后在rviz上看到的标志物就是下面这个样子的。
【参考文献】
https://www.jianshu.com/p/acbe1b8631dc
https://blog.csdn.net/zhanghm1995/article/details/84644984
https://blog.csdn.net/u013834525/article/details/80447931