Partially pulls out MapBuilderBridge. (#186)

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

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<string>& 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<SensorBridge>(
+      &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<SensorBridge>(
+      &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<cartographer::transform::Rigid3d> 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

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 <memory>
+#include <unordered_set>
+
+#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<string>& 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<cartographer::mapping::TrajectoryNode::ConstantData>
+      constant_data_;
+  cartographer::mapping::MapBuilder map_builder_;
+  std::unordered_set<string> expected_sensor_ids_;
+  int trajectory_id_ = -1;
+  TfBridge tf_bridge_;
+  std::unique_ptr<SensorBridge> sensor_bridge_;
+};
+
+}  // namespace cartographer_ros

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<carto::mapping::TrajectoryNode::ConstantData> constant_data_
+  std::unique_ptr<MapBuilderBridge> map_builder_bridge_ GUARDED_BY(mutex_);
-      GUARDED_BY(mutex_);
-  carto::mapping::MapBuilder map_builder_ GUARDED_BY(mutex_);
-  std::unique_ptr<SensorBridge> 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<string> expected_sensor_ids_;
 
   ::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_buffer_(::ros::Duration(kTfBufferCacheTimeInSeconds)),
-      tf_(tf_buffer_),
+      tf_(tf_buffer_) {}
-      tf_bridge_(options_.tracking_frame, options_.lookup_transform_timeout_sec,
-                 &tf_buffer_),
-      map_builder_(options.map_builder_options, &constant_data_) {}
 
 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<string> 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<void(const sensor_msgs::LaserScan::ConstPtr&)>(
             [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<void(const sensor_msgs::MultiEchoLaserScan::ConstPtr&)>(
             [this](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
-              sensor_bridge_->HandleMultiEchoLaserScanMessage(
+              map_builder_bridge_->sensor_bridge()
-                  kMultiEchoLaserScanTopic, msg);
+                  ->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<void(const sensor_msgs::PointCloud2::ConstPtr&)>(
               [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<void(const sensor_msgs::Imu::ConstPtr& msg)>(
             [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<void(const nav_msgs::Odometry::ConstPtr&)>(
             [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_);
+  map_builder_bridge_ = carto::common::make_unique<MapBuilderBridge>(
-  sensor_bridge_ = carto::common::make_unique<SensorBridge>(
+      options_, expected_sensor_ids, &tf_buffer_);
-      &tf_bridge_, map_builder_.GetTrajectoryBuilder(trajectory_id_));
 
   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_);
-
+  return map_builder_bridge_->HandleSubmapQuery(request, response);
-  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;
 }
 
 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_;
+  return map_builder_bridge_->HandleFinishTrajectory(request, response);
-
-  trajectory_id_ =
-      map_builder_.AddTrajectoryBuilder(expected_sensor_ids_);
-  sensor_bridge_ = carto::common::make_unique<SensorBridge>(
-      &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.";
+void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) {
-  return true;
-}
-
-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;
-
   carto::common::MutexLocker lock(&mutex_);
-  for (int trajectory_id = 0;
+  submap_list_publisher_.publish(map_builder_bridge_->GetSubmapList());
-       trajectory_id < map_builder_.num_trajectory_builders();
-       ++trajectory_id) {
-    const carto::mapping::Submaps* submaps =
-        map_builder_.GetTrajectoryBuilder(trajectory_id)->submaps();
-    const std::vector<carto::transform::Rigid3d> 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);
 }
 
 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(
+      map_builder_bridge_->map_builder()
-          *trajectory_builder->submaps());
+          ->sparse_pose_graph()
+          ->GetLocalToGlobalTransform(*trajectory_builder->submaps());
   const Rigid3d tracking_to_map = local_to_map * tracking_to_local;
 
   geometry_msgs::TransformStamped stamped_transform;
@@ -390,7 +321,8 @@ void Node::PublishPoseAndScanMatchedPointCloud(
     stamped_transform.header.stamp = ros::Time::now();
   }
 
-  const auto published_to_tracking = tf_bridge_.LookupToTracking(
+  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) {
@@ -430,8 +362,9 @@ void Node::SpinOccupancyGridThreadForever() {
     if (occupancy_grid_publisher_.getNumSubscribers() == 0) {
       continue;
     }
-    const auto trajectory_nodes =
+    const auto trajectory_nodes = map_builder_bridge_->map_builder()
-        map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
+                                      ->sparse_pose_graph()
+                                      ->GetTrajectoryNodes();
     if (trajectory_nodes.empty()) {
       continue;
     }