From 2441fb585419a7dbda23c44492df02f6287d50b4 Mon Sep 17 00:00:00 2001 From: Yutaka Takaoka Date: Fri, 2 Jun 2017 04:03:45 -0700 Subject: [PATCH] Added error handling on start trajectory service (#358) - Changed logging to using glog. --- cartographer_ros/cartographer_ros/node.cc | 123 +++++++++++++++++----- cartographer_ros/cartographer_ros/node.h | 3 + 2 files changed, 98 insertions(+), 28 deletions(-) diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 57c22fa..4311a9d 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -52,22 +52,23 @@ namespace { constexpr int kInfiniteSubscriberQueueSize = 0; constexpr int kLatestOnlyPublisherQueueSize = 1; -TrajectoryOptions ToTrajectoryOptions( - cartographer_ros_msgs::TrajectoryOptions ros_options) { - TrajectoryOptions options; - options.tracking_frame = ros_options.tracking_frame; - options.published_frame = ros_options.published_frame; - options.odom_frame = ros_options.odom_frame; - options.provide_odom_frame = ros_options.provide_odom_frame; - options.use_odometry = ros_options.use_odometry; - options.use_laser_scan = ros_options.use_laser_scan; - options.use_multi_echo_laser_scan = ros_options.use_multi_echo_laser_scan; - options.num_point_clouds = ros_options.num_point_clouds; - if (!options.trajectory_builder_options.ParseFromString( - ros_options.trajectory_builder_options_proto)) { - ROS_ERROR("Failed to parse protobuf"); +// Try to convert 'msg' into 'options'. Returns false on failure. +bool FromRosMessage(const cartographer_ros_msgs::TrajectoryOptions& msg, + TrajectoryOptions* options) { + options->tracking_frame = msg.tracking_frame; + options->published_frame = msg.published_frame; + options->odom_frame = msg.odom_frame; + options->provide_odom_frame = msg.provide_odom_frame; + options->use_odometry = msg.use_odometry; + options->use_laser_scan = msg.use_laser_scan; + options->use_multi_echo_laser_scan = msg.use_multi_echo_laser_scan; + options->num_point_clouds = msg.num_point_clouds; + if (!options->trajectory_builder_options.ParseFromString( + msg.trajectory_builder_options_proto)) { + LOG(ERROR) << "Failed to parse protobuf"; + return false; } - return options; + return true; } void ShutdownSubscriber(std::unordered_map& subscribers, @@ -76,11 +77,23 @@ void ShutdownSubscriber(std::unordered_map& subscribers, return; } subscribers[trajectory_id].shutdown(); - ROS_INFO_STREAM("Shutdown the subscriber of [" - << subscribers[trajectory_id].getTopic() << "]"); + LOG(INFO) << "Shutdown the subscriber of [" + << subscribers[trajectory_id].getTopic() << "]"; CHECK_EQ(subscribers.erase(trajectory_id), 1); } +bool IsTopicNameUnique( + const string& topic, + const std::unordered_map& subscribers) { + for (auto& entry : subscribers) { + if (entry.second.getTopic() == topic) { + LOG(ERROR) << "Topic name [" << topic << "] is already used."; + return false; + } + } + return true; +} + } // namespace Node::Node(const NodeOptions& node_options, tf2_ros::Buffer* const tf_buffer) @@ -230,10 +243,6 @@ int Node::AddTrajectory(const TrajectoryOptions& options, const cartographer_ros_msgs::SensorTopics& topics) { std::unordered_set expected_sensor_ids; - if (node_options_.map_builder_options.use_trajectory_builder_2d()) { - // Using point clouds is only supported in 3D. - CHECK_EQ(options.num_point_clouds, 0); - } if (options.use_laser_scan) { expected_sensor_ids.insert(topics.laser_scan_topic); } @@ -337,12 +346,72 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options, is_active_trajectory_[trajectory_id] = true; } +bool Node::ValidateTrajectoryOptions(const TrajectoryOptions& options) { + if (node_options_.map_builder_options.use_trajectory_builder_2d() && + options.trajectory_builder_options.has_trajectory_builder_2d_options()) { + // Using point clouds is only supported in 3D. + if (options.num_point_clouds == 0) { + return true; + } + } + if (node_options_.map_builder_options.use_trajectory_builder_3d() && + options.trajectory_builder_options.has_trajectory_builder_3d_options()) { + if (options.num_point_clouds != 0) { + return true; + } + } + return false; +} + +bool Node::ValidateTopicName( + const ::cartographer_ros_msgs::SensorTopics& topics, + const TrajectoryOptions& options) { + if (!IsTopicNameUnique(topics.laser_scan_topic, laser_scan_subscribers_)) { + return false; + } + if (!IsTopicNameUnique(topics.multi_echo_laser_scan_topic, + multi_echo_laser_scan_subscribers_)) { + return false; + } + if (!IsTopicNameUnique(topics.imu_topic, imu_subscribers_)) { + return false; + } + if (!IsTopicNameUnique(topics.odometry_topic, odom_subscribers_)) { + return false; + } + for (auto& subscribers : point_cloud_subscribers_) { + string topic = topics.point_cloud2_topic; + int count = 0; + for (auto& subscriber : subscribers.second) { + if (options.num_point_clouds > 1) { + topic += "_" + std::to_string(count + 1); + ++count; + } + if (subscriber.getTopic() == topic) { + LOG(ERROR) << "Topic name [" << topic << "] is already used"; + return false; + } + } + } + return true; +} + bool Node::HandleStartTrajectory( ::cartographer_ros_msgs::StartTrajectory::Request& request, ::cartographer_ros_msgs::StartTrajectory::Response& response) { carto::common::MutexLocker lock(&mutex_); + TrajectoryOptions options; + if (!FromRosMessage(request.options, &options) || + !Node::ValidateTrajectoryOptions(options)) { + LOG(ERROR) << "Invalid trajectory options."; + return false; + } + if (!Node::ValidateTopicName(request.topics, options)) { + LOG(ERROR) << "Invalid topics."; + return false; + } + std::unordered_set expected_sensor_ids; - TrajectoryOptions options = ToTrajectoryOptions(request.options); const int trajectory_id = AddTrajectory(options, request.topics); LaunchSubscribers(options, request.topics, trajectory_id); @@ -369,13 +438,12 @@ bool Node::HandleFinishTrajectory( carto::common::MutexLocker lock(&mutex_); const int trajectory_id = request.trajectory_id; if (is_active_trajectory_.count(trajectory_id) == 0) { - ROS_INFO_STREAM("Trajectory_id " << trajectory_id - << " is not created yet."); + LOG(INFO) << "Trajectory_id " << trajectory_id << " is not created yet."; return false; } if (!is_active_trajectory_[trajectory_id]) { - ROS_INFO_STREAM("Trajectory_id " << trajectory_id - << " has already been finished."); + LOG(INFO) << "Trajectory_id " << trajectory_id + << " has already been finished."; return false; } @@ -386,8 +454,7 @@ bool Node::HandleFinishTrajectory( if (point_cloud_subscribers_.count(trajectory_id) != 0) { for (auto& entry : point_cloud_subscribers_[trajectory_id]) { - ROS_INFO_STREAM("Shutdown the subscriber of [" << entry.getTopic() - << "]"); + LOG(INFO) << "Shutdown the subscriber of [" << entry.getTopic() << "]"; entry.shutdown(); } CHECK_EQ(point_cloud_subscribers_.erase(trajectory_id), 1); diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index 8484754..9dc2acd 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -91,6 +91,9 @@ class Node { void PublishSubmapList(const ::ros::WallTimerEvent& timer_event); void PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event); void SpinOccupancyGridThreadForever(); + bool ValidateTrajectoryOptions(const TrajectoryOptions& options); + bool ValidateTopicName(const ::cartographer_ros_msgs::SensorTopics& topics, + const TrajectoryOptions& options); const NodeOptions node_options_;