在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);

此代码生成以下内容：