前言
最近,在复现一篇激光雷达检测障碍物的文献,在将雷达数据进行聚类后,需要观察聚类的效果,这个时候需要用Rviz标记,绘制标记框,把学习过程记录一下,方便后续查阅.
1.visualization_msgs/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可以将各个标志物区分开来,使得程序可以对指定的标志物进行操作
int32 type //类型
int32 action //操作,是添加还是修改还是删除
geometry_msgs/Pose pose # Pose of the object
geometry_msgs/Vector3 scale # Scale of the object 1,1,1 means default (usually 1 meter square)
std_msgs/ColorRGBA color # Color [0.0-1.0]
duration lifetime # How long the object should last before being automatically deleted. 0 means forever
bool frame_locked # If this marker should be frame-locked, i.e. retransformed into its frame every timestep
#Only used if the type specified has some use for them (eg. POINTS, LINE_STRIP, ...)
geometry_msgs/Point[] points//这个是在序列、点集中才会用到,指明序列中每个点的位置
#Only used if the type specified has some use for them (eg. POINTS, LINE_STRIP, ...)
#number of colors must either be 0 or equal to the number of points
#NOTE: alpha is not yet used
std_msgs/ColorRGBA[] colors
# NOTE: only used for text markers
string text
# NOTE: only used for MESH_RESOURCE markers
string mesh_resource
bool mesh_use_embedded_materials
ROS之rviz显示历史运动轨迹、路径的各种方法(visualization_msgs/Marker、nav_msgs/Path- 白夜行的狼
2.实践
看了几个代码工程把它直接复制到雷达检测中显示OK,但没有标记区域,看了这篇文章才发现是Fixed Frame的问题,原来需要手动把map,改写成my_frame. 所以在复制到自己工程时在代码中直接将my_frame改成自己相应的坐标参考即可,我将其改为odom.
rviz学习系列—Markers:发送基本形状(c++)
#include "ros/ros.h"
#include <iostream>
#include <fstream>
#include <sensor_msgs/LaserScan.h>
#include <visualization_msgs/Marker.h>
#define pi 3.1415926535898
#define resolution 2*pi/360
void laserCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
std::vector<float> ranges=msg->ranges;
std::vector<float> angel(ranges.size());
int i = 0;
for(i = 0; i < 360; i++){
if(i <180){
angel[i]= -pi + i*resolution;
}
else{
angel[i] = (i-180)*resolution;
}
}
float r = 0.1;
int temp_n=0;
int flag=0;
std::vector<std::vector<int> > temp;
std::vector<int> temp_cluster;
for(i = 0; i < 359; i++){
if( pow( ranges[i+1] * cos(angel[i+1]) - ranges[i] * cos(angel[i]) , 2 ) + pow( ranges[i+1] * sin(angel[i+1]) - ranges[i] * sin(angel[i]), 2 ) <= pow(r,2) ) {
if(flag == 0){
flag = 1;
}
temp_cluster.push_back(i);
}
else{
if(flag == 1){
temp.push_back(temp_cluster);
temp_cluster.clear();
temp_n++;
}
flag = 0;
}
}
std::cout<<"\n"<<temp.size()<<"\n";
for(i=0;i<temp.size();i++){
for(int j=0;j<temp[i].size();j++){
std::cout<<temp[i][j]<<" ";
}
std::cout<<"\n";
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "perception");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("/scan", 100, laserCallback);
ros::Rate r(1);
ros::Publisher marker_pub = nh.advertise<visualization_msgs::Marker>("visualization_marker",1);
uint32_t shape = visualization_msgs::Marker::CUBE;
while (ros::ok())
{
visualization_msgs::Marker marker;
// Set the frame ID and timestamp. See the TF tutorials for information on these.
marker.header.frame_id = "/odom";
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 = 0;
// Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
marker.type = shape;
// Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
marker.action = visualization_msgs::Marker::ADD;
// 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 = 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;
// Set the scale of the marker -- 1x1x1 here means 1m on a side
marker.scale.x = 1.0;
marker.scale.y = 1.0;
marker.scale.z = 1.0;
// Set the color -- be sure to set alpha to something non-zero!
marker.color.r = 0.0f;
marker.color.g = 1.0f;
marker.color.b = 0.0f;
marker.color.a = 1.0;
marker.lifetime = ros::Duration();
// Publish the marker
while (marker_pub.getNumSubscribers() < 1)
{
if (!ros::ok())
{
return 0;
}
ROS_WARN_ONCE("Please create a subscriber to the marker");
sleep(1);
}
marker_pub.publish(marker);
// Cycle between different shapes
switch (shape)
{
case visualization_msgs::Marker::CUBE:
shape = visualization_msgs::Marker::SPHERE;
break;
case visualization_msgs::Marker::SPHERE:
shape = visualization_msgs::Marker::ARROW;
break;
case visualization_msgs::Marker::ARROW:
shape = visualization_msgs::Marker::CYLINDER;
break;
case visualization_msgs::Marker::CYLINDER:
shape = visualization_msgs::Marker::CUBE;
break;
}
r.sleep();
}
ros::spin();
return 0;
}
3.将聚类信息添加RVIZ标记区域
#include "ros/ros.h"
#include <iostream>
#include <fstream>
#include <sensor_msgs/LaserScan.h>
#include <visualization_msgs/Marker.h>
#define pi 3.1415926535898
#define resolution 2*pi/360
void laserCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
std::vector<float> ranges=msg->ranges;
std::vector<float> angel(ranges.size());
int i = 0;
for(i = 0; i < 360; i++){
if(i <180){
angel[i]= -pi + i*resolution;
}
else{
angel[i] = (i-180)*resolution;
}
}
float r = 0.1;
int temp_n=0;
int flag=0;
std::vector<std::vector<int> > temp;
std::vector<int> temp_cluster;
for(i = 0; i < 359; i++){
if( pow( ranges[i+1] * cos(angel[i+1]) - ranges[i] * cos(angel[i]) , 2 ) + pow( ranges[i+1] * sin(angel[i+1]) - ranges[i] * sin(angel[i]), 2 ) <= pow(r,2) ) {
if(flag == 0){
flag = 1;
}
temp_cluster.push_back(i);
}
else{
if(flag == 1){
temp.push_back(temp_cluster);
temp_cluster.clear();
temp_n++;
}
flag = 0;
}
}
std::cout<<"\n"<<temp.size()<<"\n";
for(i=0;i<temp.size();i++){
for(int j=0;j<temp[i].size();j++){
std::cout<<temp[i][j]<<" ";
}
std::cout<<"\n";
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "perception");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("/scan", 100, laserCallback);
ros::Rate r(1);
ros::Publisher marker_pub = nh.advertise<visualization_msgs::Marker>("visualization_marker",1);
uint32_t shape = visualization_msgs::Marker::LINE_STRIP;
while (ros::ok())
{
visualization_msgs::Marker marker;
visualization_msgs::Marker marker2;
// Set the frame ID and timestamp. See the TF tutorials for information on these.
marker.header.frame_id = "/odom";
marker.header.stamp = ros::Time::now();
marker2.header.frame_id = "/odom";
marker2.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 = 0;
marker2.ns = "basic_shapes";
marker2.id = 1;
// Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
marker.type = shape;
marker2.type = shape;
// Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
marker.action = visualization_msgs::Marker::ADD;
marker2.action = visualization_msgs::Marker::ADD;
// 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 = 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;
marker2.pose.position.x = -1;
marker2.pose.position.y = -1;
marker2.pose.position.z = 0;
marker2.pose.orientation.x = 0.0;
marker2.pose.orientation.y = 0.0;
marker2.pose.orientation.z = 0.0;
marker2.pose.orientation.w = 1.0;
// Set the scale of the marker -- 1x1x1 here means 1m on a side
marker.scale.x = 0.01;
marker.scale.y = 0.01;
marker.scale.z = 0.0;
marker2.scale.x = 0.01;
marker2.scale.y = 0.01;
marker2.scale.z = 0.0;
// Set the color -- be sure to set alpha to something non-zero!
marker.color.r = 0.0f;
marker.color.g = 1.0f;
marker.color.b = 0.0f;
marker.color.a = 1.0;
marker2.color.r = 0.0f;
marker2.color.g = 0.0f;
marker2.color.b = 1.0f;
marker2.color.a = 1.0;
marker2.lifetime = ros::Duration();
geometry_msgs:: Point p[5];
p[0].x = 0; p[0].y = 0;
p[1].x = 1; p[1].y = 0;
p[2].x = 1; p[2].y = 1;
p[3].x = 0; p[3].y = 1;
p[4].x = 0; p[4].y = 0;
for (int i = 0; i < 5; i++){
marker.points.push_back(p[i]);
marker2.points.push_back(p[i]);
}
marker_pub.publish(marker);
marker_pub.publish(marker2);
r.sleep();
}
ros::spin();
return 0;
}
出来两个框了,差不多了,下一步就是对每一个簇中的点求一个矩形框,然后发布出来,未完待续…
4.最小外接矩形
最小外接矩形有点难搞哎!先安排一下矩形吧.
#include "ros/ros.h"
#include <iostream>
#include <fstream>
#include <sensor_msgs/LaserScan.h>
#include <visualization_msgs/Marker.h>
#define pi 3.1415926535898
#define resolution 2*pi/360
ros::Publisher marker_pub;
void laserCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
//______________________对雷达数据粗略聚类___________________________
std::vector<float> ranges=msg->ranges;
std::vector<float> angel(ranges.size());
int i = 0;
for(i = 0; i < 360; i++){
if(i <180){
angel[i]= -pi + i*resolution;
}
else{
angel[i] = (i-180)*resolution;
}
}
std::vector<std::vector<int> > temp;
std::vector<int> temp_cluster;
float r = 0.1;
int temp_n=0;
int flag=0;
for(i = 0; i < 359; i++){
if( pow( ranges[i+1] * cos(angel[i+1]) - ranges[i] * cos(angel[i]) , 2 ) + pow( ranges[i+1] * sin(angel[i+1]) - ranges[i] * sin(angel[i]), 2 ) <= pow(r,2) ) {
if(flag == 0){
flag = 1;
}
temp_cluster.push_back(i);
}
else{
if(flag == 1){
temp.push_back(temp_cluster);
temp_cluster.clear();
temp_n++;
}
flag = 0;
}
}
//_________________________输出聚类结果_________________________________
std::cout<<"___________________________"<<temp.size()<<"_____________________________"<<"\n";
for(i=0;i<temp.size();i++){
for(int j=0;j<temp[i].size();j++){
std::cout<<temp[i][j]<<" ";
}
std::cout<<"\n";
}
//__________________________画矩形框____________________________________
std::vector<std::vector<float> > square_all;
std::vector<float> square;
float x_max = 0;
float x_min = 0;
float y_max = 0;
float y_min = 0;
int nn=0;
for(i=0;i<temp.size();i++){
if(temp[i].size()<=1){
continue;
}
else{
for(int j=0;j<temp[i].size();j++){
if(j==0){
x_max = ranges[temp[i][j]]*cos(angel[temp[i][j]]);
x_min = ranges[temp[i][j]]*cos(angel[temp[i][j]]);
y_max = ranges[temp[i][j]]*sin(angel[temp[i][j]]);
y_min = ranges[temp[i][j]]*sin(angel[temp[i][j]]);
}
if(ranges[temp[i][j]]*cos(angel[temp[i][j]])>x_max){
x_max = ranges[temp[i][j]]*cos(angel[temp[i][j]]);
}
else if(ranges[temp[i][j]]*cos(angel[temp[i][j]])<x_min){
x_min = ranges[temp[i][j]]*cos(angel[temp[i][j]]);
}
if(ranges[temp[i][j]]*sin(angel[temp[i][j]])>y_max){
y_max = ranges[temp[i][j]]*sin(angel[temp[i][j]]);
}
else if(ranges[temp[i][j]]*sin(angel[temp[i][j]])<y_min){
y_min = ranges[temp[i][j]]*sin(angel[temp[i][j]]);
}
}
square.push_back(x_max);
square.push_back(x_min);
square.push_back(y_max);
square.push_back(y_min);
square_all.push_back(square);
square.clear();
}
}
//_________________________输出矩形框结果_________________________________
std::cout<<"*************************"<<square_all.size()<<"*****************************"<<"\n";
for(i=0;i<square_all.size();i++){
for(int j=0;j<square_all[i].size();j++){
std::cout<<square_all[i][j]<<" ";
}
std::cout<<"\n";
}
//__________________________RVIZ显示___________________________________
uint32_t shape = visualization_msgs::Marker::LINE_STRIP;
visualization_msgs::Marker marker;
visualization_msgs::Marker marker2;
// Set the frame ID and timestamp. See the TF tutorials for information on these.
marker.header.frame_id = "/laser_link";
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 = 0;
// Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
marker.type = shape;
// Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
marker.action = visualization_msgs::Marker::ADD;
// 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 = square_all[0][1];
marker.pose.position.y = square_all[0][3];
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 = 0.0;
// Set the scale of the marker -- 1x1x1 here means 1m on a side
marker.scale.x = 0.01;
marker.scale.y = 0.01;
marker.scale.z = 0.0;
// Set the color -- be sure to set alpha to something non-zero!
marker.color.r = 0.0f;
marker.color.g = 1.0f;
marker.color.b = 0.0f;
marker.color.a = 1.0;
geometry_msgs:: Point p[5];
p[0].x = 0; p[0].y = 0;
p[1].x = square_all[0][0]-square_all[0][1]; p[1].y = 0;
p[2].x = square_all[0][0]-square_all[0][1]; p[2].y = square_all[0][2]-square_all[0][3];
p[3].x = 0; p[3].y = square_all[0][2]-square_all[0][3];
p[4].x = 0; p[4].y = 0;
for (int i = 0; i < 5; i++){
marker.points.push_back(p[i]);
}
marker_pub.publish(marker);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "perception");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("/scan", 100, laserCallback);
marker_pub = nh.advertise<visualization_msgs::Marker>("visualization_marker",1);
ros::spin();
return 0;
}
效果:链接
接下来把剩余其它几个类,加入标记,然后优化,
- 二维雷达数据聚类算法
- 加入最小外接矩形框
- 感觉C++算法还是很注重效率的,可以学习C++的内存管理和指针释放,减少代码运行内存,以及考虑算法的时间复杂度,减少代码运行的时间.
未完待续…
V2版本
#include "ros/ros.h"
#include <iostream>
#include <fstream>
#include <sensor_msgs/LaserScan.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#define pi 3.1415926535898
#define resolution 2*pi/360
ros::Publisher marker_pub;
void laserCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
//______________________对雷达数据粗略聚类___________________________
std::vector<float> ranges=msg->ranges;
std::vector<float> angel(ranges.size());
int i = 0;
for(i = 0; i < 360; i++){
if(i <180){
angel[i]= -pi + i*resolution;
}
else{
angel[i] = (i-180)*resolution;
}
}
std::vector<std::vector<int> > temp;
std::vector<int> temp_cluster;
float r = 0.1;
int temp_n=0;
int flag=0;
for(i = 0; i < 359; i++){
if( pow( ranges[i+1] * cos(angel[i+1]) - ranges[i] * cos(angel[i]) , 2 ) + pow( ranges[i+1] * sin(angel[i+1]) - ranges[i] * sin(angel[i]), 2 ) <= pow(r,2) ) {
if(flag == 0){
flag = 1;
}
temp_cluster.push_back(i);
}
else{
if(flag == 1){
temp.push_back(temp_cluster);
temp_cluster.clear();
temp_n++;
}
flag = 0;
}
}
//_________________________输出聚类结果_________________________________
std::cout<<"___________________________"<<temp.size()<<"_____________________________"<<"\n";
for(i=0;i<temp.size();i++){
for(int j=0;j<temp[i].size();j++){
std::cout<<temp[i][j]<<" ";
}
std::cout<<"\n";
}
//__________________________画矩形框____________________________________
std::vector<std::vector<float> > square_all;
std::vector<float> square;
float x_max = 0;
float x_min = 0;
float y_max = 0;
float y_min = 0;
int nn=0;
for(i=0;i<temp.size();i++){
if(temp[i].size()<=1){
continue;
}
else{
for(int j=0;j<temp[i].size();j++){
if(j==0){
x_max = ranges[temp[i][j]]*cos(angel[temp[i][j]]);
x_min = ranges[temp[i][j]]*cos(angel[temp[i][j]]);
y_max = ranges[temp[i][j]]*sin(angel[temp[i][j]]);
y_min = ranges[temp[i][j]]*sin(angel[temp[i][j]]);
}
if(ranges[temp[i][j]]*cos(angel[temp[i][j]])>x_max){
x_max = ranges[temp[i][j]]*cos(angel[temp[i][j]]);
}
else if(ranges[temp[i][j]]*cos(angel[temp[i][j]])<x_min){
x_min = ranges[temp[i][j]]*cos(angel[temp[i][j]]);
}
if(ranges[temp[i][j]]*sin(angel[temp[i][j]])>y_max){
y_max = ranges[temp[i][j]]*sin(angel[temp[i][j]]);
}
else if(ranges[temp[i][j]]*sin(angel[temp[i][j]])<y_min){
y_min = ranges[temp[i][j]]*sin(angel[temp[i][j]]);
}
}
square.push_back(x_max);
square.push_back(x_min);
square.push_back(y_max);
square.push_back(y_min);
square_all.push_back(square);
square.clear();
}
}
//_________________________输出矩形框结果_________________________________
std::cout<<"*************************"<<square_all.size()<<"*****************************"<<"\n";
for(i=0;i<square_all.size();i++){
for(int j=0;j<square_all[i].size();j++){
std::cout<<square_all[i][j]<<" ";
}
std::cout<<"\n";
}
//__________________________RVIZ显示___________________________________
uint32_t shape = visualization_msgs::Marker::CUBE_LIST;
visualization_msgs::MarkerArray marker1;
visualization_msgs::Marker marker;
// Set the frame ID and timestamp. See the TF tutorials for information on these.
marker.header.frame_id = "/laser_link";
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 = 0;
// Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
marker.type = shape;
// Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
marker.action = visualization_msgs::Marker::ADD;
// 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 = square_all[0][1];
marker.pose.position.y = square_all[0][3];
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;
// Set the scale of the marker -- 1x1x1 here means 1m on a side
marker.scale.x = 0.05;
marker.scale.y = 0.05;
marker.scale.z = 0.05;
// Set the color -- be sure to set alpha to something non-zero!
marker.color.r = 0.0f;
marker.color.g = 1.0f;
marker.color.b = 0.0f;
marker.color.a = 1.0;
for(i=0; i<square_all.size(); i++){
geometry_msgs::Point p[8];
p[0].x = 0; p[0].y = 0; p[0].z = 0.0;
p[1].x = square_all[i][0]-square_all[i][1]; p[1].y = 0; p[1].z = 0.0;
p[2].x = square_all[i][0]-square_all[i][1]; p[2].y = square_all[i][2]-square_all[i][3]; p[2].z = 0.0;
p[3].x = 0; p[3].y = square_all[i][2]-square_all[i][3]; p[3].z = 0.0;
p[4].x = 0; p[4].y = 0; p[4].z = 0.01;
p[5].x = square_all[i][0]-square_all[i][1]; p[5].y = 0; p[5].z = 0.01;
p[6].x = square_all[i][0]-square_all[i][1]; p[6].y = square_all[i][2]-square_all[i][3]; p[6].z = 0.01;
p[7].x = 0; p[7].y = square_all[i][2]-square_all[i][3]; p[7].z = 0.01;
marker.points.clear();
for (int j = 0; j < 8; j++){
marker.points.push_back(p[j]);
}
marker.id = i;
marker.pose.position.x = square_all[i][1];
marker.pose.position.y = square_all[i][3];
marker.pose.position.z = 0;
marker1.markers.push_back(marker);
marker.points.clear();
}
marker_pub.publish(marker1);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "perception");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("/scan", 100, laserCallback);
marker_pub = nh.advertise<visualization_msgs::MarkerArray>("visualization_marker",1);
ros::spin();
return 0;
}
效果链接
啊,居然这么多立方体,咋变成一个啊…
V3
视频效果链接
可以了,得到想要的效果了.
RVIZ 雷达数据聚类
Tips
消除拖影:V3版本一些拖影,效果不理想,于是发现Marker自带生命周期属性lifetime,看函数名字就知道大致含义.
marker.lifetime = ros::Duration(0.5);