From d14d0e2b90d9da0569937eaf4fd855696c72240c Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Thu, 5 Apr 2018 22:10:22 +0200 Subject: [PATCH] Check service status code in start_trajectory_main.cc (#808) Small patch to distinguish between communication and runtime errors when calling the ROS service (as introduced by RFC 13). --- .../cartographer_ros/start_trajectory_main.cc | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/cartographer_ros/cartographer_ros/start_trajectory_main.cc b/cartographer_ros/cartographer_ros/start_trajectory_main.cc index ca3d916..2930c68 100644 --- a/cartographer_ros/cartographer_ros/start_trajectory_main.cc +++ b/cartographer_ros/cartographer_ros/start_trajectory_main.cc @@ -25,6 +25,7 @@ #include "cartographer_ros/ros_log_sink.h" #include "cartographer_ros/trajectory_options.h" #include "cartographer_ros_msgs/StartTrajectory.h" +#include "cartographer_ros_msgs/StatusCode.h" #include "cartographer_ros_msgs/TrajectoryOptions.h" #include "gflags/gflags.h" #include "ros/ros.h" @@ -85,7 +86,14 @@ bool Run() { node_handle.resolveName(kOdometryTopic, true); if (!client.call(srv)) { - LOG(ERROR) << "Error starting trajectory."; + LOG(ERROR) << "Failed to call " << kStartTrajectoryServiceName << "."; + return false; + } + if (srv.response.status.code != cartographer_ros_msgs::StatusCode::OK) { + LOG(ERROR) << "Error starting trajectory - message: '" + << srv.response.status.message + << "' (status code: " << std::to_string(srv.response.status.code) + << ")."; return false; } LOG(INFO) << "Started trajectory " << srv.response.trajectory_id;