std_msgs::ColorRGBA is not coloring specific markers on RViz
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.