From 6658cffa20b965649a22f024b6724bf8f8ffd51d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Fri, 9 Jun 2017 12:39:04 +0200 Subject: [PATCH] Fix rviz trajectory rendering crash (#367) Rviz has a limit of 16384 points per marker. To get around this, each trajectory is split into multiple markers, each up to 16384 points. Fixes #366. --- .../cartographer_ros/map_builder_bridge.cc | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index be46037a7..acf62a694 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -217,8 +217,18 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodesList() { marker.scale.x = kTrajectoryLineStripMarkerScale; marker.pose.orientation.w = 1.0; for (const auto& node : single_trajectory) { - marker.points.push_back(ToGeometryMsgPoint( - (node.pose * node.constant_data->tracking_to_pose).translation())); + const ::geometry_msgs::Point node_point = ToGeometryMsgPoint( + (node.pose * node.constant_data->tracking_to_pose).translation()); + marker.points.push_back(node_point); + // Work around the 16384 point limit in rviz by splitting the + // trajectory into multiple markers. + if (marker.points.size() == 16384) { + trajectory_nodes_list.markers.push_back(marker); + marker.id = marker_id++; + marker.points.clear(); + // Push back the last point, so the two markers appear connected. + marker.points.push_back(node_point); + } } trajectory_nodes_list.markers.push_back(marker); }