rviz_marker_绘制直线_矩形框

ROS RVIZ Marker可视化 直线 矩形框

前言：因为最近在研究车道线的拟合，为了直观进行数据的可视化分析，故记录一下 ROS RVIZ中使用marker进行可视化直线和矩形框

可视化直线：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-eixKldkv-1671521761047)(https://kaho-pic-1307106074.cos.ap-guangzhou.myqcloud.com/CSDN_Pictures/%E6%B7%B1%E8%93%9D%E5%A4%9A%E4%BC%A0%E6%84%9F%E5%99%A8%E8%9E%8D%E5%90%88%E5%AE%9A%E4%BD%8D/%E7%AC%AC%E4%BA%8C%E7%AB%A0%E6%BF%80%E5%85%89%E9%87%8C%E7%A8%8B%E8%AE%A11image-20221220152051784.png)]

可视化矩形框：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-7jRNsgFR-1671521761048)(https://kaho-pic-1307106074.cos.ap-guangzhou.myqcloud.com/CSDN_Pictures/%E6%B7%B1%E8%93%9D%E5%A4%9A%E4%BC%A0%E6%84%9F%E5%99%A8%E8%9E%8D%E5%90%88%E5%AE%9A%E4%BD%8D/%E7%AC%AC%E4%BA%8C%E7%AB%A0%E6%BF%80%E5%85%89%E9%87%8C%E7%A8%8B%E8%AE%A11image-20221220152653345.png)]

注意：

#使用ros rviz marker需要注意的事项
1.line.lifetime  为marker的维持时间， ros::Duration();   永远不会被删除
2.需要指定 line.header.frame_id
3.颜色使用，本问增加了colorMap，以支持不同直线显示不同颜色
4.line.id 不能重复，重复Rviz会出现冲突
5.一个line能连接相近的两个点，在绘制矩形时，可先预想几个点的相连
参考链接： https://blog.csdn.net/Fang_cheng_/article/details/116454690

完整代码：

FILE: marker_visual.cpp

#include <iostream>
#include <vector>
#include <string>
#include<iomanip>//必要头文件

#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/PointCloud2.h>

#include <Eigen/Core>
#include <Eigen/Geometry>


using namespace std ;

int makerIdCount  = 0 ;
visualization_msgs::MarkerArray    lane ;
visualization_msgs::MarkerArray    box ;
ros::Publisher marker_pub_lane  ;
ros::Publisher marker_pub_box ;

vector<vector<int>> colorMap = vector<vector<int>> (11, vector<int>(3)) = {
        {160,82,45}, {255,140,0},{0,255,0}, {220,20,60},
        {245,255,250}, {0,0,255},{255,0,255},{255,20,147},{64,224,208} , {255,255,0},{255,0,0 },
};

void visualLaneLine(visualization_msgs::MarkerArray  &lineMakerArray, vector<Eigen::Vector3d> pointsVec,  int id, bool colorUsed, double timeDelay){  
        visualization_msgs::Marker line ;
        line.lifetime = ros::Duration(timeDelay,0);               //  ros::Duration();   永远不会被删除
        line.type = visualization_msgs::Marker::LINE_STRIP;             //  形状为线
        line.action = visualization_msgs::Marker::ADD;
        line.header.frame_id =  "camera_init" ;
        line.ns = "lane";
        line.scale.x = 0.80;

        if(colorUsed){          //  true 使用彩色 ，false 单调色
            line.color.r =   colorMap[id%10][0];            //  10种颜色选择
            line.color.g =  colorMap[id%10][1]; 
            line.color.b =  colorMap[id%10][2]; 
        }else{
            line.color.r = 160 ;  
            line.color.g  = 82 ;
            line.color.b  = 45 ;    // 红色
        }
        line.color.a =  1.0f ;
        line.pose.orientation.w = 1;
        line.id = id ;             //  线不同line ID号不能重复

        for (int i = 0; i < pointsVec.size() ; i ++)
        {
            geometry_msgs::Point p;
            p.x =  pointsVec[i].x() ;
            p.y =  pointsVec[i].y() ;
            p.z =  pointsVec[i].z() ;
            line.points.push_back(p);
        }
        lineMakerArray.markers.push_back(line);
        makerIdCount ++ ;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "marker visual");
	ros::NodeHandle n;

    marker_pub_lane = n.advertise<visualization_msgs::MarkerArray>("visualization_marker_lane_vec", 10);
    marker_pub_box = n.advertise<visualization_msgs::MarkerArray>("visualization_marker_box", 10);

    //  生成点  max :(10  20  10)  min(-10 -20 -10)
    Eigen::Vector3d max(10, 20, 10);
    Eigen::Vector3d min(-10,-20,-10);
    Eigen::Vector3d p0_b(min.x(), max.y(), max.z());
    Eigen::Vector3d p1_b(min.x(), min.y(), max.z());
    Eigen::Vector3d p2_b(min.x(), min.y(), min.z());
    Eigen::Vector3d p3_b(min.x(), max.y(), min.z());
    Eigen::Vector3d p4_b(max.x(), max.y(), min.z());
    Eigen::Vector3d p5_b(max.x(), min.y(), min.z());
    Eigen::Vector3d p6_b(max.x(), min.y(), max.z());
    Eigen::Vector3d p7_b(max.x(), max.y(), max.z());

    // 直线点
    vector<vector<Eigen::Vector3d>> linesVec(3) ;       //存储三条线
    linesVec[0].push_back(p0_b);
    linesVec[0].push_back(p1_b);
    linesVec[1].push_back(p4_b);
    linesVec[1].push_back(p5_b);
    linesVec[2].push_back(p6_b);
    linesVec[2].push_back(p7_b);

    //  矩形点
    vector<Eigen::Vector3d>  boxPointsVec ;
    boxPointsVec.push_back(p0_b);boxPointsVec.push_back(p3_b);boxPointsVec.push_back(p2_b);
    boxPointsVec.push_back(p5_b);boxPointsVec.push_back(p6_b);boxPointsVec.push_back(p1_b);  
    boxPointsVec.push_back(p0_b);boxPointsVec.push_back(p7_b);boxPointsVec.push_back(p4_b);
    boxPointsVec.push_back(p3_b);boxPointsVec.push_back(p4_b);boxPointsVec.push_back(p5_b);
    boxPointsVec.push_back(p6_b);boxPointsVec.push_back(p7_b);           

    ros::Rate loop_rate(10);

    //  可视化直线 
    // for(int i = 0; i < linesVec.size(); i++ ){
    //         visualLaneLine(lane, linesVec[i], makerIdCount, true, 1000); 
    // }
    //  可视化box
    visualLaneLine(box,boxPointsVec, makerIdCount, false, 1000); 

    while(ros::ok()){

    marker_pub_lane.publish(lane);
    marker_pub_box.publish(box);

    loop_rate.sleep();
    }
    return 0 ;
}

FILE: marker_visual.launch

<?xml version="1.0" encoding="UTF-8"?>
<launch>
    <node pkg="marker_visual" name="marker_visual" type="marker_visual" output="screen"></node>
</launch>

Rviz参考：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-jhMptzb9-1671521761049)(https://kaho-pic-1307106074.cos.ap-guangzhou.myqcloud.com/CSDN_Pictures/%E6%B7%B1%E8%93%9D%E5%A4%9A%E4%BC%A0%E6%84%9F%E5%99%A8%E8%9E%8D%E5%90%88%E5%AE%9A%E4%BD%8D/%E7%AC%AC%E4%BA%8C%E7%AB%A0%E6%BF%80%E5%85%89%E9%87%8C%E7%A8%8B%E8%AE%A11image-20221220153339235.png)]

参考博客：

Rviz可视化工具Marker示例：目标检测画矩形框

​ edited by kaho 2022.12.20