ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

std_msgs::ColorRGBA is not coloring specific markers on RViz

asked 2020-05-20 14:59:45 -0500

RayROS gravatar image

updated 2020-05-20 15:23:45 -0500

I am trying to publish colored LINE_STRIPS markers on RViz. I came across this message and tried to apply it to my case. Although all markers are published, the problem I have is that specific segments are not being published with the specific color I am trying to show.

The error I get from RViz is: Number of colors doesn't match number of points.

#include "segmentpublisher.h"
#include "constants.h"
#include <ColorRGBA.h>

SegmentPublisher::SegmentPublisher() {}

void SegmentPublisher::initPublisher(ros::NodeHandle nh, std::string name) {
    markerPublisher = nh.advertise<visualization_msgs::Marker>(name,10);
    m_initialized = true;
}

void SegmentPublisher::publish(PointData inPointData) {
    if(m_initialized) {
        markerPublisher.publish(fromPointDataToSegment(inPointData, 1.0f, 1.0f, 1.0f));
    }
}

void SegmentPublisher::publish(PointData inPointData, float r, float g, float b) {
    if(m_initialized) {
        markerPublisher.publish(fromPointDataToSegment(inPointData, r, g, b));
    }
}


visualization_msgs::Marker SegmentPublisher::fromPointDataToSegment(PointData inPointData, float r, float g, float b)
{
    visualization_msgs::Marker segment;
    segment.header.frame_id = "/segment_frame";
    segment.header.stamp = ros::Time::now();
    segment.ns = "distance";
    segment.action = visualization_msgs::Marker::ADD;
    segment.pose.orientation.w = 1.0;
    segment.id = 0;
    segment.type = visualization_msgs::Marker::LINE_LIST;
    segment.scale.x = 0.03f;
    segment.color.r = r;
    segment.color.g = g;
    segment.color.b = b;
    segment.color.a = 0.5f;

    geometry_msgs::Point segment_a, segment_b, segment_c, segment_d;
    /* segment_a is the lidar origin */
    segment_a.x = 0.0f;
    segment_a.y = 0.0f;
    segment_a.z = 0.25f;

// other operations ...
}

    if(inPointData.dockDistance() < Constants::maxDistanceDetected)
    {

        std_msgs::ColorRGBA color;
        color.r = 255;
        color.g = 0;
        color.b = 0;
        color.a = 1;

        segment_b.x = inPointData.point().x;
        segment_b.y = inPointData.point().y;
        segment_b.z = segment_a.z + inPointData.point().z;

        segment_d.x = 0.0f;
        segment_d.y = inPointData.point().y;
        segment_d.z = 0.0f;

        /* Publish segment point to y-axis */
        segment.points.push_back(segment_b); // <--  trying to color this segment
        segment.points.push_back(segment_d); //  <--  trying to color this segment
    }
    else
    {
        segment_b = segment_a;
        segment_b.z += 1.0f;

        segment.points.push_back(segment_a);
        segment.points.push_back(segment_b);
    }

    return segment;
}

I came across this source and this source too but could not understand what I am missing to show the color I am looking for. I also consulted this source to see if I was missing something useful.

Please point to the right direction for solving this problem.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2020-05-22 18:05:10 -0500

Roberto Z. gravatar image

updated 2020-05-22 18:07:45 -0500

It is defined here: Raw Message Definition, more precisely in these lines:

#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

You are pushing back into points but you are missing to push back into colors (also member of the visualization_msgs/Marker message), defined in this line:

 std_msgs/ColorRGBA[] colors

This part you already have:

   segment.points.push_back(segment_a);
   segment.points.push_back(segment_b);

This part you are still missing:

  // push back one color per segment
  segment.colors.push_back(segment_a.color); // pass in any std_msgs::ColorRGBA object
  segment.colors.push_back(segment_b.color);

The number of colors you push back has to match the number of points (you are calling them segments) that you push back.

edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2020-05-20 14:59:45 -0500

Seen: 451 times

Last updated: May 22 '20