From 5fb4314c8b7f235af121c2bce2a5761e0adb8651 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Mon, 15 Apr 2019 09:53:55 +0200 Subject: [PATCH] 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 | 95 +++++++++++++---------- cartographer_ros/cartographer_ros/node.h | 6 ++ 2 files changed, 58 insertions(+), 43 deletions(-) diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 48a68c3..437bad4 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/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 })); } +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 map_builder, @@ -466,50 +480,46 @@ bool Node::ValidateTopicNames( return true; } +cartographer_ros_msgs::StatusResponse Node::TrajectoryStateToStatus( + const int trajectory_id, const std::set& 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, - " already pending to finish."); + 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) << message; + LOG(INFO) << status_response.message; return status_response; } // First, check if we can actually finish the trajectory. - if (!(trajectory_states.count(trajectory_id))) { - const std::string error = - absl::StrCat("Trajectory ", trajectory_id, " doesn't exist."); - LOG(ERROR) << error; - 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; + status_response = TrajectoryStateToStatus( + trajectory_id, {TrajectoryState::ACTIVE} /* valid states */); + if (status_response.code != cartographer_ros_msgs::StatusCode::OK) { + LOG(ERROR) << "Can't finish trajectory: " << status_response.message; 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; } diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index 62eb6f9..d55749f 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/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_;