Follow cartographer-project/cartographer/pull/724. (#616)

cartographer-project · Dec 4, 2017 · 4dac7c3 · 4dac7c3
1 parent e1f65f3
commit 4dac7c3
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 21 deletions.
diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc
@@ -65,26 +65,7 @@ void PushAndResetLineMarker(visualization_msgs::Marker* marker,
 MapBuilderBridge::MapBuilderBridge(const NodeOptions& node_options,
                                    tf2_ros::Buffer* const tf_buffer)
     : node_options_(node_options),
-      map_builder_(
-          node_options.map_builder_options,
-          cartographer::mapping::MapBuilder::LocalSlamResultCallback(
-              [this](
-                  const int trajectory_id,
-                  const ::cartographer::common::Time time,
-                  const ::cartographer::transform::Rigid3d local_pose,
-                  ::cartographer::sensor::RangeData range_data_in_local,
-                  const std::unique_ptr<const ::cartographer::mapping::NodeId>)
-                  EXCLUDES(mutex_) {
-                    std::shared_ptr<const TrajectoryState::LocalSlamData>
-                        local_slam_data =
-                            std::make_shared<TrajectoryState::LocalSlamData>(
-                                TrajectoryState::LocalSlamData{
-                                    time, local_pose,
-                                    std::move(range_data_in_local)});
-                    cartographer::common::MutexLocker lock(&mutex_);
-                    trajectory_state_data_[trajectory_id] =
-                        std::move(local_slam_data);
-                  })),
+      map_builder_(node_options.map_builder_options),
       tf_buffer_(tf_buffer) {}
 
 void MapBuilderBridge::LoadMap(const std::string& map_filename) {
@@ -97,7 +78,11 @@ int MapBuilderBridge::AddTrajectory(
     const std::unordered_set<std::string>& expected_sensor_ids,
     const TrajectoryOptions& trajectory_options) {
   const int trajectory_id = map_builder_.AddTrajectoryBuilder(
-      expected_sensor_ids, trajectory_options.trajectory_builder_options);
+      expected_sensor_ids, trajectory_options.trajectory_builder_options,
+      ::std::bind(&MapBuilderBridge::OnLocalSlamResult, this,
+                  ::std::placeholders::_1, ::std::placeholders::_2,
+                  ::std::placeholders::_3, ::std::placeholders::_4,
+                  ::std::placeholders::_5));
   LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";
 
   // Make sure there is no trajectory with 'trajectory_id' yet.
@@ -338,4 +323,17 @@ SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) {
   return sensor_bridges_.at(trajectory_id).get();
 }
 
+void MapBuilderBridge::OnLocalSlamResult(
+    const int trajectory_id, const ::cartographer::common::Time time,
+    const ::cartographer::transform::Rigid3d local_pose,
+    ::cartographer::sensor::RangeData range_data_in_local,
+    const std::unique_ptr<const ::cartographer::mapping::NodeId>) {
+  std::shared_ptr<const TrajectoryState::LocalSlamData> local_slam_data =
+      std::make_shared<TrajectoryState::LocalSlamData>(
+          TrajectoryState::LocalSlamData{time, local_pose,
+                                         std::move(range_data_in_local)});
+  cartographer::common::MutexLocker lock(&mutex_);
+  trajectory_state_data_[trajectory_id] = std::move(local_slam_data);
+}
+
 }  // namespace cartographer_ros
diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.h b/cartographer_ros/cartographer_ros/map_builder_bridge.h
@@ -78,6 +78,13 @@ class MapBuilderBridge {
   SensorBridge* sensor_bridge(int trajectory_id);
 
  private:
+  void OnLocalSlamResult(
+      const int trajectory_id, const ::cartographer::common::Time time,
+      const ::cartographer::transform::Rigid3d local_pose,
+      ::cartographer::sensor::RangeData range_data_in_local,
+      const std::unique_ptr<const ::cartographer::mapping::NodeId>)
+      EXCLUDES(mutex_); 
+
   cartographer::common::Mutex mutex_;
   const NodeOptions node_options_;
   std::unordered_map<int, std::shared_ptr<const TrajectoryState::LocalSlamData>>