0
$\begingroup$

Rosanswers logo

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.


Originally posted by RayROS on ROS Answers with karma: 108 on 2020-05-20

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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.


Originally posted by Roberto Z. with karma: 500 on 2020-05-22

This answer was ACCEPTED on the original site

Post score: 1

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.