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

Fix bug in FinishTrajectory logic (#926) 

This PR adds additional bookkeeping for trajectories that we scheduled for
finishing.

In Node::RunFinalOptimization(...), we were calling FinishTrajectory for
every active trajectory (state == ACTIVE). Since the state only gets updated
once the corresponding worker for the FinishTrajectory task is
scheduled, we were potentially calling FinishTrajectory twice for a
single trajectory id.

Reproducible on master e.g. with 
```
roslaunch cartographer_ros offline_backpack_2d.launch bag_filenames:=b2-2016-02-02-14-01-56.bag no_rviz:=true
```
master
Sebastian Klose 2018-07-06 15:47:54 +02:00 committed by Wally B. Feed
parent bfe0d01b71
commit 0aa6ecc40f
2 changed files with 10 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
cartographer_ros/cartographer_ros/node.cc
View File

@ -460,6 +460,14 @@ cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock(
auto trajectory_states = map_builder_bridge_.GetTrajectoryStates();
cartographer_ros_msgs::StatusResponse status_response;
if (trajectories_scheduled_for_finish_.count(trajectory_id)) {
const std::string message = "Trajectory " + std::to_string(trajectory_id) +
" already pending to finish.";
status_response.code = cartographer_ros_msgs::StatusCode::OK;
status_response.message = message;
LOG(INFO) << message;
return status_response;
}
// First, check if we can actually finish the trajectory.
if (!(trajectory_states.count(trajectory_id))) {
@ -505,6 +513,7 @@ cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock(
CHECK_EQ(subscribers_.erase(trajectory_id), 1);
}
map_builder_bridge_.FinishTrajectory(trajectory_id);
trajectories_scheduled_for_finish_.emplace(trajectory_id);
const std::string message =
"Finished trajectory " + std::to_string(trajectory_id) + ".";
status_response.code = cartographer_ros_msgs::StatusCode::OK;

1
cartographer_ros/cartographer_ros/node.h
View File

@ -213,6 +213,7 @@ class Node {
std::unordered_map<int, TrajectorySensorSamplers> sensor_samplers_;
std::unordered_map<int, std::vector<Subscriber>> subscribers_;
std::unordered_set<std::string> subscribed_topics_;
std::unordered_set<int> trajectories_scheduled_for_finish_;
// We have to keep the timer handles of ::ros::WallTimers around, otherwise
// they do not fire.