Robotics StackExchange | Archived questions

[Solved] rviz2 line strip coloring

I'm using rviz2 line strip markers to visualize polygons. As the edges of my polygons have different properties, I want to visualize this with the color of the edges. According to http://wiki.ros.org/rviz/DisplayTypes/Marker#Line_Strip_.28LINE_STRIP.3D4.29 this should be possible ("In visualization 1.1+ will also optionally use the colors member for per-vertex color.").

However, with the following code:

visualization_msgs::msg::MarkerArray polygon_markers;
  polygon_markers.markers.clear();

  visualization_msgs::msg::Marker polygon_marker_template;
  polygon_marker_template.header = header;
  polygon_marker_template.header.frame_id = "world";
  polygon_marker_template.type = visualization_msgs::msg::Marker::LINE_STRIP;
  polygon_marker_template.action = visualization_msgs::msg::Marker::MODIFY;
  polygon_marker_template.color.r = 0.0;
  polygon_marker_template.color.g = 0.0;
  polygon_marker_template.color.b = 0.0;
  polygon_marker_template.color.a = 1.0;

  polygon_marker_template.pose.position.x = 0;
  polygon_marker_template.pose.position.y = 0;
  polygon_marker_template.pose.position.z = 0;
  polygon_marker_template.pose.orientation.x = 0;
  polygon_marker_template.pose.orientation.y = 0;
  polygon_marker_template.pose.orientation.z = 0;
  polygon_marker_template.pose.orientation.w = 1;
  polygon_marker_template.scale.x = 0.1;
  polygon_marker_template.scale.y = 0.0;
  polygon_marker_template.scale.z = 0.0;

  polygon_marker_template.points.clear();
  polygon_marker_template.colors.clear();

  unsigned int id = 0;
  for (const auto& pose : pose_graph.getPoseGraphPoses()) {
    std::cout << "Pose id: " << pose.getId() << std::endl;
    visualization_msgs::msg::Marker polygon_marker = polygon_marker_template;
    polygon_marker.id = id++;
    polygon_marker.points.clear();
    polygon_marker.colors.clear();

    auto polygon_pose = pose.getPolygon();
    auto pose_world = pose_graph_transformations[pose.getId()];
    auto polygon_world = polygon_pose.transformPolygon(pose_world);

    auto points = polygon_world.getXYPoints();
    auto edge_types = polygon_world.getEdgeTypes();
    for (unsigned int i = 0; i < polygon_world.getXYPoints().size(); ++i) {
      geometry_msgs::msg::Point marker_point;
      marker_point.x = points.at(i).getX();
      marker_point.y = points.at(i).getY();
      marker_point.z = 0;

      std_msgs::msg::ColorRGBA color;
      color.r = 0.0;
      color.g = 0.0;
      color.b = 0.0;
      color.a = 0.0;

      std::cout << "Point " << i << ": edge type: " << edge_types[i]
                << std::endl;
      if (edge_types[i] == EdgeType::OBSTACLE) {
        color.r = 1.0;
        color.g = 0.0;
        color.b = 0.0;
        color.a = 1.0;
      } else if (edge_types[i] == EdgeType::FRONTIER) {
        color.r = 0.0;
        color.g = 0.0;
        color.b = 1.0;
        color.a = 1.0;
      }

      polygon_marker.points.push_back(marker_point);
      polygon_marker.colors.push_back(color);
    }

    polygon_markers.markers.push_back(polygon_marker);
  }
  polygonVisualizationPublisher_->publish(polygon_markers);

I get the following visualization: visualization image description

which looks more like the description of "Line List" ("The color of a line segment then gradually changes from its start to end point.").

Asked by Andreas Ziegler on 2019-05-21 00:18:31 UTC

Comments

Could you please attach your screenshot to the question itself instead of linking to it? I've given you sufficient karma to do that.

Asked by gvdhoorn on 2019-05-21 02:58:38 UTC

Line strips define the color 'per vertex' so by definition they will fade colors along each line if two adjacent vertices are different. If instead you want to display a polygon so that each line is a different solid color you will have to use a line strip and duplicate each vertex, there is no way around this.

Asked by PeteBlackerThe3rd on 2019-05-21 06:57:50 UTC

Thanks for the workarournd.

Asked by Andreas Ziegler on 2019-05-21 11:16:33 UTC

Answers