From dc2476ef2b1644c47b86aed79f1f921523765204 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Tue, 6 Nov 2018 17:43:27 +0100 Subject: [PATCH] Consider waiting trajectories with a sensor bridge as active. (#1089) * Consider waiting trajectories with a sensor bridge as active. Fixes a corner case where trajectories that didn't start SLAMing yet couldn't be finished, e.g. due to waiting for sensor data. They don't appear in the trajectory states list of the pose graph yet but already have a trajectory builder. https://github.com/googlecartographer/cartographer/issues/1367 --- .../cartographer_ros/map_builder_bridge.cc | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index 13384c9..dc6760f 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -199,7 +199,15 @@ void MapBuilderBridge::HandleSubmapQuery( std::map MapBuilderBridge::GetTrajectoryStates() { - return map_builder_->pose_graph()->GetTrajectoryStates(); + auto trajectory_states = map_builder_->pose_graph()->GetTrajectoryStates(); + // Add active trajectories that are not yet in the pose graph, but are e.g. + // waiting for input sensor data and thus already have a sensor bridge. + for (const auto& sensor_bridge : sensor_bridges_) { + trajectory_states.insert(std::make_pair( + sensor_bridge.first, + ::cartographer::mapping::PoseGraph::TrajectoryState::ACTIVE)); + } + return trajectory_states; } cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {