ROS RVIZ Marker可视化 直线 矩形框
前言:因为最近在研究车道线的拟合,为了直观进行数据的可视化分析,故记录一下 ROS RVIZ中使用marker进行可视化直线和矩形框
可视化直线:
可视化矩形框:
注意:
#使用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参考:
参考博客:
edited by kaho 2022.12.20