Pulls out sensor data wiring into main. (#198)

cartographer_ros/cartographer_ros/map_builder_bridge.h

@@ -25,7 +25,6 @@
 #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"

cartographer_ros/cartographer_ros/node.cc

@@ -45,21 +45,14 @@ namespace carto = ::cartographer;
 
 using carto::transform::Rigid3d;
 
-constexpr int kInfiniteSubscriberQueueSize = 0;
 constexpr int kLatestOnlyPublisherQueueSize = 1;
 constexpr double kTfBufferCacheTimeInSeconds = 1e6;
 
-// Unique default topic names. Expected to be remapped as needed.
+// Default topic names; expected to be remapped as needed.
-constexpr char kLaserScanTopic[] = "scan";
-constexpr char kMultiEchoLaserScanTopic[] = "echoes";
-constexpr char kPointCloud2Topic[] = "points2";
-constexpr char kImuTopic[] = "imu";
-constexpr char kOdometryTopic[] = "odom";
 constexpr char kOccupancyGridTopic[] = "map";
 constexpr char kScanMatchedPointCloudTopic[] = "scan_matched_points2";
 constexpr char kSubmapListTopic[] = "submap_list";
 constexpr char kSubmapQueryServiceName[] = "submap_query";
-constexpr char kFinishTrajectoryServiceName[] = "finish_trajectory";
 
 Node::Node(const NodeOptions& options)
     : options_(options),
@@ -70,8 +63,6 @@ Node::Node(const NodeOptions& options)
 Node::~Node() {
   {
     carto::common::MutexLocker lock(&mutex_);
-    CHECK_GE(trajectory_id_, 0);
-    map_builder_bridge_.FinishTrajectory(trajectory_id_);
     terminating_ = true;
   }
   if (occupancy_grid_thread_.joinable()) {
@@ -81,77 +72,6 @@ Node::~Node() {
 
 void Node::Initialize() {
   carto::common::MutexLocker lock(&mutex_);
-  // For 2D SLAM, subscribe to exactly one horizontal laser.
-  if (options_.use_laser_scan) {
-    horizontal_laser_scan_subscriber_ = node_handle_.subscribe(
-        kLaserScanTopic, kInfiniteSubscriberQueueSize,
-        boost::function<void(const sensor_msgs::LaserScan::ConstPtr&)>(
-            [this](const sensor_msgs::LaserScan::ConstPtr& msg) {
-              map_builder_bridge_.sensor_bridge(trajectory_id_)->HandleLaserScanMessage(
-                  kLaserScanTopic, msg);
-            }));
-    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) {
-              map_builder_bridge_.sensor_bridge(trajectory_id_)
-                  ->HandleMultiEchoLaserScanMessage(kMultiEchoLaserScanTopic,
-                                                    msg);
-            }));
-    expected_sensor_ids_.insert(kMultiEchoLaserScanTopic);
-  }
-
-  // For 3D SLAM, subscribe to all point clouds topics.
-  if (options_.num_point_clouds > 0) {
-    for (int i = 0; i < options_.num_point_clouds; ++i) {
-      string topic = kPointCloud2Topic;
-      if (options_.num_point_clouds > 1) {
-        topic += "_" + std::to_string(i + 1);
-      }
-      point_cloud_subscribers_.push_back(node_handle_.subscribe(
-          topic, kInfiniteSubscriberQueueSize,
-          boost::function<void(const sensor_msgs::PointCloud2::ConstPtr&)>(
-              [this, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) {
-                map_builder_bridge_.sensor_bridge(trajectory_id_)->HandlePointCloud2Message(
-                    topic, msg);
-              })));
-      expected_sensor_ids_.insert(topic);
-    }
-  }
-
-  // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
-  // required.
-  if (options_.map_builder_options.use_trajectory_builder_3d() ||
-      (options_.map_builder_options.use_trajectory_builder_2d() &&
-       options_.map_builder_options.trajectory_builder_2d_options()
-           .use_imu_data())) {
-    imu_subscriber_ = node_handle_.subscribe(
-        kImuTopic, kInfiniteSubscriberQueueSize,
-        boost::function<void(const sensor_msgs::Imu::ConstPtr& msg)>(
-            [this](const sensor_msgs::Imu::ConstPtr& msg) {
-              map_builder_bridge_.sensor_bridge(trajectory_id_)->HandleImuMessage(kImuTopic,
-                                                                     msg);
-            }));
-    expected_sensor_ids_.insert(kImuTopic);
-  }
-
-  if (options_.use_odometry) {
-    odometry_subscriber_ = node_handle_.subscribe(
-        kOdometryTopic, kInfiniteSubscriberQueueSize,
-        boost::function<void(const nav_msgs::Odometry::ConstPtr&)>(
-            [this](const nav_msgs::Odometry::ConstPtr& msg) {
-              map_builder_bridge_.sensor_bridge(trajectory_id_)->HandleOdometryMessage(
-                  kOdometryTopic, msg);
-            }));
-    expected_sensor_ids_.insert(kOdometryTopic);
-  }
-
-  trajectory_id_ = map_builder_bridge_.AddTrajectory(expected_sensor_ids_,
-                                                     options_.tracking_frame);
-
   submap_list_publisher_ =
       node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
           kSubmapListTopic, kLatestOnlyPublisherQueueSize);
@@ -171,9 +91,6 @@ void Node::Initialize() {
       node_handle_.advertise<sensor_msgs::PointCloud2>(
           kScanMatchedPointCloudTopic, kLatestOnlyPublisherQueueSize);
 
-  finish_trajectory_server_ = node_handle_.advertiseService(
-      kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this);
-
   wall_timers_.push_back(node_handle_.createWallTimer(
       ::ros::WallDuration(options_.submap_publish_period_sec),
       &Node::PublishSubmapList, this));
@@ -182,6 +99,12 @@ void Node::Initialize() {
       &Node::PublishTrajectoryStates, this));
 }
 
