Refactor ROS service responses. (#708)

Provide a descriptive StatusResponse msg field consisting of
an gRPC-like StatusCode and message string to the service caller.

Implements [RFC 13](https://github.com/googlecartographer/rfcs/blob/master/text/0013-improve-ros-service-responses.md).

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() {

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

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.";
+    const std::string error =
-    return false;
+        "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
+    const std::string error = "Trajectory " + std::to_string(trajectory_id) +
-              << " has already been finished.";
+                              " has already been finished.";
-    return false;
+    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.";
+    const std::string error = "Invalid trajectory options.";
-    return false;
+    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) {

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

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

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

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

cartographer_ros_msgs/srv/FinishTrajectory.srv

@@ -14,3 +14,4 @@
 
 int32 trajectory_id
 ---
+cartographer_ros_msgs/StatusResponse status

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

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

cartographer_ros_msgs/srv/WriteState.srv

@@ -14,3 +14,4 @@
 
 string filename
 ---
+cartographer_ros_msgs/StatusResponse status

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.