Pulls out TrajectoryState into MapBuilderBridge. (#197)

cartographer_ros/cartographer_ros/map_builder_bridge.cc

@@ -127,12 +127,33 @@ MapBuilderBridge::BuildOccupancyGrid() {
   return occupancy_grid;
 }
 
+std::unordered_map<int, MapBuilderBridge::TrajectoryState>
+MapBuilderBridge::GetTrajectoryStates() {
+  std::unordered_map<int, TrajectoryState> trajectory_states;
+  for (const auto& entry : sensor_bridges_) {
+    const int trajectory_id = entry.first;
+    const SensorBridge& sensor_bridge = *entry.second;
+
+    const cartographer::mapping::TrajectoryBuilder* const trajectory_builder =
+        map_builder_.GetTrajectoryBuilder(trajectory_id);
+    const cartographer::mapping::TrajectoryBuilder::PoseEstimate
+        pose_estimate = trajectory_builder->pose_estimate();
+    if (cartographer::common::ToUniversal(pose_estimate.time) < 0) {
+      continue;
+    }
+
+    trajectory_states[trajectory_id] = {
+        pose_estimate,
+        map_builder_.sparse_pose_graph()->GetLocalToGlobalTransform(
+            *trajectory_builder->submaps()),
+        sensor_bridge.tf_bridge().LookupToTracking(pose_estimate.time,
+                                                   options_.published_frame)};
+  }
+  return trajectory_states;
+}
+
 SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) {
   return sensor_bridges_.at(trajectory_id).get();
 }
 
-cartographer::mapping::MapBuilder* MapBuilderBridge::map_builder() {
-  return &map_builder_;
-}
-
 }  // namespace cartographer_ros

cartographer_ros/cartographer_ros/map_builder_bridge.h

@@ -35,6 +35,12 @@ namespace cartographer_ros {
 
 class MapBuilderBridge {
  public:
+  struct TrajectoryState {
+    cartographer::mapping::TrajectoryBuilder::PoseEstimate pose_estimate;
+    cartographer::transform::Rigid3d local_to_map;
+    std::unique_ptr<cartographer::transform::Rigid3d> published_to_tracking;
+  };
+
   MapBuilderBridge(const NodeOptions& options, tf2_ros::Buffer* tf_buffer);
 
   MapBuilderBridge(const MapBuilderBridge&) = delete;
@@ -51,9 +57,9 @@ class MapBuilderBridge {
 
   cartographer_ros_msgs::SubmapList GetSubmapList();
   std::unique_ptr<nav_msgs::OccupancyGrid> BuildOccupancyGrid();
+  std::unordered_map<int, TrajectoryState> GetTrajectoryStates();
 
   SensorBridge* sensor_bridge(int trajectory_id);
-  cartographer::mapping::MapBuilder* map_builder();
 
  private:
   const NodeOptions options_;

cartographer_ros/cartographer_ros/node.cc

@@ -179,7 +179,7 @@ void Node::Initialize() {
       &Node::PublishSubmapList, this));
   wall_timers_.push_back(node_handle_.createWallTimer(
       ::ros::WallDuration(options_.pose_publish_period_sec),
-      &Node::PublishPoseAndScanMatchedPointCloud, this));
+      &Node::PublishTrajectoryStates, this));
 }
 
 bool Node::HandleSubmapQuery(
@@ -206,71 +206,61 @@ void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) {
   submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList());
 }
 
-void Node::PublishPoseAndScanMatchedPointCloud(
-    const ::ros::WallTimerEvent& timer_event) {
+void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) {
   carto::common::MutexLocker lock(&mutex_);
-  const carto::mapping::TrajectoryBuilder* trajectory_builder =
-      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) {
-    return;
-  }
-
-  const Rigid3d tracking_to_local = last_pose_estimate.pose;
-  const Rigid3d local_to_map =
-      map_builder_bridge_.map_builder()
-          ->sparse_pose_graph()
-          ->GetLocalToGlobalTransform(*trajectory_builder->submaps());
-  const Rigid3d tracking_to_map = local_to_map * tracking_to_local;
+  for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
+    const auto& trajectory_state = entry.second;
 
     geometry_msgs::TransformStamped stamped_transform;
