【可视化】娱乐一下,rviz上画个3D框

        搞点云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