Skip to content

Commit c71d2a9

Browse files
emersonknappsilanus23
emersonknapp
authored and
silanus23
committed
nav2_route vizualization marker rendering performance improvement (ros-navigation#5452)
* nav2_route vizualization marker use sphere_list and line_list for rendering performance Signed-off-by: Emerson Knapp <emerson.b.knapp@gmail.com> * Fix unit test and break out magic constants into named values Signed-off-by: Emerson Knapp <emerson.b.knapp@gmail.com> --------- Signed-off-by: Emerson Knapp <emerson.b.knapp@gmail.com>
1 parent 594e11c commit c71d2a9

File tree

2 files changed

+82
-86
lines changed

2 files changed

+82
-86
lines changed

‎nav2_route/include/nav2_route/utils.hpp‎

Lines changed: 73 additions & 85 deletions
Original file line numberDiff line numberDiff line change
@@ -59,102 +59,90 @@ inline visualization_msgs::msg::MarkerArray toMsg(
5959
const nav2_route::Graph & graph, const std::string & frame, const rclcpp::Time & now)
6060
{
6161
visualization_msgs::msg::MarkerArray msg;
62-
visualization_msgs::msg::Marker curr_marker;
63-
curr_marker.header.frame_id = frame;
64-
curr_marker.header.stamp = now;
65-
curr_marker.action = 0;
6662

67-
auto getSphereSize = []() {
68-
geometry_msgs::msg::Vector3 v_msg;
69-
v_msg.x = 0.05;
70-
v_msg.y = 0.05;
71-
v_msg.z = 0.05;
72-
return v_msg;
73-
};
74-
75-
auto getSphereColor = []() {
76-
std_msgs::msg::ColorRGBA c_msg;
77-
c_msg.r = 1.0;
78-
c_msg.g = 0.0;
79-
c_msg.b = 0.0;
80-
c_msg.a = 1.0;
81-
return c_msg;
82-
};
83-
84-
auto getLineColor = []() {
85-
std_msgs::msg::ColorRGBA c_msg;
86-
c_msg.r = 0.0;
87-
c_msg.g = 1.0;
88-
c_msg.b = 0.0;
89-
c_msg.a = 0.5; // So bi-directional connections stand out overlapping
90-
return c_msg;
91-
};
92-
93-
unsigned int marker_idx = 1;
94-
for (unsigned int i = 0; i != graph.size(); i++) {
95-
if (graph[i].nodeid == std::numeric_limits<int>::max()) {
96-
continue; // Skip "deleted" nodes
97-
}
98-
curr_marker.ns = "route_graph";
99-
curr_marker.id = marker_idx++;
100-
curr_marker.type = visualization_msgs::msg::Marker::SPHERE;
101-
curr_marker.pose.position.x = graph[i].coords.x;
102-
curr_marker.pose.position.y = graph[i].coords.y;
103-
curr_marker.scale = getSphereSize();
104-
curr_marker.color = getSphereColor();
105-
msg.markers.push_back(curr_marker);
106-
107-
// Add text
108-
curr_marker.ns = "route_graph_ids";
109-
curr_marker.id = marker_idx++;
110-
curr_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
111-
curr_marker.pose.position.x = graph[i].coords.x + 0.07;
112-
curr_marker.pose.position.y = graph[i].coords.y;
113-
curr_marker.text = std::to_string(graph[i].nodeid);
114-
curr_marker.scale.z = 0.1;
115-
msg.markers.push_back(curr_marker);
116-
117-
for (unsigned int j = 0; j != graph[i].neighbors.size(); j++) {
118-
curr_marker.ns = "route_graph";
119-
curr_marker.id = marker_idx++;
120-
curr_marker.type = visualization_msgs::msg::Marker::LINE_LIST;
121-
curr_marker.pose.position.x = 0; // Set to 0 since points are relative to this frame
122-
curr_marker.pose.position.y = 0; // Set to 0 since points are relative to this frame
123-
curr_marker.points.resize(2);
124-
curr_marker.points[0].x = graph[i].coords.x;
125-
curr_marker.points[0].y = graph[i].coords.y;
126-
curr_marker.points[1].x = graph[i].neighbors[j].end->coords.x;
127-
curr_marker.points[1].y = graph[i].neighbors[j].end->coords.y;
128-
curr_marker.scale.x = 0.03;
129-
curr_marker.color = getLineColor();
130-
msg.markers.push_back(curr_marker);
131-
curr_marker.points.clear(); // Reset for next node marker
132-
133-
// Add text
134-
curr_marker.ns = "route_graph_ids";
135-
curr_marker.id = marker_idx++;
136-
curr_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
137-
curr_marker.pose.position.x =
138-
graph[i].coords.x + ((graph[i].neighbors[j].end->coords.x - graph[i].coords.x) / 2.0) +
139-
0.07;
63+
visualization_msgs::msg::Marker nodes_marker;
64+
nodes_marker.header.frame_id = frame;
65+
nodes_marker.header.stamp = now;
66+
nodes_marker.action = 0;
67+
nodes_marker.ns = "route_graph_nodes";
68+
nodes_marker.type = visualization_msgs::msg::Marker::POINTS;
69+
nodes_marker.scale.x = 0.5;
70+
nodes_marker.scale.y = 0.5;
71+
nodes_marker.scale.z = 0.5;
72+
nodes_marker.color.r = 1.0;
73+
nodes_marker.color.a = 1.0;
74+
nodes_marker.points.reserve(graph.size());
75+
76+
visualization_msgs::msg::Marker edges_marker;
77+
edges_marker.header.frame_id = frame;
78+
edges_marker.header.stamp = now;
79+
edges_marker.action = 0;
80+
edges_marker.ns = "route_graph_edges";
81+
edges_marker.type = visualization_msgs::msg::Marker::LINE_LIST;
82+
edges_marker.scale.x = 0.1; // Line width
83+
edges_marker.color.g = 1.0;
84+
edges_marker.color.a = 0.5; // Semi-transparent green so bidirectional connections stand out
85+
constexpr size_t points_per_edge = 2;
86+
// This probably under-reserves but saves some initial reallocations
87+
constexpr size_t likely_min_edges_per_node = 2;
88+
edges_marker.points.reserve(graph.size() * points_per_edge * likely_min_edges_per_node);
89+
90+
geometry_msgs::msg::Point node_pos;
91+
geometry_msgs::msg::Point edge_start;
92+
geometry_msgs::msg::Point edge_end;
93+
94+
visualization_msgs::msg::Marker node_id_marker;
95+
node_id_marker.ns = "route_graph_node_ids";
96+
node_id_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
97+
node_id_marker.scale.z = 0.1;
98+
99+
visualization_msgs::msg::Marker edge_id_marker;
100+
edge_id_marker.ns = "route_graph_edge_ids";
101+
edge_id_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
102+
edge_id_marker.scale.z = 0.1;
103+
104+
for (const auto & node : graph) {
105+
node_pos.x = node.coords.x;
106+
node_pos.y = node.coords.y;
107+
nodes_marker.points.push_back(node_pos);
108+
109+
// Add text for Node ID
110+
node_id_marker.id++;
111+
node_id_marker.pose.position.x = node.coords.x + 0.07;
112+
node_id_marker.pose.position.y = node.coords.y;
113+
node_id_marker.text = std::to_string(node.nodeid);
114+
msg.markers.push_back(node_id_marker);
115+
116+
for (const auto & neighbor : node.neighbors) {
117+
edge_start.x = node.coords.x;
118+
edge_start.y = node.coords.y;
119+
edge_end.x = neighbor.end->coords.x;
120+
edge_end.y = neighbor.end->coords.y;
121+
edges_marker.points.push_back(edge_start);
122+
edges_marker.points.push_back(edge_end);
140123

141124
// Deal with overlapping bi-directional text markers by offsetting locations
142125
float y_offset = 0.0;
143-
if (graph[i].nodeid > graph[i].neighbors[j].end->nodeid) {
126+
if (node.nodeid > neighbor.end->nodeid) {
144127
y_offset = 0.05;
145128
} else {
146129
y_offset = -0.05;
147130
}
148-
149-
curr_marker.pose.position.y =
150-
graph[i].coords.y + ((graph[i].neighbors[j].end->coords.y - graph[i].coords.y) / 2.0) +
151-
y_offset;
152-
curr_marker.text = std::to_string(graph[i].neighbors[j].edgeid);
153-
curr_marker.scale.z = 0.1;
154-
msg.markers.push_back(curr_marker);
131+
const float x_offset = 0.07;
132+
133+
// Add text for Edge ID
134+
edge_id_marker.id++;
135+
edge_id_marker.pose.position.x =
136+
node.coords.x + ((neighbor.end->coords.x - node.coords.x) / 2.0) + x_offset;
137+
edge_id_marker.pose.position.y =
138+
node.coords.y + ((neighbor.end->coords.y - node.coords.y) / 2.0) + y_offset;
139+
edge_id_marker.text = std::to_string(neighbor.edgeid);
140+
msg.markers.push_back(edge_id_marker);
155141
}
156142
}
157143

144+
msg.markers.push_back(nodes_marker);
145+
msg.markers.push_back(edges_marker);
158146
return msg;
159147
}
160148

‎nav2_route/test/test_utils_and_types.cpp‎

Lines changed: 9 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -159,7 +159,15 @@ TEST(UtilsTest, test_to_visualization_msg_conversion)
159159
graph[3].addEdge(default_cost, &graph[6], ids++);
160160

161161
auto graph_msg = utils::toMsg(graph, frame, time);
162-
EXPECT_EQ(graph_msg.markers.size(), 34u); // 9 nodes and 8 edges (with text markers)
162+
constexpr size_t expected_edge_markers = 1;
163+
constexpr size_t expected_node_markers = 1;
164+
constexpr size_t expected_edge_id_text_markers = 8;
165+
constexpr size_t expected_node_id_text_markers = 9;
166+
constexpr size_t expected_total_markers =
167+
expected_edge_markers + expected_node_markers +
168+
expected_edge_id_text_markers + expected_node_id_text_markers;
169+
170+
EXPECT_EQ(graph_msg.markers.size(), expected_total_markers);
163171
for (auto & marker : graph_msg.markers) {
164172
if (marker.ns == "route_graph_ids") {
165173
EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::TEXT_VIEW_FACING);

0 commit comments

Comments
 (0)