RSDK-3323 - addSensorReading refactor

viam-cartographer/src/slam_service/slam_service.cc

@@ -647,10 +647,34 @@ void SLAMServiceImpl::SaveMapWithTimestamp() {
     }
 }
 
+bool SLAMServiceImpl::AddSensorReading(
+    cartographer::sensor::TimedPointCloudData measurement) {
+    cartographer::transform::Rigid3d tmp_global_pose;
+    bool update_latest_global_pose = false;
+
+    if (map_builder_mutex.try_lock()) {
+        if (measurement.ranges.size() > 0) {
+            trajectory_builder->AddSensorData(kRangeSensorId.id, measurement);
+            auto local_poses = map_builder.GetLocalSlamResultPoses();
+            if (local_poses.size() > 0) {
+                update_latest_global_pose = true;
+                tmp_global_pose = map_builder.GetGlobalPose(trajectory_id,
+                                                            local_poses.back());
+            }
+        }
+        map_builder_mutex.unlock();
+        if (update_latest_global_pose) {
+            std::lock_guard<std::mutex> lk(viam_response_mutex);
+            latest_global_pose = tmp_global_pose;
+        }
+        return true;
+    } else {
+        return false;
+    }
+}
+
 void SLAMServiceImpl::ProcessDataAndStartSavingMaps(double data_start_time) {
     // Prepare the trajectory builder and grab the active trajectory_id
-    cartographer::mapping::TrajectoryBuilderInterface *trajectory_builder;
-    int trajectory_id;
     {
         std::lock_guard<std::mutex> lk(map_builder_mutex);
         // Set TrajectoryBuilder
@@ -664,9 +688,6 @@ void SLAMServiceImpl::ProcessDataAndStartSavingMaps(double data_start_time) {
     bool set_start_time = false;
     auto file = GetNextDataFile();
 
-    // Define tmp_global_pose here so it always has the previous pose
-    cartographer::transform::Rigid3d tmp_global_pose =
-        cartographer::transform::Rigid3d();
     while (file != "") {
         // Ignore files that are not *.pcd files
         if (file.find(".pcd") == std::string::npos) {
@@ -704,25 +725,10 @@ void SLAMServiceImpl::ProcessDataAndStartSavingMaps(double data_start_time) {
             LOG(INFO) << "Starting to save maps...";
             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());
-                }
-            }
-        }
-        // Save a copy of the global pose
-        {
-            std::lock_guard<std::mutex> lk(viam_response_mutex);
-            latest_global_pose = tmp_global_pose;
-        }
+        // Add data to the map_builder to add to the map and save a copy of the
+        // global pose
+        auto measurement = map_builder.GetDataFromFile(file);
+        AddSensorReading(measurement);
 
         // This log line is needed by rdk integration tests.
         VLOG(1) << "Passed sensor data to SLAM " << file;
@@ -741,6 +747,8 @@ void SLAMServiceImpl::ProcessDataAndStartSavingMaps(double data_start_time) {
     if (!use_live_data) {
         // We still want to optimize the map in localization mode, but we do not
         // need to update the backup of the map
+        cartographer::transform::Rigid3d tmp_global_pose;
+        bool update_latest_global_pose = false;
         if (action_mode != ActionMode::LOCALIZING) BackupLatestMap();
         {
             std::unique_lock<std::shared_mutex> optimization_lock(
@@ -754,12 +762,13 @@ void SLAMServiceImpl::ProcessDataAndStartSavingMaps(double data_start_time) {
 
             auto local_poses = map_builder.GetLocalSlamResultPoses();
             if (local_poses.size() > 0) {
+                update_latest_global_pose = true;
                 tmp_global_pose = map_builder.GetGlobalPose(trajectory_id,
                                                             local_poses.back());
             }
         }
 
-        {
+        if (update_latest_global_pose) {
             std::lock_guard<std::mutex> lk(viam_response_mutex);
             latest_global_pose = tmp_global_pose;
         }

viam-cartographer/src/slam_service/slam_service.h

@@ -182,6 +182,10 @@ 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.
+    bool AddSensorReading(
+        cartographer::sensor::TimedPointCloudData measurement);
+
     // 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,
@@ -238,6 +242,8 @@ class SLAMServiceImpl final : public SLAMService::Service {
     std::shared_mutex optimization_shared_mutex;
     std::mutex map_builder_mutex;
     mapping::MapBuilder map_builder;
+    cartographer::mapping::TrajectoryBuilderInterface *trajectory_builder;
+    int trajectory_id;
 
     std::atomic<bool> finished_processing_offline{false};
     std::thread *thread_save_map_with_timestamp;