12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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.
master
Michael Grupp 2019-04-15 09:53:55 +02:00 committed by Andre Gaschler
parent b28ffea97f
commit 5fb4314c8b
2 changed files with 58 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

93
cartographer_ros/cartographer_ros/node.cc
View File

@ -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) << 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;
}

6
cartographer_ros/cartographer_ros/node.h
View File

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