Moves TfBridge into SensorBridge. (#195)

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<TfBridge>(
-      tracking_frame, options_.lookup_transform_timeout_sec, tf_buffer_);
   sensor_bridges_[trajectory_id] =
       cartographer::common::make_unique<SensorBridge>(
-          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_;
 }

cartographer_ros/cartographer_ros/map_builder_bridge.h

@@ -53,7 +53,6 @@ class MapBuilderBridge {
   std::unique_ptr<nav_msgs::OccupancyGrid> 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<int, std::unique_ptr<TfBridge>> tf_bridges_;
   std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_;
 };
 

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_)
+      map_builder_bridge_.sensor_bridge(trajectory_id_)
-          ->LookupToTracking(last_pose_estimate.time, options_.published_frame);
+          ->tf_bridge()
+          .LookupToTracking(last_pose_estimate.time, options_.published_frame);
   if (published_to_tracking != nullptr) {
     if (options_.provide_odom_frame) {
       std::vector<geometry_msgs::TransformStamped> stamped_transforms;

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<float>(),

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_;
 };