Unify trajectory state checks for service handlers. (#1262)

Some service handlers need to check if a trajectory is in a valid
state for the requested operation. If it's not, they return an error
response.

`Node::CheckTrajectoryState` allows to avoid code duplication in such
situations.

cartographer_ros/cartographer_ros/node.cc

@@ -49,6 +49,12 @@
 
 namespace cartographer_ros {
 
+namespace carto = ::cartographer;
+
+using carto::transform::Rigid3d;
+using TrajectoryState =
+    ::cartographer::mapping::PoseGraphInterface::TrajectoryState;
+
 namespace {
 
 cartographer_ros_msgs::SensorTopics DefaultSensorTopics() {
@@ -80,14 +86,22 @@ template <typename MessageType>
           }));
 }
 
+std::string TrajectoryStateToString(const TrajectoryState trajectory_state) {
+  switch (trajectory_state) {
+    case TrajectoryState::ACTIVE:
+      return "ACTIVE";
+    case TrajectoryState::FINISHED:
+      return "FINISHED";
+    case TrajectoryState::FROZEN:
+      return "FROZEN";
+    case TrajectoryState::DELETED:
+      return "DELETED";
+  }
+  return "";
+}
+
 }  // namespace
 
-namespace carto = ::cartographer;
-
-using carto::transform::Rigid3d;
-using TrajectoryState =
-    ::cartographer::mapping::PoseGraphInterface::TrajectoryState;
-
 Node::Node(
     const NodeOptions& node_options,
     std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
@@ -466,50 +480,46 @@ bool Node::ValidateTopicNames(
   return true;
 }
 
+cartographer_ros_msgs::StatusResponse Node::TrajectoryStateToStatus(
+    const int trajectory_id, const std::set<TrajectoryState>& valid_states) {
+  const auto trajectory_states = map_builder_bridge_.GetTrajectoryStates();
+  cartographer_ros_msgs::StatusResponse status_response;
+
+  if (!(trajectory_states.count(trajectory_id))) {
+    status_response.message =
+        absl::StrCat("Trajectory ", trajectory_id, " doesn't exist.");
+    status_response.code = cartographer_ros_msgs::StatusCode::NOT_FOUND;
+    return status_response;
+  }
+
+  const auto trajectory_state = trajectory_states.at(trajectory_id);
+  status_response.message =
+      absl::StrCat("Trajectory ", trajectory_id, " is in '",
+                   TrajectoryStateToString(trajectory_state), "' state.");
+  if (valid_states.count(trajectory_state)) {
+    status_response.code = cartographer_ros_msgs::StatusCode::OK;
+  } else {
+    status_response.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
+  }
+  return status_response;
+}
+
 cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock(
     const int trajectory_id) {
-  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 = absl::StrCat("Trajectory ", trajectory_id,
+    status_response.message = absl::StrCat("Trajectory ", trajectory_id,
                                            " already pending to finish.");
     status_response.code = cartographer_ros_msgs::StatusCode::OK;
-    status_response.message = message;
+    LOG(INFO) << status_response.message;
-    LOG(INFO) << message;
     return status_response;
   }
 
   // First, check if we can actually finish the trajectory.
-  if (!(trajectory_states.count(trajectory_id))) {
+  status_response = TrajectoryStateToStatus(
-    const std::string error =
+      trajectory_id, {TrajectoryState::ACTIVE} /* valid states */);
-        absl::StrCat("Trajectory ", trajectory_id, " doesn't exist.");
+  if (status_response.code != cartographer_ros_msgs::StatusCode::OK) {
-    LOG(ERROR) << error;
+    LOG(ERROR) << "Can't finish trajectory: " << status_response.message;
-    status_response.code = cartographer_ros_msgs::StatusCode::NOT_FOUND;
-    status_response.message = error;
-    return status_response;
-  } else if (trajectory_states.at(trajectory_id) == TrajectoryState::FROZEN) {
-    const std::string error =
-        absl::StrCat("Trajectory ", trajectory_id, " is frozen.");
-    LOG(ERROR) << error;
-    status_response.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
-    status_response.message = error;
-    return status_response;
-  } else if (trajectory_states.at(trajectory_id) == TrajectoryState::FINISHED) {
-    const std::string error = absl::StrCat("Trajectory ", trajectory_id,
-                                           " has already been finished.");
-    LOG(ERROR) << error;
-    status_response.code =
-        cartographer_ros_msgs::StatusCode::RESOURCE_EXHAUSTED;
-    status_response.message = error;
-    return status_response;
-  } else if (trajectory_states.at(trajectory_id) == TrajectoryState::DELETED) {
-    const std::string error =
-        absl::StrCat("Trajectory ", trajectory_id, " has been deleted.");
-    LOG(ERROR) << error;
-    status_response.code =
-        cartographer_ros_msgs::StatusCode::RESOURCE_EXHAUSTED;
-    status_response.message = error;
     return status_response;
   }
 
@@ -525,10 +535,9 @@ cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock(
   }
   map_builder_bridge_.FinishTrajectory(trajectory_id);
   trajectories_scheduled_for_finish_.emplace(trajectory_id);
-  const std::string message =
+  status_response.message =
       absl::StrCat("Finished trajectory ", trajectory_id, ".");
   status_response.code = cartographer_ros_msgs::StatusCode::OK;
-  status_response.message = message;
   return status_response;
 }
 

cartographer_ros/cartographer_ros/node.h

@@ -174,6 +174,12 @@ class Node {
       int trajectory_id) EXCLUSIVE_LOCKS_REQUIRED(mutex_);
   void MaybeWarnAboutTopicMismatch(const ::ros::WallTimerEvent&);
 
+  // Helper function for service handlers that need to check trajectory states.
+  cartographer_ros_msgs::StatusResponse TrajectoryStateToStatus(
+      int trajectory_id,
+      const std::set<
+          cartographer::mapping::PoseGraphInterface::TrajectoryState>&
+          valid_states);
   const NodeOptions node_options_;
 
   tf2_ros::TransformBroadcaster tf_broadcaster_;