From 9fb2b09a0b3d906f61cc182676eebaddb50cdafe Mon Sep 17 00:00:00 2001
From: Jihoon Lee <jihoonlee.in@gmail.com>
Date: Tue, 14 Nov 2017 11:30:39 +0100
Subject: [PATCH] Unify finish trajectory logic (#552)

Port active trajectory checks and subscriber shutdown logic into FinishTrajectory(), so that a trajectory can be cleanly finished by function call.
---
 cartographer_ros/cartographer_ros/node.cc | 55 ++++++++++++-----------
 cartographer_ros/cartographer_ros/node.h  |  6 ++-
 2 files changed, 32 insertions(+), 29 deletions(-)

diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc
index 60cde5a..4c6877e 100644
--- a/cartographer_ros/cartographer_ros/node.cc
+++ b/cartographer_ros/cartographer_ros/node.cc
@@ -375,6 +375,30 @@ bool Node::ValidateTopicNames(
   return true;
 }
 
+bool Node::FinishTrajectoryUnderLock(const int trajectory_id) {
+  if (is_active_trajectory_.count(trajectory_id) == 0) {
+    LOG(INFO) << "Trajectory_id " << trajectory_id << " is not created yet.";
+    return false;
+  }
+  if (!is_active_trajectory_[trajectory_id]) {
+    LOG(INFO) << "Trajectory_id " << trajectory_id
+              << " has already been finished.";
+    return false;
+  }
+
+  // Shutdown the subscribers of this trajectory.
+  for (auto& entry : subscribers_[trajectory_id]) {
+    entry.subscriber.shutdown();
+    subscribed_topics_.erase(entry.topic);
+    LOG(INFO) << "Shutdown the subscriber of [" << entry.topic << "]";
+  }
+  CHECK_EQ(subscribers_.erase(trajectory_id), 1);
+  CHECK(is_active_trajectory_.at(trajectory_id));
+  map_builder_bridge_.FinishTrajectory(trajectory_id);
+  is_active_trajectory_[trajectory_id] = false;
+  return true;
+}
+
 bool Node::HandleStartTrajectory(
     ::cartographer_ros_msgs::StartTrajectory::Request& request,
     ::cartographer_ros_msgs::StartTrajectory::Response& response) {
@@ -421,27 +445,7 @@ bool Node::HandleFinishTrajectory(
     ::cartographer_ros_msgs::FinishTrajectory::Request& request,
     ::cartographer_ros_msgs::FinishTrajectory::Response& response) {
   carto::common::MutexLocker lock(&mutex_);
-  const int trajectory_id = request.trajectory_id;
-  if (is_active_trajectory_.count(trajectory_id) == 0) {
-    LOG(INFO) << "Trajectory_id " << trajectory_id << " is not created yet.";
-    return false;
-  }
-  if (!is_active_trajectory_[trajectory_id]) {
-    LOG(INFO) << "Trajectory_id " << trajectory_id
-              << " has already been finished.";
-    return false;
-  }
-
-  // Shutdown the subscribers of this trajectory.
-  for (auto& entry : subscribers_[trajectory_id]) {
-    entry.subscriber.shutdown();
-    subscribed_topics_.erase(entry.topic);
-    LOG(INFO) << "Shutdown the subscriber of [" << entry.topic << "]";
-  }
-  CHECK_EQ(subscribers_.erase(trajectory_id), 1);
-  map_builder_bridge_.FinishTrajectory(trajectory_id);
-  is_active_trajectory_[trajectory_id] = false;
-  return true;
+  return FinishTrajectoryUnderLock(request.trajectory_id);
 }
 
 bool Node::HandleWriteState(
@@ -457,17 +461,14 @@ void Node::FinishAllTrajectories() {
   for (auto& entry : is_active_trajectory_) {
     const int trajectory_id = entry.first;
     if (entry.second) {
-      map_builder_bridge_.FinishTrajectory(trajectory_id);
-      entry.second = false;
+      CHECK(FinishTrajectoryUnderLock(trajectory_id));
     }
   }
 }
 
-void Node::FinishTrajectory(const int trajectory_id) {
+bool Node::FinishTrajectory(const int trajectory_id) {
   carto::common::MutexLocker lock(&mutex_);
-  CHECK(is_active_trajectory_.at(trajectory_id));
-  map_builder_bridge_.FinishTrajectory(trajectory_id);
-  is_active_trajectory_[trajectory_id] = false;
+  return FinishTrajectoryUnderLock(trajectory_id);
 }
 
 void Node::RunFinalOptimization() {
diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h
index 06b1c88..c101c5d 100644
--- a/cartographer_ros/cartographer_ros/node.h
+++ b/cartographer_ros/cartographer_ros/node.h
@@ -54,8 +54,9 @@ class Node {
 
   // Finishes all yet active trajectories.
   void FinishAllTrajectories();
-  // Finishes a single given trajectory.
-  void FinishTrajectory(int trajectory_id);
+  // Finishes a single given trajectory. Returns false if the trajectory did not
+  // exist or was already finished.
+  bool FinishTrajectory(int trajectory_id);
 
   // Runs final optimization. All trajectories have to be finished when calling.
   void RunFinalOptimization();
@@ -134,6 +135,7 @@ class Node {
   bool ValidateTrajectoryOptions(const TrajectoryOptions& options);
   bool ValidateTopicNames(const ::cartographer_ros_msgs::SensorTopics& topics,
                           const TrajectoryOptions& options);
+  bool FinishTrajectoryUnderLock(int trajectory_id) REQUIRES(mutex_);
 
   const NodeOptions node_options_;