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/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();

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_);

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;