-  stamped_transform.header.stamp = ToRos(last_pose_estimate.time);
+    stamped_transform.header.stamp = ToRos(trajectory_state.pose_estimate.time);
+
+    const auto& tracking_to_local = trajectory_state.pose_estimate.pose;
+    const Rigid3d tracking_to_map =
+        trajectory_state.local_to_map * tracking_to_local;
 
     // We only publish a point cloud if it has changed. It is not needed at high
     // frequency, and republishing it would be computationally wasteful.
-  if (last_pose_estimate.time != last_scan_matched_point_cloud_time_) {
+    if (trajectory_state.pose_estimate.time != last_scan_matched_point_cloud_time_) {
       scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message(
-        carto::common::ToUniversal(last_pose_estimate.time),
+          carto::common::ToUniversal(trajectory_state.pose_estimate.time),
           options_.tracking_frame,
           carto::sensor::TransformPointCloud(
-            last_pose_estimate.point_cloud,
+              trajectory_state.pose_estimate.point_cloud,
               tracking_to_local.inverse().cast<float>())));
-    last_scan_matched_point_cloud_time_ = last_pose_estimate.time;
+      last_scan_matched_point_cloud_time_ = trajectory_state.pose_estimate.time;
     } else {
       // If we do not publish a new point cloud, we still allow time of the
       // published poses to advance.
       stamped_transform.header.stamp = ros::Time::now();
     }
 
-  const auto published_to_tracking =
-      map_builder_bridge_.sensor_bridge(trajectory_id_)
-          ->tf_bridge()
-          .LookupToTracking(last_pose_estimate.time, options_.published_frame);
-  if (published_to_tracking != nullptr) {
+    if (trajectory_state.published_to_tracking != nullptr) {
       if (options_.provide_odom_frame) {
         std::vector<geometry_msgs::TransformStamped> stamped_transforms;
 
         stamped_transform.header.frame_id = options_.map_frame;
+        // TODO(damonkohler): 'odom_frame' and 'published_frame' must be
+        // per-trajectory to fully support the multi-robot use case.
         stamped_transform.child_frame_id = options_.odom_frame;
-      stamped_transform.transform = ToGeometryMsgTransform(local_to_map);
+        stamped_transform.transform = ToGeometryMsgTransform(trajectory_state.local_to_map);
         stamped_transforms.push_back(stamped_transform);
 
         stamped_transform.header.frame_id = options_.odom_frame;
         stamped_transform.child_frame_id = options_.published_frame;
-      stamped_transform.transform =
-          ToGeometryMsgTransform(tracking_to_local * (*published_to_tracking));
+        stamped_transform.transform = ToGeometryMsgTransform(
+            tracking_to_local * (*trajectory_state.published_to_tracking));
         stamped_transforms.push_back(stamped_transform);
 
         tf_broadcaster_.sendTransform(stamped_transforms);
       } else {
         stamped_transform.header.frame_id = options_.map_frame;
         stamped_transform.child_frame_id = options_.published_frame;
-      stamped_transform.transform =
-          ToGeometryMsgTransform(tracking_to_map * (*published_to_tracking));
+        stamped_transform.transform = ToGeometryMsgTransform(
+            tracking_to_map * (*trajectory_state.published_to_tracking));
         tf_broadcaster_.sendTransform(stamped_transform);
       }
     }
+  }
 }
 
 void Node::SpinOccupancyGridThreadForever() {

cartographer_ros/cartographer_ros/node.h

@@ -55,8 +55,7 @@ class Node {
       cartographer_ros_msgs::FinishTrajectory::Response& response);
 
   void PublishSubmapList(const ::ros::WallTimerEvent& timer_event);
-  void PublishPoseAndScanMatchedPointCloud(
-      const ::ros::WallTimerEvent& timer_event);
+  void PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event);
   void SpinOccupancyGridThreadForever();
 
   const NodeOptions options_;