nav2_route vizualization marker rendering performance improvement

nav2_route/include/nav2_route/utils.hpp

@@ -59,102 +59,90 @@ inline visualization_msgs::msg::MarkerArray toMsg(
   const nav2_route::Graph & graph, const std::string & frame, const rclcpp::Time & now)
 {
   visualization_msgs::msg::MarkerArray msg;
-  visualization_msgs::msg::Marker curr_marker;
-  curr_marker.header.frame_id = frame;
-  curr_marker.header.stamp = now;
-  curr_marker.action = 0;
 
-  auto getSphereSize = []() {
-      geometry_msgs::msg::Vector3 v_msg;
-      v_msg.x = 0.05;
-      v_msg.y = 0.05;
-      v_msg.z = 0.05;
-      return v_msg;
-    };
-
-  auto getSphereColor = []() {
-      std_msgs::msg::ColorRGBA c_msg;
-      c_msg.r = 1.0;
-      c_msg.g = 0.0;
-      c_msg.b = 0.0;
-      c_msg.a = 1.0;
-      return c_msg;
-    };
-
-  auto getLineColor = []() {
-      std_msgs::msg::ColorRGBA c_msg;
-      c_msg.r = 0.0;
-      c_msg.g = 1.0;
-      c_msg.b = 0.0;
-      c_msg.a = 0.5;  // So bi-directional connections stand out overlapping
-      return c_msg;
-    };
-
-  unsigned int marker_idx = 1;
-  for (unsigned int i = 0; i != graph.size(); i++) {
-    if (graph[i].nodeid == std::numeric_limits<int>::max()) {
-      continue;  // Skip "deleted" nodes
-    }
-    curr_marker.ns = "route_graph";
-    curr_marker.id = marker_idx++;
-    curr_marker.type = visualization_msgs::msg::Marker::SPHERE;
-    curr_marker.pose.position.x = graph[i].coords.x;
-    curr_marker.pose.position.y = graph[i].coords.y;
-    curr_marker.scale = getSphereSize();
-    curr_marker.color = getSphereColor();
-    msg.markers.push_back(curr_marker);
-
-    // Add text
-    curr_marker.ns = "route_graph_ids";
-    curr_marker.id = marker_idx++;
-    curr_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
-    curr_marker.pose.position.x = graph[i].coords.x + 0.07;
-    curr_marker.pose.position.y = graph[i].coords.y;
-    curr_marker.text = std::to_string(graph[i].nodeid);
-    curr_marker.scale.z = 0.1;
-    msg.markers.push_back(curr_marker);
-
-    for (unsigned int j = 0; j != graph[i].neighbors.size(); j++) {
-      curr_marker.ns = "route_graph";
-      curr_marker.id = marker_idx++;
-      curr_marker.type = visualization_msgs::msg::Marker::LINE_LIST;
-      curr_marker.pose.position.x = 0;  // Set to 0 since points are relative to this frame
-      curr_marker.pose.position.y = 0;  // Set to 0 since points are relative to this frame
-      curr_marker.points.resize(2);
-      curr_marker.points[0].x = graph[i].coords.x;
-      curr_marker.points[0].y = graph[i].coords.y;
-      curr_marker.points[1].x = graph[i].neighbors[j].end->coords.x;
-      curr_marker.points[1].y = graph[i].neighbors[j].end->coords.y;
-      curr_marker.scale.x = 0.03;
-      curr_marker.color = getLineColor();
-      msg.markers.push_back(curr_marker);
-      curr_marker.points.clear();  // Reset for next node marker
-
-      // Add text
-      curr_marker.ns = "route_graph_ids";
-      curr_marker.id = marker_idx++;
-      curr_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
-      curr_marker.pose.position.x =
-        graph[i].coords.x + ((graph[i].neighbors[j].end->coords.x - graph[i].coords.x) / 2.0) +
-        0.07;
+  visualization_msgs::msg::Marker nodes_marker;
+  nodes_marker.header.frame_id = frame;
+  nodes_marker.header.stamp = now;
+  nodes_marker.action = 0;
+  nodes_marker.ns = "route_graph_nodes";
+  nodes_marker.type = visualization_msgs::msg::Marker::POINTS;
+  nodes_marker.scale.x = 0.5;
+  nodes_marker.scale.y = 0.5;
+  nodes_marker.scale.z = 0.5;
+  nodes_marker.color.r = 1.0;
+  nodes_marker.color.a = 1.0;
+  nodes_marker.points.reserve(graph.size());
+
+  visualization_msgs::msg::Marker edges_marker;
+  edges_marker.header.frame_id = frame;
+  edges_marker.header.stamp = now;
+  edges_marker.action = 0;
+  edges_marker.ns = "route_graph_edges";
+  edges_marker.type = visualization_msgs::msg::Marker::LINE_LIST;
+  edges_marker.scale.x = 0.1;  // Line width
+  edges_marker.color.g = 1.0;
+  edges_marker.color.a = 0.5;  // Semi-transparent green so bidirectional connections stand out
+  constexpr size_t points_per_edge = 2;
+  // This probably under-reserves but saves some initial reallocations
+  constexpr size_t likely_min_edges_per_node = 2;
+  edges_marker.points.reserve(graph.size() * points_per_edge * likely_min_edges_per_node);
+
+  geometry_msgs::msg::Point node_pos;
+  geometry_msgs::msg::Point edge_start;
+  geometry_msgs::msg::Point edge_end;
+
+  visualization_msgs::msg::Marker node_id_marker;
+  node_id_marker.ns = "route_graph_node_ids";
+  node_id_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
+  node_id_marker.scale.z = 0.1;
+
+  visualization_msgs::msg::Marker edge_id_marker;
+  edge_id_marker.ns = "route_graph_edge_ids";
+  edge_id_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
+  edge_id_marker.scale.z = 0.1;
+
+  for (const auto & node : graph) {
+    node_pos.x = node.coords.x;
+    node_pos.y = node.coords.y;
+    nodes_marker.points.push_back(node_pos);
+
+    // Add text for Node ID
+    node_id_marker.id++;
+    node_id_marker.pose.position.x = node.coords.x + 0.07;
+    node_id_marker.pose.position.y = node.coords.y;
+    node_id_marker.text = std::to_string(node.nodeid);
+    msg.markers.push_back(node_id_marker);
+
+    for (const auto & neighbor : node.neighbors) {
+      edge_start.x = node.coords.x;
+      edge_start.y = node.coords.y;
+      edge_end.x = neighbor.end->coords.x;
+      edge_end.y = neighbor.end->coords.y;
+      edges_marker.points.push_back(edge_start);
+      edges_marker.points.push_back(edge_end);
 
       // Deal with overlapping bi-directional text markers by offsetting locations
       float y_offset = 0.0;
-      if (graph[i].nodeid > graph[i].neighbors[j].end->nodeid) {
+      if (node.nodeid > neighbor.end->nodeid) {
         y_offset = 0.05;
       } else {
         y_offset = -0.05;
       }
-
-      curr_marker.pose.position.y =
-        graph[i].coords.y + ((graph[i].neighbors[j].end->coords.y - graph[i].coords.y) / 2.0) +
-        y_offset;
-      curr_marker.text = std::to_string(graph[i].neighbors[j].edgeid);
-      curr_marker.scale.z = 0.1;
-      msg.markers.push_back(curr_marker);
+      const float x_offset = 0.07;
+
+      // Add text for Edge ID
+      edge_id_marker.id++;
+      edge_id_marker.pose.position.x =
+        node.coords.x + ((neighbor.end->coords.x - node.coords.x) / 2.0) + x_offset;
+      edge_id_marker.pose.position.y =
+        node.coords.y + ((neighbor.end->coords.y - node.coords.y) / 2.0) + y_offset;
+      edge_id_marker.text = std::to_string(neighbor.edgeid);
+      msg.markers.push_back(edge_id_marker);
     }
   }
 
+  msg.markers.push_back(nodes_marker);
+  msg.markers.push_back(edges_marker);
   return msg;
 }
 

nav2_route/test/test_utils_and_types.cpp

@@ -159,7 +159,15 @@ TEST(UtilsTest, test_to_visualization_msg_conversion)
   graph[3].addEdge(default_cost, &graph[6], ids++);
 
   auto graph_msg = utils::toMsg(graph, frame, time);
-  EXPECT_EQ(graph_msg.markers.size(), 34u);  // 9 nodes and 8 edges (with text markers)
+  constexpr size_t expected_edge_markers = 1;
+  constexpr size_t expected_node_markers = 1;
+  constexpr size_t expected_edge_id_text_markers = 8;
+  constexpr size_t expected_node_id_text_markers = 9;
+  constexpr size_t expected_total_markers =
+    expected_edge_markers + expected_node_markers +
+    expected_edge_id_text_markers + expected_node_id_text_markers;
+
+  EXPECT_EQ(graph_msg.markers.size(), expected_total_markers);
   for (auto & marker : graph_msg.markers) {
     if (marker.ns == "route_graph_ids") {
       EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::TEXT_VIEW_FACING);