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

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
master
Michael Grupp 2018-11-06 17:43:27 +01:00 committed by Alexander Belyaev
parent c493369594
commit dc2476ef2b
1 changed files with 9 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -199,7 +199,15 @@ void MapBuilderBridge::HandleSubmapQuery(
std::map<int, ::cartographer::mapping::PoseGraphInterface::TrajectoryState>
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() {