From 141bf26dbbdbd9fba111fd7e9fb31d2842138972 Mon Sep 17 00:00:00 2001 From: Damon Kohler Date: Tue, 29 Nov 2016 13:28:11 +0100 Subject: [PATCH] Pulls out sensor data wiring into main. (#198) --- .../cartographer_ros/map_builder_bridge.h | 1 - cartographer_ros/cartographer_ros/node.cc | 105 ++-------------- cartographer_ros/cartographer_ros/node.h | 13 +- .../cartographer_ros/node_main.cc | 119 +++++++++++++++++- 4 files changed, 128 insertions(+), 110 deletions(-) diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.h b/cartographer_ros/cartographer_ros/map_builder_bridge.h index 56ee1ef..901b175 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.h +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.h @@ -25,7 +25,6 @@ #include "cartographer_ros/node_options.h" #include "cartographer_ros/sensor_bridge.h" #include "cartographer_ros/tf_bridge.h" -#include "cartographer_ros_msgs/FinishTrajectory.h" #include "cartographer_ros_msgs/SubmapEntry.h" #include "cartographer_ros_msgs/SubmapList.h" #include "cartographer_ros_msgs/SubmapQuery.h" diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 7ce2f81..2de27da 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -45,21 +45,14 @@ namespace carto = ::cartographer; using carto::transform::Rigid3d; -constexpr int kInfiniteSubscriberQueueSize = 0; constexpr int kLatestOnlyPublisherQueueSize = 1; constexpr double kTfBufferCacheTimeInSeconds = 1e6; -// Unique default topic names. Expected to be remapped as needed. -constexpr char kLaserScanTopic[] = "scan"; -constexpr char kMultiEchoLaserScanTopic[] = "echoes"; -constexpr char kPointCloud2Topic[] = "points2"; -constexpr char kImuTopic[] = "imu"; -constexpr char kOdometryTopic[] = "odom"; +// Default topic names; expected to be remapped as needed. constexpr char kOccupancyGridTopic[] = "map"; constexpr char kScanMatchedPointCloudTopic[] = "scan_matched_points2"; constexpr char kSubmapListTopic[] = "submap_list"; constexpr char kSubmapQueryServiceName[] = "submap_query"; -constexpr char kFinishTrajectoryServiceName[] = "finish_trajectory"; Node::Node(const NodeOptions& options) : options_(options), @@ -70,8 +63,6 @@ Node::Node(const NodeOptions& options) Node::~Node() { { carto::common::MutexLocker lock(&mutex_); - CHECK_GE(trajectory_id_, 0); - map_builder_bridge_.FinishTrajectory(trajectory_id_); terminating_ = true; } if (occupancy_grid_thread_.joinable()) { @@ -81,77 +72,6 @@ Node::~Node() { void Node::Initialize() { carto::common::MutexLocker lock(&mutex_); - // For 2D SLAM, subscribe to exactly one horizontal laser. - if (options_.use_laser_scan) { - horizontal_laser_scan_subscriber_ = node_handle_.subscribe( - kLaserScanTopic, kInfiniteSubscriberQueueSize, - boost::function( - [this](const sensor_msgs::LaserScan::ConstPtr& msg) { - map_builder_bridge_.sensor_bridge(trajectory_id_)->HandleLaserScanMessage( - kLaserScanTopic, msg); - })); - expected_sensor_ids_.insert(kLaserScanTopic); - } - if (options_.use_multi_echo_laser_scan) { - horizontal_laser_scan_subscriber_ = node_handle_.subscribe( - kMultiEchoLaserScanTopic, kInfiniteSubscriberQueueSize, - boost::function( - [this](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { - map_builder_bridge_.sensor_bridge(trajectory_id_) - ->HandleMultiEchoLaserScanMessage(kMultiEchoLaserScanTopic, - msg); - })); - expected_sensor_ids_.insert(kMultiEchoLaserScanTopic); - } - - // For 3D SLAM, subscribe to all point clouds topics. - if (options_.num_point_clouds > 0) { - for (int i = 0; i < options_.num_point_clouds; ++i) { - string topic = kPointCloud2Topic; - if (options_.num_point_clouds > 1) { - topic += "_" + std::to_string(i + 1); - } - point_cloud_subscribers_.push_back(node_handle_.subscribe( - topic, kInfiniteSubscriberQueueSize, - boost::function( - [this, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) { - map_builder_bridge_.sensor_bridge(trajectory_id_)->HandlePointCloud2Message( - topic, msg); - }))); - expected_sensor_ids_.insert(topic); - } - } - - // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is - // required. - if (options_.map_builder_options.use_trajectory_builder_3d() || - (options_.map_builder_options.use_trajectory_builder_2d() && - options_.map_builder_options.trajectory_builder_2d_options() - .use_imu_data())) { - imu_subscriber_ = node_handle_.subscribe( - kImuTopic, kInfiniteSubscriberQueueSize, - boost::function( - [this](const sensor_msgs::Imu::ConstPtr& msg) { - map_builder_bridge_.sensor_bridge(trajectory_id_)->HandleImuMessage(kImuTopic, - msg); - })); - expected_sensor_ids_.insert(kImuTopic); - } - - if (options_.use_odometry) { - odometry_subscriber_ = node_handle_.subscribe( - kOdometryTopic, kInfiniteSubscriberQueueSize, - boost::function( - [this](const nav_msgs::Odometry::ConstPtr& msg) { - map_builder_bridge_.sensor_bridge(trajectory_id_)->HandleOdometryMessage( - kOdometryTopic, msg); - })); - expected_sensor_ids_.insert(kOdometryTopic); - } - - trajectory_id_ = map_builder_bridge_.AddTrajectory(expected_sensor_ids_, - options_.tracking_frame); - submap_list_publisher_ = node_handle_.advertise<::cartographer_ros_msgs::SubmapList>( kSubmapListTopic, kLatestOnlyPublisherQueueSize); @@ -171,9 +91,6 @@ void Node::Initialize() { node_handle_.advertise( kScanMatchedPointCloudTopic, kLatestOnlyPublisherQueueSize); - finish_trajectory_server_ = node_handle_.advertiseService( - kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this); - wall_timers_.push_back(node_handle_.createWallTimer( ::ros::WallDuration(options_.submap_publish_period_sec), &Node::PublishSubmapList, this)); @@ -182,6 +99,12 @@ void Node::Initialize() { &Node::PublishTrajectoryStates, this)); } +void Node::Spin() { ::ros::spin(); } + +::ros::NodeHandle* Node::node_handle() { return &node_handle_; } + +MapBuilderBridge* Node::map_builder_bridge() { return &map_builder_bridge_; } + bool Node::HandleSubmapQuery( ::cartographer_ros_msgs::SubmapQuery::Request& request, ::cartographer_ros_msgs::SubmapQuery::Response& response) { @@ -189,18 +112,6 @@ bool Node::HandleSubmapQuery( return map_builder_bridge_.HandleSubmapQuery(request, response); } -bool Node::HandleFinishTrajectory( - ::cartographer_ros_msgs::FinishTrajectory::Request& request, - ::cartographer_ros_msgs::FinishTrajectory::Response&) { - carto::common::MutexLocker lock(&mutex_); - const int previous_trajectory_id = trajectory_id_; - trajectory_id_ = map_builder_bridge_.AddTrajectory(expected_sensor_ids_, - options_.tracking_frame); - map_builder_bridge_.FinishTrajectory(previous_trajectory_id); - map_builder_bridge_.WriteAssets(request.stem); - return true; -} - void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) { carto::common::MutexLocker lock(&mutex_); submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList()); @@ -282,6 +193,4 @@ void Node::SpinOccupancyGridThreadForever() { } } -void Node::SpinForever() { ::ros::spin(); } - } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index 1e1ba19..a291ed0 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -43,16 +43,16 @@ class Node { Node(const Node&) = delete; Node& operator=(const Node&) = delete; - void SpinForever(); void Initialize(); + void Spin(); + + ::ros::NodeHandle* node_handle(); + MapBuilderBridge* map_builder_bridge(); private: bool HandleSubmapQuery( cartographer_ros_msgs::SubmapQuery::Request& request, cartographer_ros_msgs::SubmapQuery::Response& response); - bool HandleFinishTrajectory( - cartographer_ros_msgs::FinishTrajectory::Request& request, - cartographer_ros_msgs::FinishTrajectory::Response& response); void PublishSubmapList(const ::ros::WallTimerEvent& timer_event); void PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event); @@ -70,16 +70,11 @@ class Node { std::unordered_set expected_sensor_ids_; ::ros::NodeHandle node_handle_; - ::ros::Subscriber imu_subscriber_; - ::ros::Subscriber horizontal_laser_scan_subscriber_; - std::vector<::ros::Subscriber> point_cloud_subscribers_; - ::ros::Subscriber odometry_subscriber_; ::ros::Publisher submap_list_publisher_; ::ros::ServiceServer submap_query_server_; ::ros::Publisher scan_matched_point_cloud_publisher_; cartographer::common::Time last_scan_matched_point_cloud_time_ = cartographer::common::Time::min(); - ::ros::ServiceServer finish_trajectory_server_; ::ros::Publisher occupancy_grid_publisher_; std::thread occupancy_grid_thread_; diff --git a/cartographer_ros/cartographer_ros/node_main.cc b/cartographer_ros/cartographer_ros/node_main.cc index 619a0e1..d1ee92f 100644 --- a/cartographer_ros/cartographer_ros/node_main.cc +++ b/cartographer_ros/cartographer_ros/node_main.cc @@ -35,6 +35,16 @@ DEFINE_string(configuration_basename, "", namespace cartographer_ros { namespace { +constexpr int kInfiniteSubscriberQueueSize = 0; + +// Default topic names; expected to be remapped as needed. +constexpr char kLaserScanTopic[] = "scan"; +constexpr char kMultiEchoLaserScanTopic[] = "echoes"; +constexpr char kPointCloud2Topic[] = "points2"; +constexpr char kImuTopic[] = "imu"; +constexpr char kOdometryTopic[] = "odom"; +constexpr char kFinishTrajectoryServiceName[] = "finish_trajectory"; + void Run() { auto file_resolver = cartographer::common::make_unique< cartographer::common::ConfigurationFileResolver>( @@ -44,9 +54,114 @@ void Run() { cartographer::common::LuaParameterDictionary lua_parameter_dictionary( code, std::move(file_resolver)); - Node node(CreateNodeOptions(&lua_parameter_dictionary)); + const auto options = CreateNodeOptions(&lua_parameter_dictionary); + Node node(options); + + int trajectory_id = -1; + std::unordered_set expected_sensor_ids; + + // For 2D SLAM, subscribe to exactly one horizontal laser. + ::ros::Subscriber laser_scan_subscriber; + if (options.use_laser_scan) { + laser_scan_subscriber = node.node_handle()->subscribe( + kLaserScanTopic, kInfiniteSubscriberQueueSize, + boost::function( + [&](const sensor_msgs::LaserScan::ConstPtr& msg) { + node.map_builder_bridge() + ->sensor_bridge(trajectory_id) + ->HandleLaserScanMessage(kLaserScanTopic, msg); + })); + expected_sensor_ids.insert(kLaserScanTopic); + } + if (options.use_multi_echo_laser_scan) { + laser_scan_subscriber = node.node_handle()->subscribe( + kMultiEchoLaserScanTopic, kInfiniteSubscriberQueueSize, + boost::function( + [&](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { + node.map_builder_bridge() + ->sensor_bridge(trajectory_id) + ->HandleMultiEchoLaserScanMessage(kMultiEchoLaserScanTopic, + msg); + })); + expected_sensor_ids.insert(kMultiEchoLaserScanTopic); + } + + // For 3D SLAM, subscribe to all point clouds topics. + std::vector<::ros::Subscriber> point_cloud_subscribers; + if (options.num_point_clouds > 0) { + for (int i = 0; i < options.num_point_clouds; ++i) { + string topic = kPointCloud2Topic; + if (options.num_point_clouds > 1) { + topic += "_" + std::to_string(i + 1); + } + point_cloud_subscribers.push_back(node.node_handle()->subscribe( + topic, kInfiniteSubscriberQueueSize, + boost::function( + [&, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) { + node.map_builder_bridge() + ->sensor_bridge(trajectory_id) + ->HandlePointCloud2Message(topic, msg); + }))); + expected_sensor_ids.insert(topic); + } + } + + // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is + // required. + ::ros::Subscriber imu_subscriber; + if (options.map_builder_options.use_trajectory_builder_3d() || + (options.map_builder_options.use_trajectory_builder_2d() && + options.map_builder_options.trajectory_builder_2d_options() + .use_imu_data())) { + imu_subscriber = node.node_handle()->subscribe( + kImuTopic, kInfiniteSubscriberQueueSize, + boost::function( + [&](const sensor_msgs::Imu::ConstPtr& msg) { + node.map_builder_bridge() + ->sensor_bridge(trajectory_id) + ->HandleImuMessage(kImuTopic, msg); + })); + expected_sensor_ids.insert(kImuTopic); + } + + // For both 2D and 3D SLAM, odometry is optional. + ::ros::Subscriber odometry_subscriber; + if (options.use_odometry) { + odometry_subscriber = node.node_handle()->subscribe( + kOdometryTopic, kInfiniteSubscriberQueueSize, + boost::function( + [&](const nav_msgs::Odometry::ConstPtr& msg) { + node.map_builder_bridge() + ->sensor_bridge(trajectory_id) + ->HandleOdometryMessage(kOdometryTopic, msg); + })); + expected_sensor_ids.insert(kOdometryTopic); + } + + trajectory_id = node.map_builder_bridge()->AddTrajectory( + expected_sensor_ids, options.tracking_frame); + + ::ros::ServiceServer finish_trajectory_server = + node.node_handle()->advertiseService( + kFinishTrajectoryServiceName, + boost::function( + [&](::cartographer_ros_msgs::FinishTrajectory::Request& request, + ::cartographer_ros_msgs::FinishTrajectory::Response&) { + const int previous_trajectory_id = trajectory_id; + trajectory_id = node.map_builder_bridge()->AddTrajectory( + expected_sensor_ids, options.tracking_frame); + node.map_builder_bridge()->FinishTrajectory( + previous_trajectory_id); + node.map_builder_bridge()->WriteAssets(request.stem); + return true; + })); + node.Initialize(); - node.SpinForever(); + node.Spin(); + + node.map_builder_bridge()->FinishTrajectory(trajectory_id); } } // namespace