+void Node::Spin() { ::ros::spin(); }
+
+::ros::NodeHandle* Node::node_handle() { return &node_handle_; }
+
+MapBuilderBridge* Node::map_builder_bridge() { return &map_builder_bridge_; }
+
 bool Node::HandleSubmapQuery(
     ::cartographer_ros_msgs::SubmapQuery::Request& request,
     ::cartographer_ros_msgs::SubmapQuery::Response& response) {
@@ -189,18 +112,6 @@ bool Node::HandleSubmapQuery(
   return map_builder_bridge_.HandleSubmapQuery(request, response);
 }
 
-bool Node::HandleFinishTrajectory(
-    ::cartographer_ros_msgs::FinishTrajectory::Request& request,
-    ::cartographer_ros_msgs::FinishTrajectory::Response&) {
-  carto::common::MutexLocker lock(&mutex_);
-  const int previous_trajectory_id = trajectory_id_;
-  trajectory_id_ = map_builder_bridge_.AddTrajectory(expected_sensor_ids_,
-                                                     options_.tracking_frame);
-  map_builder_bridge_.FinishTrajectory(previous_trajectory_id);
-  map_builder_bridge_.WriteAssets(request.stem);
-  return true;
-}
-
 void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) {
   carto::common::MutexLocker lock(&mutex_);
   submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList());
@@ -282,6 +193,4 @@ void Node::SpinOccupancyGridThreadForever() {
   }
 }
 
-void Node::SpinForever() { ::ros::spin(); }
-
 }  // namespace cartographer_ros

cartographer_ros/cartographer_ros/node.h

@@ -43,16 +43,16 @@ class Node {
   Node(const Node&) = delete;
   Node& operator=(const Node&) = delete;
 
-  void SpinForever();
   void Initialize();
+  void Spin();
+
+  ::ros::NodeHandle* node_handle();
+  MapBuilderBridge* map_builder_bridge();
 
  private:
   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);
 
   void PublishSubmapList(const ::ros::WallTimerEvent& timer_event);
   void PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event);
