From 0aa6ecc40fbcc585b3ca7abdef0c7b1b0629e488 Mon Sep 17 00:00:00 2001 From: Sebastian Klose Date: Fri, 6 Jul 2018 15:47:54 +0200 Subject: [PATCH] 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 ``` --- cartographer_ros/cartographer_ros/node.cc | 9 +++++++++ cartographer_ros/cartographer_ros/node.h | 1 + 2 files changed, 10 insertions(+) diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index ef38adc..90f03bc 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -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; diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index 2b4bbc1..2e9e844 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -213,6 +213,7 @@ class Node { std::unordered_map sensor_samplers_; std::unordered_map> subscribers_; std::unordered_set subscribed_topics_; + std::unordered_set trajectories_scheduled_for_finish_; // We have to keep the timer handles of ::ros::WallTimers around, otherwise // they do not fire.