RSDK-3323 - addSensorReading refactor

viam-cartographer/src/slam_service/slam_service.cc

@@ -647,6 +647,23 @@ void SLAMServiceImpl::SaveMapWithTimestamp() {
     }
 }
 
+cartographer::transform::Rigid3d SLAMServiceImpl::AddSensorReading(
+    cartographer::sensor::TimedPointCloudData measurement,
+    cartographer::mapping::TrajectoryBuilderInterface *trajectory_builder,
+    int trajectory_id, cartographer::transform::Rigid3d tmp_global_pose) {
+    std::lock_guard<std::mutex> lk(map_builder_mutex);
+
+    if (measurement.ranges.size() > 0) {
+        trajectory_builder->AddSensorData(kRangeSensorId.id, measurement);
+        auto local_poses = map_builder.GetLocalSlamResultPoses();
+        if (local_poses.size() > 0) {
+            tmp_global_pose =
+                map_builder.GetGlobalPose(trajectory_id, local_poses.back());
+        }
+    }
+    return tmp_global_pose;
+}
+
 void SLAMServiceImpl::ProcessDataAndStartSavingMaps(double data_start_time) {
     // Prepare the trajectory builder and grab the active trajectory_id
     cartographer::mapping::TrajectoryBuilderInterface *trajectory_builder;
@@ -705,19 +722,10 @@ void SLAMServiceImpl::ProcessDataAndStartSavingMaps(double data_start_time) {
             StartSaveMap();
         }
         // Add data to the map_builder to add to the map
-        {
-            std::lock_guard<std::mutex> lk(map_builder_mutex);
-            auto measurement = map_builder.GetDataFromFile(file);
-            if (measurement.ranges.size() > 0) {
-                trajectory_builder->AddSensorData(kRangeSensorId.id,
-                                                  measurement);
-                auto local_poses = map_builder.GetLocalSlamResultPoses();
-                if (local_poses.size() > 0) {
-                    tmp_global_pose = map_builder.GetGlobalPose(
-                        trajectory_id, local_poses.back());
-                }
-            }
-        }
+        auto measurement = map_builder.GetDataFromFile(file);
+        tmp_global_pose = AddSensorReading(measurement, trajectory_builder,
+                                           trajectory_id, tmp_global_pose);
+
         // Save a copy of the global pose
         {
             std::lock_guard<std::mutex> lk(viam_response_mutex);

viam-cartographer/src/slam_service/slam_service.h

@@ -182,6 +182,12 @@ class SLAMServiceImpl final : public SLAMService::Service {
     // string if it fails.
     std::string TryFileClose(std::ifstream &file, std::string filename);
 
+    // AddSensorReading adds sensor readings to the map builder.
+    cartographer::transform::Rigid3d AddSensorReading(
+        cartographer::sensor::TimedPointCloudData measurement,
+        cartographer::mapping::TrajectoryBuilderInterface *trajectory_builder,
+        int trajectory_id, cartographer::transform::Rigid3d tmp_global_pose);
+
     // ProcessDataAndStartSavingMaps processes the data in the data directory
     // that is newer than the provided data_cutoff_time
     // and starts the process to save maps in parallel. In offline mode,