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

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).
master
Michael Grupp 2018-04-05 22:10:22 +02:00 committed by Wally B. Feed
parent 4e0fbcb38b
commit d14d0e2b90
1 changed files with 9 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
cartographer_ros/cartographer_ros/start_trajectory_main.cc
View File

@ -25,6 +25,7 @@
#include "cartographer_ros/ros_log_sink.h" #include "cartographer_ros/ros_log_sink.h"
#include "cartographer_ros/trajectory_options.h" #include "cartographer_ros/trajectory_options.h"
#include "cartographer_ros_msgs/StartTrajectory.h" #include "cartographer_ros_msgs/StartTrajectory.h"
#include "cartographer_ros_msgs/StatusCode.h"
#include "cartographer_ros_msgs/TrajectoryOptions.h" #include "cartographer_ros_msgs/TrajectoryOptions.h"
#include "gflags/gflags.h" #include "gflags/gflags.h"
#include "ros/ros.h" #include "ros/ros.h"
@ -85,7 +86,14 @@ bool Run() {
node_handle.resolveName(kOdometryTopic, true); node_handle.resolveName(kOdometryTopic, true);
if (!client.call(srv)) { 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; return false;
} }
LOG(INFO) << "Started trajectory " << srv.response.trajectory_id; LOG(INFO) << "Started trajectory " << srv.response.trajectory_id;