@@ -70,16 +70,11 @@ class Node {
   std::unordered_set<string> expected_sensor_ids_;
 
   ::ros::NodeHandle node_handle_;
-  ::ros::Subscriber imu_subscriber_;
-  ::ros::Subscriber horizontal_laser_scan_subscriber_;
-  std::vector<::ros::Subscriber> point_cloud_subscribers_;
-  ::ros::Subscriber odometry_subscriber_;
   ::ros::Publisher submap_list_publisher_;
   ::ros::ServiceServer submap_query_server_;
   ::ros::Publisher scan_matched_point_cloud_publisher_;
   cartographer::common::Time last_scan_matched_point_cloud_time_ =
       cartographer::common::Time::min();
-  ::ros::ServiceServer finish_trajectory_server_;
 
   ::ros::Publisher occupancy_grid_publisher_;
   std::thread occupancy_grid_thread_;

cartographer_ros/cartographer_ros/node_main.cc

@@ -35,6 +35,16 @@ DEFINE_string(configuration_basename, "",
 namespace cartographer_ros {
 namespace {
 
+constexpr int kInfiniteSubscriberQueueSize = 0;
+
+// Default topic names; expected to be remapped as needed.
+constexpr char kLaserScanTopic[] = "scan";
+constexpr char kMultiEchoLaserScanTopic[] = "echoes";
+constexpr char kPointCloud2Topic[] = "points2";
+constexpr char kImuTopic[] = "imu";
+constexpr char kOdometryTopic[] = "odom";
+constexpr char kFinishTrajectoryServiceName[] = "finish_trajectory";
+
 void Run() {
   auto file_resolver = cartographer::common::make_unique<
       cartographer::common::ConfigurationFileResolver>(
@@ -44,9 +54,114 @@ void Run() {
   cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
       code, std::move(file_resolver));
 
-  Node node(CreateNodeOptions(&lua_parameter_dictionary));
+  const auto options = CreateNodeOptions(&lua_parameter_dictionary);
+  Node node(options);
+
+  int trajectory_id = -1;
+  std::unordered_set<string> expected_sensor_ids;
+
+  // For 2D SLAM, subscribe to exactly one horizontal laser.
+  ::ros::Subscriber laser_scan_subscriber;
+  if (options.use_laser_scan) {
+    laser_scan_subscriber = node.node_handle()->subscribe(
+        kLaserScanTopic, kInfiniteSubscriberQueueSize,
+        boost::function<void(const sensor_msgs::LaserScan::ConstPtr&)>(
+            [&](const sensor_msgs::LaserScan::ConstPtr& msg) {
+              node.map_builder_bridge()
+                  ->sensor_bridge(trajectory_id)
+                  ->HandleLaserScanMessage(kLaserScanTopic, msg);
+            }));
+    expected_sensor_ids.insert(kLaserScanTopic);
+  }
+  if (options.use_multi_echo_laser_scan) {
+    laser_scan_subscriber = node.node_handle()->subscribe(
+        kMultiEchoLaserScanTopic, kInfiniteSubscriberQueueSize,
+        boost::function<void(const sensor_msgs::MultiEchoLaserScan::ConstPtr&)>(
+            [&](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
+              node.map_builder_bridge()
+                  ->sensor_bridge(trajectory_id)
+                  ->HandleMultiEchoLaserScanMessage(kMultiEchoLaserScanTopic,
+                                                    msg);
+            }));
+    expected_sensor_ids.insert(kMultiEchoLaserScanTopic);
+  }
+
+  // For 3D SLAM, subscribe to all point clouds topics.
+  std::vector<::ros::Subscriber> point_cloud_subscribers;
+  if (options.num_point_clouds > 0) {
+    for (int i = 0; i < options.num_point_clouds; ++i) {
+      string topic = kPointCloud2Topic;
+      if (options.num_point_clouds > 1) {
+        topic += "_" + std::to_string(i + 1);
+      }
+      point_cloud_subscribers.push_back(node.node_handle()->subscribe(
+          topic, kInfiniteSubscriberQueueSize,
+          boost::function<void(const sensor_msgs::PointCloud2::ConstPtr&)>(
+              [&, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) {
+                node.map_builder_bridge()
+                    ->sensor_bridge(trajectory_id)
+                    ->HandlePointCloud2Message(topic, msg);
+              })));
+      expected_sensor_ids.insert(topic);
+    }
+  }
+
+  // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
+  // required.
+  ::ros::Subscriber imu_subscriber;
+  if (options.map_builder_options.use_trajectory_builder_3d() ||
+      (options.map_builder_options.use_trajectory_builder_2d() &&
+       options.map_builder_options.trajectory_builder_2d_options()
+           .use_imu_data())) {
+    imu_subscriber = node.node_handle()->subscribe(
+        kImuTopic, kInfiniteSubscriberQueueSize,
+        boost::function<void(const sensor_msgs::Imu::ConstPtr& msg)>(
+            [&](const sensor_msgs::Imu::ConstPtr& msg) {
+              node.map_builder_bridge()
+                  ->sensor_bridge(trajectory_id)
+                  ->HandleImuMessage(kImuTopic, msg);
+            }));
+    expected_sensor_ids.insert(kImuTopic);
+  }
+
+  // For both 2D and 3D SLAM, odometry is optional.
+  ::ros::Subscriber odometry_subscriber;
+  if (options.use_odometry) {
+    odometry_subscriber = node.node_handle()->subscribe(
+        kOdometryTopic, kInfiniteSubscriberQueueSize,
+        boost::function<void(const nav_msgs::Odometry::ConstPtr&)>(
+            [&](const nav_msgs::Odometry::ConstPtr& msg) {
+              node.map_builder_bridge()
+                  ->sensor_bridge(trajectory_id)
+                  ->HandleOdometryMessage(kOdometryTopic, msg);
+            }));
+    expected_sensor_ids.insert(kOdometryTopic);
+  }
+
+  trajectory_id = node.map_builder_bridge()->AddTrajectory(
+      expected_sensor_ids, options.tracking_frame);
+
+  ::ros::ServiceServer finish_trajectory_server =
+      node.node_handle()->advertiseService(
+          kFinishTrajectoryServiceName,
+          boost::function<bool(
+              ::cartographer_ros_msgs::FinishTrajectory::Request&,
+              ::cartographer_ros_msgs::FinishTrajectory::Response&)>(
+              [&](::cartographer_ros_msgs::FinishTrajectory::Request& request,
+                  ::cartographer_ros_msgs::FinishTrajectory::Response&) {
+                const int previous_trajectory_id = trajectory_id;
+                trajectory_id = node.map_builder_bridge()->AddTrajectory(
+                    expected_sensor_ids, options.tracking_frame);
+                node.map_builder_bridge()->FinishTrajectory(
+                    previous_trajectory_id);
+                node.map_builder_bridge()->WriteAssets(request.stem);
+                return true;
+              }));
+
   node.Initialize();
-  node.SpinForever();
+  node.Spin();
+
+  node.map_builder_bridge()->FinishTrajectory(trajectory_id);
 }
 
 }  // namespace