From 0d1e248f5f2303843191dd86e48d1123a55b8d73 Mon Sep 17 00:00:00 2001 From: Damon Kohler Date: Mon, 28 Nov 2016 12:52:34 +0100 Subject: [PATCH] Support multiple simultaneous trajectories. (#194) --- .../cartographer_ros/map_builder_bridge.cc | 90 +++++++++++-------- .../cartographer_ros/map_builder_bridge.h | 30 +++---- cartographer_ros/cartographer_ros/node.cc | 55 ++++++------ cartographer_ros/cartographer_ros/node.h | 4 +- 4 files changed, 98 insertions(+), 81 deletions(-) diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index 37e51e5..f3e7c08 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -23,22 +23,50 @@ namespace cartographer_ros { -MapBuilderBridge::MapBuilderBridge( - const NodeOptions& options, - const std::unordered_set& expected_sensor_ids, - tf2_ros::Buffer* const tf_buffer) +MapBuilderBridge::MapBuilderBridge(const NodeOptions& options, + tf2_ros::Buffer* const tf_buffer) : options_(options), map_builder_(options.map_builder_options, &constant_data_), - expected_sensor_ids_(expected_sensor_ids), - trajectory_id_(map_builder_.AddTrajectoryBuilder(expected_sensor_ids_)), - tf_bridge_(options_.tracking_frame, options_.lookup_transform_timeout_sec, - tf_buffer) { - sensor_bridge_ = cartographer::common::make_unique( - &tf_bridge_, map_builder_.GetTrajectoryBuilder(trajectory_id_)); + tf_buffer_(tf_buffer) {} + +int MapBuilderBridge::AddTrajectory( + const std::unordered_set& expected_sensor_ids, + const string& tracking_frame) { + const int trajectory_id = + map_builder_.AddTrajectoryBuilder(expected_sensor_ids); + LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'."; + + CHECK_EQ(tf_bridges_.count(trajectory_id), 0); + CHECK_EQ(sensor_bridges_.count(trajectory_id), 0); + tf_bridges_[trajectory_id] = cartographer::common::make_unique( + tracking_frame, options_.lookup_transform_timeout_sec, tf_buffer_); + sensor_bridges_[trajectory_id] = + cartographer::common::make_unique( + tf_bridge(trajectory_id), + map_builder_.GetTrajectoryBuilder(trajectory_id)); + return trajectory_id; } -MapBuilderBridge::~MapBuilderBridge() { - map_builder_.FinishTrajectory(trajectory_id_); +void MapBuilderBridge::FinishTrajectory(const int trajectory_id) { + LOG(INFO) << "Finishing trajectory with ID '" << trajectory_id << "'..."; + + CHECK_EQ(tf_bridges_.count(trajectory_id), 1); + CHECK_EQ(sensor_bridges_.count(trajectory_id), 1); + map_builder_.FinishTrajectory(trajectory_id); + map_builder_.sparse_pose_graph()->RunFinalOptimization(); + tf_bridges_.erase(trajectory_id); + sensor_bridges_.erase(trajectory_id); +} + +void MapBuilderBridge::WriteAssets(const string& stem) { + const auto trajectory_nodes = + map_builder_.sparse_pose_graph()->GetTrajectoryNodes(); + if (trajectory_nodes.empty()) { + LOG(WARNING) << "No data was collected and no assets will be written."; + } else { + LOG(INFO) << "Writing assets to '" << stem << "'..."; + cartographer_ros::WriteAssets(trajectory_nodes, options_, stem); + } } bool MapBuilderBridge::HandleSubmapQuery( @@ -63,32 +91,6 @@ bool MapBuilderBridge::HandleSubmapQuery( return true; } -bool MapBuilderBridge::HandleFinishTrajectory( - cartographer_ros_msgs::FinishTrajectory::Request& request, - cartographer_ros_msgs::FinishTrajectory::Response& response) { - LOG(INFO) << "Finishing trajectory..."; - - const int previous_trajectory_id = trajectory_id_; - trajectory_id_ = map_builder_.AddTrajectoryBuilder(expected_sensor_ids_); - sensor_bridge_ = cartographer::common::make_unique( - &tf_bridge_, map_builder_.GetTrajectoryBuilder(trajectory_id_)); - - map_builder_.FinishTrajectory(previous_trajectory_id); - map_builder_.sparse_pose_graph()->RunFinalOptimization(); - - const auto trajectory_nodes = - map_builder_.sparse_pose_graph()->GetTrajectoryNodes(); - if (trajectory_nodes.empty()) { - LOG(WARNING) << "No data collected and no assets will be written."; - } else { - LOG(INFO) << "Writing assets..."; - WriteAssets(trajectory_nodes, options_, request.stem); - } - - LOG(INFO) << "New trajectory started."; - return true; -} - cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() { cartographer_ros_msgs::SubmapList submap_list; submap_list.header.stamp = ::ros::Time::now(); @@ -130,4 +132,16 @@ MapBuilderBridge::BuildOccupancyGrid() { return occupancy_grid; } +SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) { + return sensor_bridges_.at(trajectory_id).get(); +} + +TfBridge* MapBuilderBridge::tf_bridge(const int trajectory_id) { + return tf_bridges_.at(trajectory_id).get(); +} + +cartographer::mapping::MapBuilder* MapBuilderBridge::map_builder() { + return &map_builder_; +} + } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.h b/cartographer_ros/cartographer_ros/map_builder_bridge.h index 712ae09..9959cd4 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.h +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.h @@ -19,6 +19,7 @@ #include #include +#include #include "cartographer/mapping/map_builder.h" #include "cartographer_ros/node_options.h" @@ -34,30 +35,26 @@ namespace cartographer_ros { class MapBuilderBridge { public: - MapBuilderBridge(const NodeOptions& options, - const std::unordered_set& expected_sensor_ids, - tf2_ros::Buffer* tf_buffer); - ~MapBuilderBridge(); + MapBuilderBridge(const NodeOptions& options, tf2_ros::Buffer* tf_buffer); MapBuilderBridge(const MapBuilderBridge&) = delete; MapBuilderBridge& operator=(const MapBuilderBridge&) = delete; + int AddTrajectory(const std::unordered_set& expected_sensor_ids, + const string& tracking_frame); + void FinishTrajectory(int trajectory_id); + void WriteAssets(const string& stem); + 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); cartographer_ros_msgs::SubmapList GetSubmapList(); std::unique_ptr BuildOccupancyGrid(); - SensorBridge* sensor_bridge() { return sensor_bridge_.get(); } - - // TODO(damonkohler): Remove these accessors. - int trajectory_id() const { return trajectory_id_; } - TfBridge* tf_bridge() { return &tf_bridge_; } - cartographer::mapping::MapBuilder* map_builder() { return &map_builder_; } + SensorBridge* sensor_bridge(int trajectory_id); + TfBridge* tf_bridge(int trajectory_id); + cartographer::mapping::MapBuilder* map_builder(); private: const NodeOptions options_; @@ -65,10 +62,9 @@ class MapBuilderBridge { std::deque constant_data_; cartographer::mapping::MapBuilder map_builder_; - std::unordered_set expected_sensor_ids_; - int trajectory_id_ = -1; - TfBridge tf_bridge_; - std::unique_ptr sensor_bridge_; + tf2_ros::Buffer* const tf_buffer_; + std::unordered_map> tf_bridges_; + std::unordered_map> sensor_bridges_; }; } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index df696f1..0a7e8c1 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -64,11 +64,14 @@ constexpr char kFinishTrajectoryServiceName[] = "finish_trajectory"; Node::Node(const NodeOptions& options) : options_(options), tf_buffer_(::ros::Duration(kTfBufferCacheTimeInSeconds)), - tf_(tf_buffer_) {} + tf_(tf_buffer_), + map_builder_bridge_(options_, &tf_buffer_) {} 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()) { @@ -78,29 +81,27 @@ Node::~Node() { void Node::Initialize() { carto::common::MutexLocker lock(&mutex_); - std::unordered_set expected_sensor_ids; - // 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()->HandleLaserScanMessage( + map_builder_bridge_.sensor_bridge(trajectory_id_)->HandleLaserScanMessage( kLaserScanTopic, msg); })); - expected_sensor_ids.insert(kLaserScanTopic); + 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() + map_builder_bridge_.sensor_bridge(trajectory_id_) ->HandleMultiEchoLaserScanMessage(kMultiEchoLaserScanTopic, msg); })); - expected_sensor_ids.insert(kMultiEchoLaserScanTopic); + expected_sensor_ids_.insert(kMultiEchoLaserScanTopic); } // For 3D SLAM, subscribe to all point clouds topics. @@ -114,10 +115,10 @@ void Node::Initialize() { topic, kInfiniteSubscriberQueueSize, boost::function( [this, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) { - map_builder_bridge_->sensor_bridge()->HandlePointCloud2Message( + map_builder_bridge_.sensor_bridge(trajectory_id_)->HandlePointCloud2Message( topic, msg); }))); - expected_sensor_ids.insert(topic); + expected_sensor_ids_.insert(topic); } } @@ -131,10 +132,10 @@ void Node::Initialize() { kImuTopic, kInfiniteSubscriberQueueSize, boost::function( [this](const sensor_msgs::Imu::ConstPtr& msg) { - map_builder_bridge_->sensor_bridge()->HandleImuMessage(kImuTopic, + map_builder_bridge_.sensor_bridge(trajectory_id_)->HandleImuMessage(kImuTopic, msg); })); - expected_sensor_ids.insert(kImuTopic); + expected_sensor_ids_.insert(kImuTopic); } if (options_.use_odometry) { @@ -142,14 +143,14 @@ void Node::Initialize() { kOdometryTopic, kInfiniteSubscriberQueueSize, boost::function( [this](const nav_msgs::Odometry::ConstPtr& msg) { - map_builder_bridge_->sensor_bridge()->HandleOdometryMessage( + map_builder_bridge_.sensor_bridge(trajectory_id_)->HandleOdometryMessage( kOdometryTopic, msg); })); - expected_sensor_ids.insert(kOdometryTopic); + expected_sensor_ids_.insert(kOdometryTopic); } - map_builder_bridge_ = carto::common::make_unique( - options_, expected_sensor_ids, &tf_buffer_); + trajectory_id_ = map_builder_bridge_.AddTrajectory(expected_sensor_ids_, + options_.tracking_frame); submap_list_publisher_ = node_handle_.advertise<::cartographer_ros_msgs::SubmapList>( @@ -185,27 +186,31 @@ bool Node::HandleSubmapQuery( ::cartographer_ros_msgs::SubmapQuery::Request& request, ::cartographer_ros_msgs::SubmapQuery::Response& response) { carto::common::MutexLocker lock(&mutex_); - return map_builder_bridge_->HandleSubmapQuery(request, response); + return map_builder_bridge_.HandleSubmapQuery(request, response); } bool Node::HandleFinishTrajectory( ::cartographer_ros_msgs::FinishTrajectory::Request& request, - ::cartographer_ros_msgs::FinishTrajectory::Response& response) { + ::cartographer_ros_msgs::FinishTrajectory::Response&) { carto::common::MutexLocker lock(&mutex_); - return map_builder_bridge_->HandleFinishTrajectory(request, response); + 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()); + submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList()); } void Node::PublishPoseAndScanMatchedPointCloud( const ::ros::WallTimerEvent& timer_event) { carto::common::MutexLocker lock(&mutex_); const carto::mapping::TrajectoryBuilder* trajectory_builder = - map_builder_bridge_->map_builder()->GetTrajectoryBuilder( - map_builder_bridge_->trajectory_id()); + map_builder_bridge_.map_builder()->GetTrajectoryBuilder(trajectory_id_); const carto::mapping::TrajectoryBuilder::PoseEstimate last_pose_estimate = trajectory_builder->pose_estimate(); if (carto::common::ToUniversal(last_pose_estimate.time) < 0) { @@ -214,7 +219,7 @@ void Node::PublishPoseAndScanMatchedPointCloud( const Rigid3d tracking_to_local = last_pose_estimate.pose; const Rigid3d local_to_map = - map_builder_bridge_->map_builder() + map_builder_bridge_.map_builder() ->sparse_pose_graph() ->GetLocalToGlobalTransform(*trajectory_builder->submaps()); const Rigid3d tracking_to_map = local_to_map * tracking_to_local; @@ -239,8 +244,8 @@ void Node::PublishPoseAndScanMatchedPointCloud( } const auto published_to_tracking = - map_builder_bridge_->tf_bridge()->LookupToTracking( - last_pose_estimate.time, options_.published_frame); + map_builder_bridge_.tf_bridge(trajectory_id_) + ->LookupToTracking(last_pose_estimate.time, options_.published_frame); if (published_to_tracking != nullptr) { if (options_.provide_odom_frame) { std::vector stamped_transforms; @@ -279,7 +284,7 @@ void Node::SpinOccupancyGridThreadForever() { if (occupancy_grid_publisher_.getNumSubscribers() == 0) { continue; } - const auto occupancy_grid = map_builder_bridge_->BuildOccupancyGrid(); + const auto occupancy_grid = map_builder_bridge_.BuildOccupancyGrid(); if (occupancy_grid != nullptr) { occupancy_grid_publisher_.publish(*occupancy_grid); } diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index 2be8a15..ac464b1 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -66,7 +66,9 @@ class Node { tf2_ros::TransformBroadcaster tf_broadcaster_; cartographer::common::Mutex mutex_; - std::unique_ptr map_builder_bridge_ GUARDED_BY(mutex_); + MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_); + int trajectory_id_ = -1; + std::unordered_set expected_sensor_ids_; ::ros::NodeHandle node_handle_; ::ros::Subscriber imu_subscriber_;