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).
master
Michael Grupp 2018-02-06 18:06:08 +01:00 committed by Wally B. Feed
parent 8f14350e83
commit 3ca30fc904
12 changed files with 128 additions and 29 deletions

View File

@ -21,6 +21,8 @@
#include "cartographer/io/proto_stream.h" #include "cartographer/io/proto_stream.h"
#include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/pose_graph.h"
#include "cartographer_ros/msg_conversion.h" #include "cartographer_ros/msg_conversion.h"
#include "cartographer_ros_msgs/StatusCode.h"
#include "cartographer_ros_msgs/StatusResponse.h"
namespace cartographer_ros { namespace cartographer_ros {
@ -118,13 +120,13 @@ void MapBuilderBridge::RunFinalOptimization() {
map_builder_->pose_graph()->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); cartographer::io::ProtoStreamWriter writer(filename);
map_builder_->SerializeState(&writer); 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::Request& request,
cartographer_ros_msgs::SubmapQuery::Response& response) { cartographer_ros_msgs::SubmapQuery::Response& response) {
cartographer::mapping::proto::SubmapQuery::Response response_proto; cartographer::mapping::proto::SubmapQuery::Response response_proto;
@ -134,7 +136,9 @@ bool MapBuilderBridge::HandleSubmapQuery(
map_builder_->SubmapToProto(submap_id, &response_proto); map_builder_->SubmapToProto(submap_id, &response_proto);
if (!error.empty()) { if (!error.empty()) {
LOG(ERROR) << error; 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) CHECK(response_proto.textures_size() > 0)
@ -152,7 +156,8 @@ bool MapBuilderBridge::HandleSubmapQuery(
texture.slice_pose = ToGeometryMsgPose( texture.slice_pose = ToGeometryMsgPose(
cartographer::transform::ToRigid3(texture_proto.slice_pose())); 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_msgs::SubmapList MapBuilderBridge::GetSubmapList() {

View File

@ -71,9 +71,9 @@ class MapBuilderBridge {
const TrajectoryOptions& trajectory_options); const TrajectoryOptions& trajectory_options);
void FinishTrajectory(int trajectory_id); void FinishTrajectory(int trajectory_id);
void RunFinalOptimization(); 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::Request& request,
cartographer_ros_msgs::SubmapQuery::Response& response); cartographer_ros_msgs::SubmapQuery::Response& response);

View File

@ -34,6 +34,8 @@
#include "cartographer_ros/sensor_bridge.h" #include "cartographer_ros/sensor_bridge.h"
#include "cartographer_ros/tf_bridge.h" #include "cartographer_ros/tf_bridge.h"
#include "cartographer_ros/time_conversion.h" #include "cartographer_ros/time_conversion.h"
#include "cartographer_ros_msgs/StatusCode.h"
#include "cartographer_ros_msgs/StatusResponse.h"
#include "glog/logging.h" #include "glog/logging.h"
#include "nav_msgs/Odometry.h" #include "nav_msgs/Odometry.h"
#include "ros/serialization.h" #include "ros/serialization.h"
@ -130,7 +132,8 @@ bool Node::HandleSubmapQuery(
::cartographer_ros_msgs::SubmapQuery::Request& request, ::cartographer_ros_msgs::SubmapQuery::Request& request,
::cartographer_ros_msgs::SubmapQuery::Response& response) { ::cartographer_ros_msgs::SubmapQuery::Response& response) {
carto::common::MutexLocker lock(&mutex_); 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) { void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) {
@ -400,15 +403,25 @@ bool Node::ValidateTopicNames(
return true; 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) { 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]) { 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. // Shutdown the subscribers of this trajectory.
@ -421,7 +434,11 @@ bool Node::FinishTrajectoryUnderLock(const int trajectory_id) {
CHECK(is_active_trajectory_.at(trajectory_id)); CHECK(is_active_trajectory_.at(trajectory_id));
map_builder_bridge_.FinishTrajectory(trajectory_id); map_builder_bridge_.FinishTrajectory(trajectory_id);
is_active_trajectory_[trajectory_id] = false; 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( bool Node::HandleStartTrajectory(
@ -431,14 +448,20 @@ bool Node::HandleStartTrajectory(
TrajectoryOptions options; TrajectoryOptions options;
if (!FromRosMessage(request.options, &options) || if (!FromRosMessage(request.options, &options) ||
!ValidateTrajectoryOptions(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;
if (!ValidateTopicNames(request.topics, options)) { response.status.message = error;
LOG(ERROR) << "Invalid topics."; } else if (!ValidateTopicNames(request.topics, options)) {
return false; 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.trajectory_id = AddTrajectory(options, request.topics);
response.status.code = cartographer_ros_msgs::StatusCode::OK;
response.status.message = "Success.";
}
return true; return true;
} }
@ -486,14 +509,21 @@ bool Node::HandleFinishTrajectory(
::cartographer_ros_msgs::FinishTrajectory::Request& request, ::cartographer_ros_msgs::FinishTrajectory::Request& request,
::cartographer_ros_msgs::FinishTrajectory::Response& response) { ::cartographer_ros_msgs::FinishTrajectory::Response& response) {
carto::common::MutexLocker lock(&mutex_); carto::common::MutexLocker lock(&mutex_);
return FinishTrajectoryUnderLock(request.trajectory_id); response.status = FinishTrajectoryUnderLock(request.trajectory_id);
return true;
} }
bool Node::HandleWriteState( bool Node::HandleWriteState(
::cartographer_ros_msgs::WriteState::Request& request, ::cartographer_ros_msgs::WriteState::Request& request,
::cartographer_ros_msgs::WriteState::Response& response) { ::cartographer_ros_msgs::WriteState::Response& response) {
carto::common::MutexLocker lock(&mutex_); 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; return true;
} }
@ -502,14 +532,16 @@ void Node::FinishAllTrajectories() {
for (auto& entry : is_active_trajectory_) { for (auto& entry : is_active_trajectory_) {
const int trajectory_id = entry.first; const int trajectory_id = entry.first;
if (entry.second) { 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) { bool Node::FinishTrajectory(const int trajectory_id) {
carto::common::MutexLocker lock(&mutex_); carto::common::MutexLocker lock(&mutex_);
return FinishTrajectoryUnderLock(trajectory_id); return FinishTrajectoryUnderLock(trajectory_id).code ==
cartographer_ros_msgs::StatusCode::OK;
} }
void Node::RunFinalOptimization() { void Node::RunFinalOptimization() {
@ -600,7 +632,8 @@ void Node::HandlePointCloud2Message(
void Node::SerializeState(const std::string& filename) { void Node::SerializeState(const std::string& filename) {
carto::common::MutexLocker lock(&mutex_); 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) { void Node::LoadMap(const std::string& map_filename) {

View File

@ -35,6 +35,7 @@
#include "cartographer_ros_msgs/FinishTrajectory.h" #include "cartographer_ros_msgs/FinishTrajectory.h"
#include "cartographer_ros_msgs/SensorTopics.h" #include "cartographer_ros_msgs/SensorTopics.h"
#include "cartographer_ros_msgs/StartTrajectory.h" #include "cartographer_ros_msgs/StartTrajectory.h"
#include "cartographer_ros_msgs/StatusResponse.h"
#include "cartographer_ros_msgs/SubmapEntry.h" #include "cartographer_ros_msgs/SubmapEntry.h"
#include "cartographer_ros_msgs/SubmapList.h" #include "cartographer_ros_msgs/SubmapList.h"
#include "cartographer_ros_msgs/SubmapQuery.h" #include "cartographer_ros_msgs/SubmapQuery.h"
@ -155,7 +156,8 @@ class Node {
bool ValidateTrajectoryOptions(const TrajectoryOptions& options); bool ValidateTrajectoryOptions(const TrajectoryOptions& options);
bool ValidateTopicNames(const ::cartographer_ros_msgs::SensorTopics& topics, bool ValidateTopicNames(const ::cartographer_ros_msgs::SensorTopics& topics,
const TrajectoryOptions& options); const TrajectoryOptions& options);
bool FinishTrajectoryUnderLock(int trajectory_id) REQUIRES(mutex_); cartographer_ros_msgs::StatusResponse FinishTrajectoryUnderLock(
int trajectory_id) REQUIRES(mutex_);
const NodeOptions node_options_; const NodeOptions node_options_;

View File

@ -24,6 +24,8 @@ find_package(catkin REQUIRED COMPONENTS message_generation ${PACKAGE_DEPENDENCIE
add_message_files( add_message_files(
FILES FILES
StatusCode.msg
StatusResponse.msg
SubmapList.msg SubmapList.msg
SubmapEntry.msg SubmapEntry.msg
SubmapTexture.msg SubmapTexture.msg

View File

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

View File

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

View File

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

View File

@ -15,4 +15,5 @@
cartographer_ros_msgs/TrajectoryOptions options cartographer_ros_msgs/TrajectoryOptions options
cartographer_ros_msgs/SensorTopics topics cartographer_ros_msgs/SensorTopics topics
--- ---
cartographer_ros_msgs/StatusResponse status
int32 trajectory_id int32 trajectory_id

View File

@ -15,6 +15,6 @@
int32 trajectory_id int32 trajectory_id
int32 submap_index int32 submap_index
--- ---
cartographer_ros_msgs/StatusResponse status
int32 submap_version int32 submap_version
SubmapTexture[] textures SubmapTexture[] textures
string error_message

View File

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

View File

@ -96,6 +96,11 @@ submap_list (`cartographer_ros_msgs/SubmapList`_)
Services 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`_) submap_query (`cartographer_ros_msgs/SubmapQuery`_)
Fetches the requested submap. Fetches the requested submap.