ros - 在 RViz 中发送不同颜色的标记

Question

我正在尝试在 RViz 中发送不同颜色的标记，以便我可以看到我的程序是否正在相应地工作。我现在只能发送对象点的坐标值。我的问题是我无法为这些点设置颜色。这是我的代码:

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <iostream>
#include <math.h>
#include <cmath>
#include <algorithm>
#include <numeric>
#include <visualization_msgs/Marker.h>
#include "std_msgs/String.h"
#include "std_msgs/Float32.h"
#include "std_msgs/Int32.h"
#include "rosbag/bag.h"
#include <rosbag/view.h>
#include <vector>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH
ros::Publisher pub;   
ros::Publisher marker_pub;

void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
    pcl::PointCloud<pcl::PointXYZRGB> output;
    pcl::fromROSMsg(*input,output);
    visualization_msgs::Marker points;
    points.header.frame_id = "/camera_link";
    points.header.stamp = ros::Time::now();
    points.ns = "lines";
    points.action = visualization_msgs::Marker::ADD;
    points.pose.orientation.w = 1.0;
    points.type = visualization_msgs::Marker::POINTS;
    points.scale.x = 0.003;
    points.scale.y = 0.003;
    points.id = 0;
    points.color.r=1; points.color.a=1; 
    geometry_msgs::Point p;

for(int i=0;i<ac1.size();i++) {
                   if(i%10==0 && !isnan(ac1.at(i)) && !isnan(bc1.at(i)) && !isnan(cc1.at(i)) && cc1.at(i)!=0) {
                     points.id=i; points.color.r=xc.at(i)/255; points.color.g=yc.at(i)/255; points.color.b=zc.at(i)/255;
                     p.x=ac1.at(i); p.y=bc1.at(i); p.z=cc1.at(i);  points.points.push_back(p);
                     marker_pub.publish(points); } 
}
cout<<"finish"<<endl;
}


int
main(int argc, char** argv)
{
    ROS_INFO("main");
    ros::init(argc, argv,"obj_rec");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe ("/camera/depth/points", 1, cloud_cb);
    pub = nh.advertise<sensor_msgs::PointCloud2>("output",1);
    marker_pub = nh.advertise<visualization_msgs::Marker> ("out",1);
    ros::spin();
}

这里的ac1, bc1 和cc1 向量包含对象的坐标和xc, yc 和zc 向量包含对应点的RGB 值。我没有放完整的代码，因为它太长了，而且要运行这个单个 ROS 节点，还需要其他 C++ 程序。

Answer 1

如果您使用 POINTS 作为您的标记，正如您的代码所建议的，您应该指定字段 colors(不要与字段 color< 混淆)/)。 colors 字段必须是一个列表，其元素数与 points 字段中的元素数相同。这在消息中指定 documentation .

这段代码产生三个不同颜色的点:

ros::NodeHandle n;
ros::Publisher vis_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 0);

visualization_msgs::Marker marker;
marker.header.frame_id = "/map";
marker.header.stamp = ros::Time();
marker.type = visualization_msgs::Marker::POINTS;
marker.action = visualization_msgs::Marker::ADD;
marker.scale.x = 0.1;
marker.scale.y = 0.1;
marker.scale.z = 0.1;
marker.color.a = 1.0;
marker.color.r = 0.0;
marker.color.g = 1.0;
marker.color.b = 0.0;

geometry_msgs::Point p_1;
geometry_msgs::Point p_2;
geometry_msgs::Point p_3;

p_1.x = 0.0;
p_1.y = 0.0;
p_1.z = 0.0;

p_2.x = 1.0;
p_2.y = 1.0;
p_2.z = 1.0;

p_3.x = 2.0;
p_3.y = 2.0;
p_3.z = 2.0;

std::vector<geometry_msgs::Point> my_points;
my_points.push_back(p_1);
my_points.push_back(p_2);
my_points.push_back(p_3);

for (int ii = 0; ii < my_points.size(); ++ii)
{
    std_msgs::ColorRGBA c;
    if( ii == 0)
        c.r = 1.0;
    else if(ii == 1)
        c.g = 1.0;
    else
        c.b = 1.0;
    c.a = 1.0;

    marker.points.push_back(my_points[ii]);
    // Here, the field colors is populated with a specific color per point.
    marker.colors.push_back(c);
}

vis_pub.publish(marker);

此代码产生以下内容: