From 38f1c4dc14272ba9ce473f308c19b8d1bd6cd2ce Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Tue, 17 Oct 2017 11:00:31 +0200 Subject: [PATCH] Follow googlecartographer/cartographer#591. (#542) --- cartographer_ros/cartographer_ros/map_builder_bridge.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index 624c144..b71f169 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -199,7 +199,7 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() { continue; } const ::geometry_msgs::Point node_point = - ToGeometryMsgPoint(node.pose.translation()); + ToGeometryMsgPoint(node.global_pose.translation()); marker.points.push_back(node_point); // Work around the 16384 point limit in RViz by splitting the // trajectory into multiple markers. @@ -290,7 +290,7 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() { const auto& trajectory_node_pose = all_trajectory_nodes[constraint.node_id.trajectory_id] [constraint.node_id.node_index] - .pose; + .global_pose; const cartographer::transform::Rigid3d constraint_pose = submap_pose * constraint.pose.zbar_ij;