From 4b39f6c97a10361766884855dacab9388486b103 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Wed, 13 Aug 2025 12:55:03 -0700 Subject: [PATCH 1/2] nav2_route vizualization marker use sphere_list and line_list for rendering performance Signed-off-by: Emerson Knapp --- nav2_route/include/nav2_route/utils.hpp | 157 +++++++++++------------- 1 file changed, 72 insertions(+), 85 deletions(-) diff --git a/nav2_route/include/nav2_route/utils.hpp b/nav2_route/include/nav2_route/utils.hpp index 9ccf65eb41e..fe233535998 100644 --- a/nav2_route/include/nav2_route/utils.hpp +++ b/nav2_route/include/nav2_route/utils.hpp @@ -59,102 +59,89 @@ 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::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 + // Each edge has 2 points, each node is likely to have at least 2 edges. + // This likely under-reserves but saves some initial reallocations + edges_marker.points.reserve(graph.size() * 2 * 2); + + 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; } From 87416166071eea9e8c1c21ec136c6c941b15f058 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Mon, 25 Aug 2025 16:02:24 -0700 Subject: [PATCH 2/2] Fix unit test and break out magic constants into named values Signed-off-by: Emerson Knapp --- nav2_route/include/nav2_route/utils.hpp | 7 ++++--- nav2_route/test/test_utils_and_types.cpp | 10 +++++++++- 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/nav2_route/include/nav2_route/utils.hpp b/nav2_route/include/nav2_route/utils.hpp index fe233535998..04deda14bec 100644 --- a/nav2_route/include/nav2_route/utils.hpp +++ b/nav2_route/include/nav2_route/utils.hpp @@ -82,9 +82,10 @@ inline visualization_msgs::msg::MarkerArray toMsg( 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 - // Each edge has 2 points, each node is likely to have at least 2 edges. - // This likely under-reserves but saves some initial reallocations - edges_marker.points.reserve(graph.size() * 2 * 2); + 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; diff --git a/nav2_route/test/test_utils_and_types.cpp b/nav2_route/test/test_utils_and_types.cpp index a6b9a7e37e9..25703f95ff3 100644 --- a/nav2_route/test/test_utils_and_types.cpp +++ b/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);