diff --git a/cartographer_ros/cartographer_ros/CMakeLists.txt b/cartographer_ros/cartographer_ros/CMakeLists.txt index a198274..14a885d 100644 --- a/cartographer_ros/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/cartographer_ros/CMakeLists.txt @@ -10,6 +10,20 @@ google_library(assets_writer occupancy_grid ) +google_library(map_builder_bridge + USES_CARTOGRAPHER + SRCS + map_builder_bridge.cc + HDRS + map_builder_bridge.h + DEPENDS + assets_writer + msg_conversion + node_options + sensor_bridge + tf_bridge +) + google_library(map_writer USES_GLOG USES_YAMLCPP @@ -128,7 +142,7 @@ google_binary(cartographer_node SRCS node_main.cc DEPENDS - assets_writer + map_builder_bridge msg_conversion node_options occupancy_grid diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc new file mode 100644 index 0000000..99d375b --- /dev/null +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -0,0 +1,118 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer_ros/map_builder_bridge.h" + +#include "cartographer_ros/assets_writer.h" +#include "cartographer_ros/msg_conversion.h" +#include "cartographer_ros_msgs/TrajectorySubmapList.h" + +namespace cartographer_ros { + +MapBuilderBridge::MapBuilderBridge( + const NodeOptions& options, + const std::unordered_set& expected_sensor_ids, + 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_)); +} + +MapBuilderBridge::~MapBuilderBridge() { + map_builder_.FinishTrajectory(trajectory_id_); +} + +bool MapBuilderBridge::HandleSubmapQuery( + cartographer_ros_msgs::SubmapQuery::Request& request, + cartographer_ros_msgs::SubmapQuery::Response& response) { + cartographer::mapping::proto::SubmapQuery::Response response_proto; + const std::string error = map_builder_.SubmapToProto( + request.trajectory_id, request.submap_index, &response_proto); + if (!error.empty()) { + LOG(ERROR) << error; + return false; + } + + response.submap_version = response_proto.submap_version(); + response.cells.insert(response.cells.begin(), response_proto.cells().begin(), + response_proto.cells().end()); + response.width = response_proto.width(); + response.height = response_proto.height(); + response.resolution = response_proto.resolution(); + response.slice_pose = ToGeometryMsgPose( + cartographer::transform::ToRigid3(response_proto.slice_pose())); + 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(); + submap_list.header.frame_id = options_.map_frame; + for (int trajectory_id = 0; + trajectory_id < map_builder_.num_trajectory_builders(); + ++trajectory_id) { + const cartographer::mapping::Submaps* submaps = + map_builder_.GetTrajectoryBuilder(trajectory_id)->submaps(); + const std::vector submap_transforms = + map_builder_.sparse_pose_graph()->GetSubmapTransforms(*submaps); + CHECK_EQ(submap_transforms.size(), submaps->size()); + + cartographer_ros_msgs::TrajectorySubmapList trajectory_submap_list; + for (int submap_index = 0; submap_index != submaps->size(); + ++submap_index) { + cartographer_ros_msgs::SubmapEntry submap_entry; + submap_entry.submap_version = + submaps->Get(submap_index)->end_laser_fan_index; + submap_entry.pose = ToGeometryMsgPose(submap_transforms[submap_index]); + trajectory_submap_list.submap.push_back(submap_entry); + } + submap_list.trajectory.push_back(trajectory_submap_list); + } + return submap_list; +} + +} // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.h b/cartographer_ros/cartographer_ros/map_builder_bridge.h new file mode 100644 index 0000000..dd1e226 --- /dev/null +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.h @@ -0,0 +1,69 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +#include "cartographer/mapping/map_builder.h" +#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" + +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 MapBuilderBridge&) = delete; + MapBuilderBridge& operator=(const MapBuilderBridge&) = delete; + + 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(); + + 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_; } + + private: + const NodeOptions options_; + + 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_; +}; + +} // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/node_main.cc b/cartographer_ros/cartographer_ros/node_main.cc index 47a592c..5daf355 100644 --- a/cartographer_ros/cartographer_ros/node_main.cc +++ b/cartographer_ros/cartographer_ros/node_main.cc @@ -33,7 +33,7 @@ #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" -#include "cartographer_ros/assets_writer.h" +#include "cartographer_ros/map_builder_bridge.h" #include "cartographer_ros/msg_conversion.h" #include "cartographer_ros/node_options.h" #include "cartographer_ros/occupancy_grid.h" @@ -71,10 +71,10 @@ namespace { namespace carto = ::cartographer; using carto::transform::Rigid3d; -using carto::kalman_filter::PoseCovariance; 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"; @@ -119,18 +119,9 @@ class Node { tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_; tf2_ros::TransformBroadcaster tf_broadcaster_; - TfBridge tf_bridge_; carto::common::Mutex mutex_; - std::deque constant_data_ - GUARDED_BY(mutex_); - carto::mapping::MapBuilder map_builder_ GUARDED_BY(mutex_); - std::unique_ptr sensor_bridge_ GUARDED_BY(mutex_); - int trajectory_id_ = -1 GUARDED_BY(mutex_); - - // Set of all topics we subscribe to. We use the non-remapped default names - // which are unique. - std::unordered_set expected_sensor_ids_; + std::unique_ptr map_builder_bridge_ GUARDED_BY(mutex_); ::ros::NodeHandle node_handle_; ::ros::Subscriber imu_subscriber_; @@ -155,17 +146,13 @@ class Node { Node::Node(const NodeOptions& options) : options_(options), - tf_buffer_(::ros::Duration(1000)), - tf_(tf_buffer_), - tf_bridge_(options_.tracking_frame, options_.lookup_transform_timeout_sec, - &tf_buffer_), - map_builder_(options.map_builder_options, &constant_data_) {} + tf_buffer_(::ros::Duration(kTfBufferCacheTimeInSeconds)), + tf_(tf_buffer_) {} Node::~Node() { { carto::common::MutexLocker lock(&mutex_); terminating_ = true; - map_builder_.FinishTrajectory(trajectory_id_); } if (occupancy_grid_thread_.joinable()) { occupancy_grid_thread_.join(); @@ -174,6 +161,7 @@ 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) { @@ -181,19 +169,21 @@ void Node::Initialize() { kLaserScanTopic, kInfiniteSubscriberQueueSize, boost::function( [this](const sensor_msgs::LaserScan::ConstPtr& msg) { - sensor_bridge_->HandleLaserScanMessage(kLaserScanTopic, msg); + map_builder_bridge_->sensor_bridge()->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) { - sensor_bridge_->HandleMultiEchoLaserScanMessage( - kMultiEchoLaserScanTopic, msg); + map_builder_bridge_->sensor_bridge() + ->HandleMultiEchoLaserScanMessage(kMultiEchoLaserScanTopic, + msg); })); - expected_sensor_ids_.insert(kMultiEchoLaserScanTopic); + expected_sensor_ids.insert(kMultiEchoLaserScanTopic); } // For 3D SLAM, subscribe to all point clouds topics. @@ -207,9 +197,10 @@ void Node::Initialize() { topic, kInfiniteSubscriberQueueSize, boost::function( [this, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) { - sensor_bridge_->HandlePointCloud2Message(topic, msg); + map_builder_bridge_->sensor_bridge()->HandlePointCloud2Message( + topic, msg); }))); - expected_sensor_ids_.insert(topic); + expected_sensor_ids.insert(topic); } } @@ -223,9 +214,10 @@ void Node::Initialize() { kImuTopic, kInfiniteSubscriberQueueSize, boost::function( [this](const sensor_msgs::Imu::ConstPtr& msg) { - sensor_bridge_->HandleImuMessage(kImuTopic, msg); + map_builder_bridge_->sensor_bridge()->HandleImuMessage(kImuTopic, + msg); })); - expected_sensor_ids_.insert(kImuTopic); + expected_sensor_ids.insert(kImuTopic); } if (options_.use_odometry) { @@ -233,14 +225,14 @@ void Node::Initialize() { kOdometryTopic, kInfiniteSubscriberQueueSize, boost::function( [this](const nav_msgs::Odometry::ConstPtr& msg) { - sensor_bridge_->HandleOdometryMessage(kOdometryTopic, msg); + map_builder_bridge_->sensor_bridge()->HandleOdometryMessage( + kOdometryTopic, msg); })); - expected_sensor_ids_.insert(kOdometryTopic); + expected_sensor_ids.insert(kOdometryTopic); } - trajectory_id_ = map_builder_.AddTrajectoryBuilder(expected_sensor_ids_); - sensor_bridge_ = carto::common::make_unique( - &tf_bridge_, map_builder_.GetTrajectoryBuilder(trajectory_id_)); + map_builder_bridge_ = carto::common::make_unique( + options_, expected_sensor_ids, &tf_buffer_); submap_list_publisher_ = node_handle_.advertise<::cartographer_ros_msgs::SubmapList>( @@ -276,89 +268,27 @@ bool Node::HandleSubmapQuery( ::cartographer_ros_msgs::SubmapQuery::Request& request, ::cartographer_ros_msgs::SubmapQuery::Response& response) { carto::common::MutexLocker lock(&mutex_); - - carto::mapping::proto::SubmapQuery::Response response_proto; - const std::string error = map_builder_.SubmapToProto( - request.trajectory_id, request.submap_index, &response_proto); - if (!error.empty()) { - LOG(ERROR) << error; - return false; - } - - response.submap_version = response_proto.submap_version(); - response.cells.insert(response.cells.begin(), response_proto.cells().begin(), - response_proto.cells().end()); - response.width = response_proto.width(); - response.height = response_proto.height(); - response.resolution = response_proto.resolution(); - response.slice_pose = ToGeometryMsgPose( - carto::transform::ToRigid3(response_proto.slice_pose())); - return true; + return map_builder_bridge_->HandleSubmapQuery(request, response); } bool Node::HandleFinishTrajectory( ::cartographer_ros_msgs::FinishTrajectory::Request& request, ::cartographer_ros_msgs::FinishTrajectory::Response& response) { - LOG(INFO) << "Finishing trajectory..."; - carto::common::MutexLocker lock(&mutex_); - const int previous_trajectory_id = trajectory_id_; - - trajectory_id_ = - map_builder_.AddTrajectoryBuilder(expected_sensor_ids_); - sensor_bridge_ = carto::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; + return map_builder_bridge_->HandleFinishTrajectory(request, response); } -void Node::PublishSubmapList(const ::ros::WallTimerEvent& timer_event) { - ::cartographer_ros_msgs::SubmapList ros_submap_list; - ros_submap_list.header.stamp = ::ros::Time::now(); - ros_submap_list.header.frame_id = options_.map_frame; - +void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) { carto::common::MutexLocker lock(&mutex_); - for (int trajectory_id = 0; - trajectory_id < map_builder_.num_trajectory_builders(); - ++trajectory_id) { - const carto::mapping::Submaps* submaps = - map_builder_.GetTrajectoryBuilder(trajectory_id)->submaps(); - const std::vector submap_transforms = - map_builder_.sparse_pose_graph()->GetSubmapTransforms(*submaps); - CHECK_EQ(submap_transforms.size(), submaps->size()); - - ::cartographer_ros_msgs::TrajectorySubmapList ros_trajectory; - for (int submap_index = 0; submap_index != submaps->size(); - ++submap_index) { - ::cartographer_ros_msgs::SubmapEntry ros_submap; - ros_submap.submap_version = - submaps->Get(submap_index)->end_laser_fan_index; - ros_submap.pose = ToGeometryMsgPose(submap_transforms[submap_index]); - ros_trajectory.submap.push_back(ros_submap); - } - ros_submap_list.trajectory.push_back(ros_trajectory); - } - submap_list_publisher_.publish(ros_submap_list); + 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_.GetTrajectoryBuilder(trajectory_id_); + map_builder_bridge_->map_builder()->GetTrajectoryBuilder( + map_builder_bridge_->trajectory_id()); const carto::mapping::TrajectoryBuilder::PoseEstimate last_pose_estimate = trajectory_builder->pose_estimate(); if (carto::common::ToUniversal(last_pose_estimate.time) < 0) { @@ -367,8 +297,9 @@ void Node::PublishPoseAndScanMatchedPointCloud( const Rigid3d tracking_to_local = last_pose_estimate.pose; const Rigid3d local_to_map = - map_builder_.sparse_pose_graph()->GetLocalToGlobalTransform( - *trajectory_builder->submaps()); + map_builder_bridge_->map_builder() + ->sparse_pose_graph() + ->GetLocalToGlobalTransform(*trajectory_builder->submaps()); const Rigid3d tracking_to_map = local_to_map * tracking_to_local; geometry_msgs::TransformStamped stamped_transform; @@ -390,8 +321,9 @@ void Node::PublishPoseAndScanMatchedPointCloud( stamped_transform.header.stamp = ros::Time::now(); } - const auto published_to_tracking = tf_bridge_.LookupToTracking( - last_pose_estimate.time, options_.published_frame); + const auto published_to_tracking = + map_builder_bridge_->tf_bridge()->LookupToTracking( + last_pose_estimate.time, options_.published_frame); if (published_to_tracking != nullptr) { if (options_.provide_odom_frame) { std::vector stamped_transforms; @@ -430,8 +362,9 @@ void Node::SpinOccupancyGridThreadForever() { if (occupancy_grid_publisher_.getNumSubscribers() == 0) { continue; } - const auto trajectory_nodes = - map_builder_.sparse_pose_graph()->GetTrajectoryNodes(); + const auto trajectory_nodes = map_builder_bridge_->map_builder() + ->sparse_pose_graph() + ->GetTrajectoryNodes(); if (trajectory_nodes.empty()) { continue; }