diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index f3e7c08..63c9971 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -36,13 +36,10 @@ int MapBuilderBridge::AddTrajectory( 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), + tracking_frame, options_.lookup_transform_timeout_sec, tf_buffer_, map_builder_.GetTrajectoryBuilder(trajectory_id)); return trajectory_id; } @@ -50,11 +47,9 @@ int MapBuilderBridge::AddTrajectory( 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); } @@ -136,10 +131,6 @@ 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_; } diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.h b/cartographer_ros/cartographer_ros/map_builder_bridge.h index 9959cd4..31ee7ba 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.h +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.h @@ -53,7 +53,6 @@ class MapBuilderBridge { std::unique_ptr BuildOccupancyGrid(); SensorBridge* sensor_bridge(int trajectory_id); - TfBridge* tf_bridge(int trajectory_id); cartographer::mapping::MapBuilder* map_builder(); private: @@ -63,7 +62,6 @@ class MapBuilderBridge { constant_data_; cartographer::mapping::MapBuilder map_builder_; tf2_ros::Buffer* const tf_buffer_; - std::unordered_map> tf_bridges_; std::unordered_map> sensor_bridges_; }; diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 0a7e8c1..dc94b81 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -244,8 +244,9 @@ void Node::PublishPoseAndScanMatchedPointCloud( } const auto published_to_tracking = - map_builder_bridge_.tf_bridge(trajectory_id_) - ->LookupToTracking(last_pose_estimate.time, options_.published_frame); + map_builder_bridge_.sensor_bridge(trajectory_id_) + ->tf_bridge() + .LookupToTracking(last_pose_estimate.time, options_.published_frame); if (published_to_tracking != nullptr) { if (options_.provide_odom_frame) { std::vector stamped_transforms; diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.cc b/cartographer_ros/cartographer_ros/sensor_bridge.cc index cf1d56c..0b33d70 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.cc +++ b/cartographer_ros/cartographer_ros/sensor_bridge.cc @@ -38,14 +38,16 @@ const string& CheckNoLeadingSlash(const string& frame_id) { } // namespace SensorBridge::SensorBridge( - const TfBridge* const tf_bridge, + const string& tracking_frame, const double lookup_transform_timeout_sec, + tf2_ros::Buffer* const tf_buffer, carto::mapping::TrajectoryBuilder* const trajectory_builder) - : tf_bridge_(tf_bridge), trajectory_builder_(trajectory_builder) {} + : tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer), + trajectory_builder_(trajectory_builder) {} void SensorBridge::HandleOdometryMessage( const string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) { const carto::common::Time time = FromRos(msg->header.stamp); - const auto sensor_to_tracking = tf_bridge_->LookupToTracking( + const auto sensor_to_tracking = tf_bridge_.LookupToTracking( time, CheckNoLeadingSlash(msg->child_frame_id)); if (sensor_to_tracking != nullptr) { trajectory_builder_->AddOdometerData( @@ -59,7 +61,7 @@ void SensorBridge::HandleImuMessage(const string& sensor_id, CHECK_NE(msg->linear_acceleration_covariance[0], -1); CHECK_NE(msg->angular_velocity_covariance[0], -1); const carto::common::Time time = FromRos(msg->header.stamp); - const auto sensor_to_tracking = tf_bridge_->LookupToTracking( + const auto sensor_to_tracking = tf_bridge_.LookupToTracking( time, CheckNoLeadingSlash(msg->header.frame_id)); if (sensor_to_tracking != nullptr) { CHECK(sensor_to_tracking->translation().norm() < 1e-5) @@ -98,12 +100,14 @@ void SensorBridge::HandlePointCloud2Message( point_cloud); } +const TfBridge& SensorBridge::tf_bridge() const { return tf_bridge_; } + void SensorBridge::HandleRangefinder(const string& sensor_id, const carto::common::Time time, const string& frame_id, const carto::sensor::PointCloud& ranges) { const auto sensor_to_tracking = - tf_bridge_->LookupToTracking(time, CheckNoLeadingSlash(frame_id)); + tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id)); if (sensor_to_tracking != nullptr) { trajectory_builder_->AddRangefinderData( sensor_id, time, sensor_to_tracking->translation().cast(), diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.h b/cartographer_ros/cartographer_ros/sensor_bridge.h index f8bd26a..add78b1 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.h +++ b/cartographer_ros/cartographer_ros/sensor_bridge.h @@ -36,7 +36,8 @@ namespace cartographer_ros { class SensorBridge { public: explicit SensorBridge( - const TfBridge* tf_bridge, + const string& tracking_frame, double lookup_transform_timeout_sec, + tf2_ros::Buffer* tf_buffer, ::cartographer::mapping::TrajectoryBuilder* trajectory_builder); SensorBridge(const SensorBridge&) = delete; @@ -54,13 +55,15 @@ class SensorBridge { void HandlePointCloud2Message(const string& sensor_id, const sensor_msgs::PointCloud2::ConstPtr& msg); + const TfBridge& tf_bridge() const; + private: void HandleRangefinder(const string& sensor_id, const ::cartographer::common::Time time, const string& frame_id, const ::cartographer::sensor::PointCloud& ranges); - const TfBridge* const tf_bridge_; + const TfBridge tf_bridge_; ::cartographer::mapping::TrajectoryBuilder* const trajectory_builder_; };