diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index a021ad9..87ee608 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -21,6 +21,8 @@ #include "cartographer/io/proto_stream.h" #include "cartographer/mapping/pose_graph.h" #include "cartographer_ros/msg_conversion.h" +#include "cartographer_ros_msgs/StatusCode.h" +#include "cartographer_ros_msgs/StatusResponse.h" namespace cartographer_ros { @@ -118,13 +120,13 @@ void MapBuilderBridge::RunFinalOptimization() { map_builder_->pose_graph()->RunFinalOptimization(); } -void MapBuilderBridge::SerializeState(const std::string& filename) { +bool MapBuilderBridge::SerializeState(const std::string& filename) { cartographer::io::ProtoStreamWriter writer(filename); map_builder_->SerializeState(&writer); - CHECK(writer.Close()) << "Could not write state."; + return writer.Close(); } -bool MapBuilderBridge::HandleSubmapQuery( +void MapBuilderBridge::HandleSubmapQuery( cartographer_ros_msgs::SubmapQuery::Request& request, cartographer_ros_msgs::SubmapQuery::Response& response) { cartographer::mapping::proto::SubmapQuery::Response response_proto; @@ -134,7 +136,9 @@ bool MapBuilderBridge::HandleSubmapQuery( map_builder_->SubmapToProto(submap_id, &response_proto); if (!error.empty()) { LOG(ERROR) << error; - return false; + response.status.code = cartographer_ros_msgs::StatusCode::NOT_FOUND; + response.status.message = error; + return; } CHECK(response_proto.textures_size() > 0) @@ -152,7 +156,8 @@ bool MapBuilderBridge::HandleSubmapQuery( texture.slice_pose = ToGeometryMsgPose( cartographer::transform::ToRigid3(texture_proto.slice_pose())); } - return true; + response.status.message = "Success."; + response.status.code = cartographer_ros_msgs::StatusCode::OK; } cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() { diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.h b/cartographer_ros/cartographer_ros/map_builder_bridge.h index 605559a..f53d742 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.h +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.h @@ -71,9 +71,9 @@ class MapBuilderBridge { const TrajectoryOptions& trajectory_options); void FinishTrajectory(int trajectory_id); void RunFinalOptimization(); - void SerializeState(const std::string& filename); + bool SerializeState(const std::string& filename); - bool HandleSubmapQuery( + void HandleSubmapQuery( cartographer_ros_msgs::SubmapQuery::Request& request, cartographer_ros_msgs::SubmapQuery::Response& response); diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 9363abc..47ce8df 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -34,6 +34,8 @@ #include "cartographer_ros/sensor_bridge.h" #include "cartographer_ros/tf_bridge.h" #include "cartographer_ros/time_conversion.h" +#include "cartographer_ros_msgs/StatusCode.h" +#include "cartographer_ros_msgs/StatusResponse.h" #include "glog/logging.h" #include "nav_msgs/Odometry.h" #include "ros/serialization.h" @@ -130,7 +132,8 @@ bool Node::HandleSubmapQuery( ::cartographer_ros_msgs::SubmapQuery::Request& request, ::cartographer_ros_msgs::SubmapQuery::Response& response) { carto::common::MutexLocker lock(&mutex_); - return map_builder_bridge_.HandleSubmapQuery(request, response); + map_builder_bridge_.HandleSubmapQuery(request, response); + return true; } void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) { @@ -400,15 +403,25 @@ bool Node::ValidateTopicNames( return true; } -bool Node::FinishTrajectoryUnderLock(const int trajectory_id) { +cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock( + const int trajectory_id) { + cartographer_ros_msgs::StatusResponse status_response; if (is_active_trajectory_.count(trajectory_id) == 0) { - LOG(INFO) << "Trajectory_id " << trajectory_id << " is not created yet."; - return false; + const std::string error = + "Trajectory " + std::to_string(trajectory_id) + " is not created yet."; + LOG(INFO) << error; + status_response.code = cartographer_ros_msgs::StatusCode::NOT_FOUND; + status_response.message = error; + return status_response; } if (!is_active_trajectory_[trajectory_id]) { - LOG(INFO) << "Trajectory_id " << trajectory_id - << " has already been finished."; - return false; + const std::string error = "Trajectory " + std::to_string(trajectory_id) + + " has already been finished."; + LOG(INFO) << error; + status_response.code = + cartographer_ros_msgs::StatusCode::RESOURCE_EXHAUSTED; + status_response.message = error; + return status_response; } // Shutdown the subscribers of this trajectory. @@ -421,7 +434,11 @@ bool Node::FinishTrajectoryUnderLock(const int trajectory_id) { CHECK(is_active_trajectory_.at(trajectory_id)); map_builder_bridge_.FinishTrajectory(trajectory_id); is_active_trajectory_[trajectory_id] = false; - return true; + const std::string message = + "Finished trajectory " + std::to_string(trajectory_id) + "."; + status_response.code = cartographer_ros_msgs::StatusCode::OK; + status_response.message = message; + return status_response; } bool Node::HandleStartTrajectory( @@ -431,14 +448,20 @@ bool Node::HandleStartTrajectory( TrajectoryOptions options; if (!FromRosMessage(request.options, &options) || !ValidateTrajectoryOptions(options)) { - LOG(ERROR) << "Invalid trajectory options."; - return false; + const std::string error = "Invalid trajectory options."; + LOG(ERROR) << error; + response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT; + response.status.message = error; + } else if (!ValidateTopicNames(request.topics, options)) { + const std::string error = "Invalid topics."; + LOG(ERROR) << error; + response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT; + response.status.message = error; + } else { + response.trajectory_id = AddTrajectory(options, request.topics); + response.status.code = cartographer_ros_msgs::StatusCode::OK; + response.status.message = "Success."; } - if (!ValidateTopicNames(request.topics, options)) { - LOG(ERROR) << "Invalid topics."; - return false; - } - response.trajectory_id = AddTrajectory(options, request.topics); return true; } @@ -486,14 +509,21 @@ bool Node::HandleFinishTrajectory( ::cartographer_ros_msgs::FinishTrajectory::Request& request, ::cartographer_ros_msgs::FinishTrajectory::Response& response) { carto::common::MutexLocker lock(&mutex_); - return FinishTrajectoryUnderLock(request.trajectory_id); + response.status = FinishTrajectoryUnderLock(request.trajectory_id); + return true; } bool Node::HandleWriteState( ::cartographer_ros_msgs::WriteState::Request& request, ::cartographer_ros_msgs::WriteState::Response& response) { carto::common::MutexLocker lock(&mutex_); - map_builder_bridge_.SerializeState(request.filename); + if (map_builder_bridge_.SerializeState(request.filename)) { + response.status.code = cartographer_ros_msgs::StatusCode::OK; + response.status.message = "State written to '" + request.filename + "'."; + } else { + response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT; + response.status.message = "Failed to write '" + request.filename + "'."; + } return true; } @@ -502,14 +532,16 @@ void Node::FinishAllTrajectories() { for (auto& entry : is_active_trajectory_) { const int trajectory_id = entry.first; if (entry.second) { - CHECK(FinishTrajectoryUnderLock(trajectory_id)); + CHECK_EQ(FinishTrajectoryUnderLock(trajectory_id).code, + cartographer_ros_msgs::StatusCode::OK); } } } bool Node::FinishTrajectory(const int trajectory_id) { carto::common::MutexLocker lock(&mutex_); - return FinishTrajectoryUnderLock(trajectory_id); + return FinishTrajectoryUnderLock(trajectory_id).code == + cartographer_ros_msgs::StatusCode::OK; } void Node::RunFinalOptimization() { @@ -600,7 +632,8 @@ void Node::HandlePointCloud2Message( void Node::SerializeState(const std::string& filename) { carto::common::MutexLocker lock(&mutex_); - map_builder_bridge_.SerializeState(filename); + CHECK(map_builder_bridge_.SerializeState(filename)) + << "Could not write state."; } void Node::LoadMap(const std::string& map_filename) { diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index 1ec6623..ebf4a7a 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -35,6 +35,7 @@ #include "cartographer_ros_msgs/FinishTrajectory.h" #include "cartographer_ros_msgs/SensorTopics.h" #include "cartographer_ros_msgs/StartTrajectory.h" +#include "cartographer_ros_msgs/StatusResponse.h" #include "cartographer_ros_msgs/SubmapEntry.h" #include "cartographer_ros_msgs/SubmapList.h" #include "cartographer_ros_msgs/SubmapQuery.h" @@ -155,7 +156,8 @@ class Node { bool ValidateTrajectoryOptions(const TrajectoryOptions& options); bool ValidateTopicNames(const ::cartographer_ros_msgs::SensorTopics& topics, const TrajectoryOptions& options); - bool FinishTrajectoryUnderLock(int trajectory_id) REQUIRES(mutex_); + cartographer_ros_msgs::StatusResponse FinishTrajectoryUnderLock( + int trajectory_id) REQUIRES(mutex_); const NodeOptions node_options_; diff --git a/cartographer_ros_msgs/CMakeLists.txt b/cartographer_ros_msgs/CMakeLists.txt index 2cb8bd0..d5d680e 100644 --- a/cartographer_ros_msgs/CMakeLists.txt +++ b/cartographer_ros_msgs/CMakeLists.txt @@ -24,6 +24,8 @@ find_package(catkin REQUIRED COMPONENTS message_generation ${PACKAGE_DEPENDENCIE add_message_files( FILES + StatusCode.msg + StatusResponse.msg SubmapList.msg SubmapEntry.msg SubmapTexture.msg diff --git a/cartographer_ros_msgs/msg/StatusCode.msg b/cartographer_ros_msgs/msg/StatusCode.msg new file mode 100644 index 0000000..bd0005d --- /dev/null +++ b/cartographer_ros_msgs/msg/StatusCode.msg @@ -0,0 +1,32 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Definition of status code constants equivalent to the gRPC API. +# https://developers.google.com/maps-booking/reference/grpc-api/status_codes +uint8 OK=0 +uint8 CANCELLED=1 +uint8 UNKNOWN=2 +uint8 INVALID_ARGUMENT=3 +uint8 DEADLINE_EXCEEDED=4 +uint8 NOT_FOUND=5 +uint8 ALREADY_EXISTS=6 +uint8 PERMISSION_DENIED=7 +uint8 RESOURCE_EXHAUSTED=8 +uint8 FAILED_PRECONDITION=9 +uint8 ABORTED=10 +uint8 OUT_OF_RANGE=11 +uint8 UNIMPLEMENTED=12 +uint8 INTERNAL=13 +uint8 UNAVAILABLE=14 +uint8 DATA_LOSS=15 diff --git a/cartographer_ros_msgs/msg/StatusResponse.msg b/cartographer_ros_msgs/msg/StatusResponse.msg new file mode 100644 index 0000000..5b79139 --- /dev/null +++ b/cartographer_ros_msgs/msg/StatusResponse.msg @@ -0,0 +1,17 @@ +# Copyright 2018 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# A common message type to indicate the outcome of a service call. +uint8 code +string message diff --git a/cartographer_ros_msgs/srv/FinishTrajectory.srv b/cartographer_ros_msgs/srv/FinishTrajectory.srv index aee0be1..6e7d0c2 100644 --- a/cartographer_ros_msgs/srv/FinishTrajectory.srv +++ b/cartographer_ros_msgs/srv/FinishTrajectory.srv @@ -14,3 +14,4 @@ int32 trajectory_id --- +cartographer_ros_msgs/StatusResponse status diff --git a/cartographer_ros_msgs/srv/StartTrajectory.srv b/cartographer_ros_msgs/srv/StartTrajectory.srv index 97c70aa..98e245a 100644 --- a/cartographer_ros_msgs/srv/StartTrajectory.srv +++ b/cartographer_ros_msgs/srv/StartTrajectory.srv @@ -15,4 +15,5 @@ cartographer_ros_msgs/TrajectoryOptions options cartographer_ros_msgs/SensorTopics topics --- +cartographer_ros_msgs/StatusResponse status int32 trajectory_id diff --git a/cartographer_ros_msgs/srv/SubmapQuery.srv b/cartographer_ros_msgs/srv/SubmapQuery.srv index 0f40dcc..5553246 100644 --- a/cartographer_ros_msgs/srv/SubmapQuery.srv +++ b/cartographer_ros_msgs/srv/SubmapQuery.srv @@ -15,6 +15,6 @@ int32 trajectory_id int32 submap_index --- +cartographer_ros_msgs/StatusResponse status int32 submap_version SubmapTexture[] textures -string error_message diff --git a/cartographer_ros_msgs/srv/WriteState.srv b/cartographer_ros_msgs/srv/WriteState.srv index fdbac78..cb7c1c8 100644 --- a/cartographer_ros_msgs/srv/WriteState.srv +++ b/cartographer_ros_msgs/srv/WriteState.srv @@ -14,3 +14,4 @@ string filename --- +cartographer_ros_msgs/StatusResponse status diff --git a/docs/source/ros_api.rst b/docs/source/ros_api.rst index 1632377..ad183de 100644 --- a/docs/source/ros_api.rst +++ b/docs/source/ros_api.rst @@ -96,6 +96,11 @@ submap_list (`cartographer_ros_msgs/SubmapList`_) Services -------- +All services responses include also a ``StatusResponse`` that comprises a ``code`` and a ``message`` field. +For consistency, the integer ``code`` is equivalent to the status codes used in the `gRPC`_ API. + +.. _gRPC: https://developers.google.com/maps-booking/reference/grpc-api/status_codes + submap_query (`cartographer_ros_msgs/SubmapQuery`_) Fetches the requested submap.