Sending different coloured markers in RViz

Question

I am trying to send different colored markers in RViz so that I can visualize whether my program is working accordingly. I am only able to send the coordinate values of the points of the object for now. My problem is that I am not able to set the colors for those points. Here is my code:

#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();
}

Here ac1, bc1 and cc1 vectors contain the coordinates of the object and the xc, yc and zc vector contains RGB values of corresponding points. I haven't put the entire code because it is too long and to run this single ROS node other C++ programs are required.

Answer 1

If you are using POINTS as your marker, as your code suggests, you should specify the field colors (not to be confused with the field color). The field colors must be a list with the same number of elements as there are in the field points. This is specified in the message documentation.

This snippet produce three points with different colors:

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

This code produces the following: