From a375153a213662402be312a4b23da240c9efbb28 Mon Sep 17 00:00:00 2001
From: Michael Grupp <grupp@magazino.eu>
Date: Mon, 9 Apr 2018 18:59:34 +0200
Subject: [PATCH] Take frozen state into account when finishing trajectories.
 (#811)

Until now, the error response of an /finish_trajectory request for a
frozen trajectory was 'Trajectory ... is not created yet.'.

This is a lie. The new response is more accurate because the trajectory
__is__ created, but it just can't be finished because it's frozen.
---
 .../cartographer_ros/map_builder_bridge.cc         | 11 +++++++++++
 .../cartographer_ros/map_builder_bridge.h          |  1 +
 cartographer_ros/cartographer_ros/node.cc          | 14 ++++++++++++--
 3 files changed, 24 insertions(+), 2 deletions(-)

diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc
index 479ad9e..207b871 100644
--- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc
+++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc
@@ -197,6 +197,17 @@ void MapBuilderBridge::HandleSubmapQuery(
   response.status.code = cartographer_ros_msgs::StatusCode::OK;
 }
 
+std::set<int> MapBuilderBridge::GetFrozenTrajectoryIds() {
+  std::set<int> frozen_trajectory_ids;
+  const auto node_poses = map_builder_->pose_graph()->GetTrajectoryNodePoses();
+  for (const int trajectory_id : node_poses.trajectory_ids()) {
+    if (map_builder_->pose_graph()->IsTrajectoryFrozen(trajectory_id)) {
+      frozen_trajectory_ids.insert(trajectory_id);
+    }
+  }
+  return frozen_trajectory_ids;
+}
+
 cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
   cartographer_ros_msgs::SubmapList submap_list;
   submap_list.header.stamp = ::ros::Time::now();
diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.h b/cartographer_ros/cartographer_ros/map_builder_bridge.h
index 7f98766..f4dc8b9 100644
--- a/cartographer_ros/cartographer_ros/map_builder_bridge.h
+++ b/cartographer_ros/cartographer_ros/map_builder_bridge.h
@@ -77,6 +77,7 @@ class MapBuilderBridge {
       cartographer_ros_msgs::SubmapQuery::Request& request,
       cartographer_ros_msgs::SubmapQuery::Response& response);
 
+  std::set<int> GetFrozenTrajectoryIds();
   cartographer_ros_msgs::SubmapList GetSubmapList();
   std::unordered_map<int, TrajectoryState> GetTrajectoryStates()
       EXCLUDES(mutex_);
diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc
index f2e874c..84ce7d3 100644
--- a/cartographer_ros/cartographer_ros/node.cc
+++ b/cartographer_ros/cartographer_ros/node.cc
@@ -443,10 +443,20 @@ bool Node::ValidateTopicNames(
 cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock(
     const int trajectory_id) {
   cartographer_ros_msgs::StatusResponse status_response;
+
+  // First, check if we can actually finish the trajectory.
+  if (map_builder_bridge_.GetFrozenTrajectoryIds().count(trajectory_id)) {
+    const std::string error =
+        "Trajectory " + std::to_string(trajectory_id) + " is frozen.";
+    LOG(ERROR) << error;
+    status_response.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
+    status_response.message = error;
+    return status_response;
+  }
   if (is_active_trajectory_.count(trajectory_id) == 0) {
     const std::string error =
         "Trajectory " + std::to_string(trajectory_id) + " is not created yet.";
-    LOG(INFO) << error;
+    LOG(ERROR) << error;
     status_response.code = cartographer_ros_msgs::StatusCode::NOT_FOUND;
     status_response.message = error;
     return status_response;
@@ -454,7 +464,7 @@ cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock(
   if (!is_active_trajectory_[trajectory_id]) {
     const std::string error = "Trajectory " + std::to_string(trajectory_id) +
                               " has already been finished.";
-    LOG(INFO) << error;
+    LOG(ERROR) << error;
     status_response.code =
         cartographer_ros_msgs::StatusCode::RESOURCE_EXHAUSTED;
     status_response.message = error;