12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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.
master
Juraj Oršulić 2017-06-09 12:39:04 +02:00 committed by Wolfgang Hess
parent 51fe6fa021
commit 6658cffa20
1 changed files with 12 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -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);
}