From a553862fa844ee39f43cb3ac5ff4f3bf40328a91 Mon Sep 17 00:00:00 2001 From: Kim Mishra Date: Fri, 26 May 2023 11:58:46 -0400 Subject: [PATCH 1/9] addSensorReading initial refactor --- .../src/slam_service/slam_service.cc | 36 ++++++++++++------- .../src/slam_service/slam_service.h | 3 ++ 2 files changed, 26 insertions(+), 13 deletions(-) diff --git a/viam-cartographer/src/slam_service/slam_service.cc b/viam-cartographer/src/slam_service/slam_service.cc index 5b4d6643..a643f0ab 100644 --- a/viam-cartographer/src/slam_service/slam_service.cc +++ b/viam-cartographer/src/slam_service/slam_service.cc @@ -647,6 +647,26 @@ 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 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 +725,9 @@ void SLAMServiceImpl::ProcessDataAndStartSavingMaps(double data_start_time) { StartSaveMap(); } // Add data to the map_builder to add to the map - { - std::lock_guard 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 lk(viam_response_mutex); diff --git a/viam-cartographer/src/slam_service/slam_service.h b/viam-cartographer/src/slam_service/slam_service.h index 3ad7260d..82d02be7 100644 --- a/viam-cartographer/src/slam_service/slam_service.h +++ b/viam-cartographer/src/slam_service/slam_service.h @@ -182,6 +182,9 @@ 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, From 35ebe17d7a42f4327caeb199326d1a0c8f361790 Mon Sep 17 00:00:00 2001 From: Kim Mishra Date: Fri, 26 May 2023 12:03:47 -0400 Subject: [PATCH 2/9] lint fixes --- .../src/slam_service/slam_service.cc | 20 +++++++++---------- .../src/slam_service/slam_service.h | 5 ++++- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/viam-cartographer/src/slam_service/slam_service.cc b/viam-cartographer/src/slam_service/slam_service.cc index a643f0ab..e8a7f2d0 100644 --- a/viam-cartographer/src/slam_service/slam_service.cc +++ b/viam-cartographer/src/slam_service/slam_service.cc @@ -648,20 +648,17 @@ 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 -) { + cartographer::sensor::TimedPointCloudData measurement, + cartographer::mapping::TrajectoryBuilderInterface *trajectory_builder, + int trajectory_id, cartographer::transform::Rigid3d tmp_global_pose) { std::lock_guard lk(map_builder_mutex); - + if (measurement.ranges.size() > 0) { - trajectory_builder->AddSensorData(kRangeSensorId.id, - measurement); + 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()); + tmp_global_pose = + map_builder.GetGlobalPose(trajectory_id, local_poses.back()); } } return tmp_global_pose; @@ -726,7 +723,8 @@ void SLAMServiceImpl::ProcessDataAndStartSavingMaps(double data_start_time) { } // Add data to the map_builder to add to the map auto measurement = map_builder.GetDataFromFile(file); - tmp_global_pose = AddSensorReading(measurement, trajectory_builder, trajectory_id, tmp_global_pose); + tmp_global_pose = AddSensorReading(measurement, trajectory_builder, + trajectory_id, tmp_global_pose); // Save a copy of the global pose { diff --git a/viam-cartographer/src/slam_service/slam_service.h b/viam-cartographer/src/slam_service/slam_service.h index 82d02be7..f6c62631 100644 --- a/viam-cartographer/src/slam_service/slam_service.h +++ b/viam-cartographer/src/slam_service/slam_service.h @@ -183,7 +183,10 @@ class SLAMServiceImpl final : public SLAMService::Service { 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); + 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 From fb514956feb9f1b7afbedd0c2b10ad674bd32ef7 Mon Sep 17 00:00:00 2001 From: Kim Mishra Date: Fri, 26 May 2023 11:58:46 -0400 Subject: [PATCH 3/9] addSensorReading initial refactor --- .../src/slam_service/slam_service.cc | 36 ++++++++++++------- .../src/slam_service/slam_service.h | 3 ++ 2 files changed, 26 insertions(+), 13 deletions(-) diff --git a/viam-cartographer/src/slam_service/slam_service.cc b/viam-cartographer/src/slam_service/slam_service.cc index 5b4d6643..a643f0ab 100644 --- a/viam-cartographer/src/slam_service/slam_service.cc +++ b/viam-cartographer/src/slam_service/slam_service.cc @@ -647,6 +647,26 @@ 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 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 +725,9 @@ void SLAMServiceImpl::ProcessDataAndStartSavingMaps(double data_start_time) { StartSaveMap(); } // Add data to the map_builder to add to the map - { - std::lock_guard 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 lk(viam_response_mutex); diff --git a/viam-cartographer/src/slam_service/slam_service.h b/viam-cartographer/src/slam_service/slam_service.h index 3ad7260d..82d02be7 100644 --- a/viam-cartographer/src/slam_service/slam_service.h +++ b/viam-cartographer/src/slam_service/slam_service.h @@ -182,6 +182,9 @@ 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, From f153d87616cb097bba1131a860170775196dc932 Mon Sep 17 00:00:00 2001 From: Kim Mishra Date: Fri, 26 May 2023 12:03:47 -0400 Subject: [PATCH 4/9] lint fixes --- .../src/slam_service/slam_service.cc | 20 +++++++++---------- .../src/slam_service/slam_service.h | 5 ++++- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/viam-cartographer/src/slam_service/slam_service.cc b/viam-cartographer/src/slam_service/slam_service.cc index a643f0ab..e8a7f2d0 100644 --- a/viam-cartographer/src/slam_service/slam_service.cc +++ b/viam-cartographer/src/slam_service/slam_service.cc @@ -648,20 +648,17 @@ 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 -) { + cartographer::sensor::TimedPointCloudData measurement, + cartographer::mapping::TrajectoryBuilderInterface *trajectory_builder, + int trajectory_id, cartographer::transform::Rigid3d tmp_global_pose) { std::lock_guard lk(map_builder_mutex); - + if (measurement.ranges.size() > 0) { - trajectory_builder->AddSensorData(kRangeSensorId.id, - measurement); + 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()); + tmp_global_pose = + map_builder.GetGlobalPose(trajectory_id, local_poses.back()); } } return tmp_global_pose; @@ -726,7 +723,8 @@ void SLAMServiceImpl::ProcessDataAndStartSavingMaps(double data_start_time) { } // Add data to the map_builder to add to the map auto measurement = map_builder.GetDataFromFile(file); - tmp_global_pose = AddSensorReading(measurement, trajectory_builder, trajectory_id, tmp_global_pose); + tmp_global_pose = AddSensorReading(measurement, trajectory_builder, + trajectory_id, tmp_global_pose); // Save a copy of the global pose { diff --git a/viam-cartographer/src/slam_service/slam_service.h b/viam-cartographer/src/slam_service/slam_service.h index 82d02be7..f6c62631 100644 --- a/viam-cartographer/src/slam_service/slam_service.h +++ b/viam-cartographer/src/slam_service/slam_service.h @@ -183,7 +183,10 @@ class SLAMServiceImpl final : public SLAMService::Service { 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); + 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 From e4f5337e6453d048b14d75c478b4bd22c69073bf Mon Sep 17 00:00:00 2001 From: Kim Mishra Date: Fri, 26 May 2023 14:04:13 -0400 Subject: [PATCH 5/9] moving latest global pose logic into add sensor reading --- .../src/slam_service/slam_service.cc | 20 +++++++++---------- .../src/slam_service/slam_service.h | 2 +- 2 files changed, 10 insertions(+), 12 deletions(-) diff --git a/viam-cartographer/src/slam_service/slam_service.cc b/viam-cartographer/src/slam_service/slam_service.cc index e8a7f2d0..1461b24c 100644 --- a/viam-cartographer/src/slam_service/slam_service.cc +++ b/viam-cartographer/src/slam_service/slam_service.cc @@ -647,7 +647,7 @@ void SLAMServiceImpl::SaveMapWithTimestamp() { } } -cartographer::transform::Rigid3d SLAMServiceImpl::AddSensorReading( +void SLAMServiceImpl::AddSensorReading( cartographer::sensor::TimedPointCloudData measurement, cartographer::mapping::TrajectoryBuilderInterface *trajectory_builder, int trajectory_id, cartographer::transform::Rigid3d tmp_global_pose) { @@ -661,7 +661,10 @@ cartographer::transform::Rigid3d SLAMServiceImpl::AddSensorReading( map_builder.GetGlobalPose(trajectory_id, local_poses.back()); } } - return tmp_global_pose; + { + std::lock_guard lk(viam_response_mutex); + latest_global_pose = tmp_global_pose; + } } void SLAMServiceImpl::ProcessDataAndStartSavingMaps(double data_start_time) { @@ -721,16 +724,11 @@ 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 + // 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); - tmp_global_pose = AddSensorReading(measurement, trajectory_builder, - trajectory_id, tmp_global_pose); - - // Save a copy of the global pose - { - std::lock_guard lk(viam_response_mutex); - latest_global_pose = tmp_global_pose; - } + AddSensorReading(measurement, trajectory_builder, trajectory_id, + tmp_global_pose); // This log line is needed by rdk integration tests. VLOG(1) << "Passed sensor data to SLAM " << file; diff --git a/viam-cartographer/src/slam_service/slam_service.h b/viam-cartographer/src/slam_service/slam_service.h index f6c62631..7bded0f9 100644 --- a/viam-cartographer/src/slam_service/slam_service.h +++ b/viam-cartographer/src/slam_service/slam_service.h @@ -183,7 +183,7 @@ class SLAMServiceImpl final : public SLAMService::Service { std::string TryFileClose(std::ifstream &file, std::string filename); // AddSensorReading adds sensor readings to the map builder. - cartographer::transform::Rigid3d AddSensorReading( + void AddSensorReading( cartographer::sensor::TimedPointCloudData measurement, cartographer::mapping::TrajectoryBuilderInterface *trajectory_builder, int trajectory_id, cartographer::transform::Rigid3d tmp_global_pose); From 9210f11f1a025a300f7368801338d523374c212e Mon Sep 17 00:00:00 2001 From: Kim Mishra Date: Fri, 26 May 2023 15:52:17 -0400 Subject: [PATCH 6/9] clean up tmp_global_pose and member variables --- .../src/slam_service/slam_service.cc | 38 +++++++++---------- .../src/slam_service/slam_service.h | 7 ++-- 2 files changed, 23 insertions(+), 22 deletions(-) diff --git a/viam-cartographer/src/slam_service/slam_service.cc b/viam-cartographer/src/slam_service/slam_service.cc index 1461b24c..1072bee9 100644 --- a/viam-cartographer/src/slam_service/slam_service.cc +++ b/viam-cartographer/src/slam_service/slam_service.cc @@ -648,20 +648,23 @@ void SLAMServiceImpl::SaveMapWithTimestamp() { } void SLAMServiceImpl::AddSensorReading( - cartographer::sensor::TimedPointCloudData measurement, - cartographer::mapping::TrajectoryBuilderInterface *trajectory_builder, - int trajectory_id, cartographer::transform::Rigid3d tmp_global_pose) { - std::lock_guard lk(map_builder_mutex); + cartographer::sensor::TimedPointCloudData measurement) { + cartographer::transform::Rigid3d tmp_global_pose; + bool update_latest_global_pose = false; - 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()); + { + std::lock_guard 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) { + update_latest_global_pose = true; + tmp_global_pose = map_builder.GetGlobalPose(trajectory_id, + local_poses.back()); + } } } - { + if (update_latest_global_pose) { std::lock_guard lk(viam_response_mutex); latest_global_pose = tmp_global_pose; } @@ -669,8 +672,6 @@ void SLAMServiceImpl::AddSensorReading( 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 lk(map_builder_mutex); // Set TrajectoryBuilder @@ -684,9 +685,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) { @@ -727,8 +725,7 @@ void SLAMServiceImpl::ProcessDataAndStartSavingMaps(double data_start_time) { // 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, trajectory_builder, trajectory_id, - tmp_global_pose); + AddSensorReading(measurement); // This log line is needed by rdk integration tests. VLOG(1) << "Passed sensor data to SLAM " << file; @@ -747,6 +744,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 optimization_lock( @@ -760,12 +759,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 lk(viam_response_mutex); latest_global_pose = tmp_global_pose; } diff --git a/viam-cartographer/src/slam_service/slam_service.h b/viam-cartographer/src/slam_service/slam_service.h index 7bded0f9..e319b4f6 100644 --- a/viam-cartographer/src/slam_service/slam_service.h +++ b/viam-cartographer/src/slam_service/slam_service.h @@ -184,9 +184,7 @@ class SLAMServiceImpl final : public SLAMService::Service { // AddSensorReading adds sensor readings to the map builder. void AddSensorReading( - cartographer::sensor::TimedPointCloudData measurement, - cartographer::mapping::TrajectoryBuilderInterface *trajectory_builder, - int trajectory_id, cartographer::transform::Rigid3d tmp_global_pose); + cartographer::sensor::TimedPointCloudData measurement); // ProcessDataAndStartSavingMaps processes the data in the data directory // that is newer than the provided data_cutoff_time @@ -258,6 +256,9 @@ class SLAMServiceImpl final : public SLAMService::Service { // started. std::string latest_pointcloud_map; // --- + + cartographer::mapping::TrajectoryBuilderInterface *trajectory_builder; + int trajectory_id; }; } // namespace viam From 2fb0f269fd40a8a0f3cb7516f38c42619c267b68 Mon Sep 17 00:00:00 2001 From: Kim Mishra Date: Tue, 30 May 2023 12:25:12 -0400 Subject: [PATCH 7/9] protect trajectory builder * trajectory id under map builder mutex --- viam-cartographer/src/slam_service/slam_service.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/viam-cartographer/src/slam_service/slam_service.h b/viam-cartographer/src/slam_service/slam_service.h index e319b4f6..54abf550 100644 --- a/viam-cartographer/src/slam_service/slam_service.h +++ b/viam-cartographer/src/slam_service/slam_service.h @@ -242,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 finished_processing_offline{false}; std::thread *thread_save_map_with_timestamp; @@ -257,8 +259,6 @@ class SLAMServiceImpl final : public SLAMService::Service { std::string latest_pointcloud_map; // --- - cartographer::mapping::TrajectoryBuilderInterface *trajectory_builder; - int trajectory_id; }; } // namespace viam From 986e18184ccf308d79a9c650d080302232ba9bfc Mon Sep 17 00:00:00 2001 From: Kim Mishra Date: Tue, 30 May 2023 15:55:15 -0400 Subject: [PATCH 8/9] return info about lock availibility --- .../src/slam_service/slam_service.cc | 17 ++++++++++------- .../src/slam_service/slam_service.h | 2 +- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/viam-cartographer/src/slam_service/slam_service.cc b/viam-cartographer/src/slam_service/slam_service.cc index 1072bee9..7da26ae1 100644 --- a/viam-cartographer/src/slam_service/slam_service.cc +++ b/viam-cartographer/src/slam_service/slam_service.cc @@ -647,13 +647,12 @@ void SLAMServiceImpl::SaveMapWithTimestamp() { } } -void SLAMServiceImpl::AddSensorReading( +bool SLAMServiceImpl::AddSensorReading( cartographer::sensor::TimedPointCloudData measurement) { cartographer::transform::Rigid3d tmp_global_pose; bool update_latest_global_pose = false; - { - std::lock_guard lk(map_builder_mutex); + if (map_builder_mutex.try_lock()) { if (measurement.ranges.size() > 0) { trajectory_builder->AddSensorData(kRangeSensorId.id, measurement); auto local_poses = map_builder.GetLocalSlamResultPoses(); @@ -663,10 +662,14 @@ void SLAMServiceImpl::AddSensorReading( local_poses.back()); } } - } - if (update_latest_global_pose) { - std::lock_guard lk(viam_response_mutex); - latest_global_pose = tmp_global_pose; + map_builder_mutex.unlock(); + if (update_latest_global_pose) { + std::lock_guard lk(viam_response_mutex); + latest_global_pose = tmp_global_pose; + } + return true; + } else { + return false; } } diff --git a/viam-cartographer/src/slam_service/slam_service.h b/viam-cartographer/src/slam_service/slam_service.h index 54abf550..bb50e372 100644 --- a/viam-cartographer/src/slam_service/slam_service.h +++ b/viam-cartographer/src/slam_service/slam_service.h @@ -183,7 +183,7 @@ class SLAMServiceImpl final : public SLAMService::Service { std::string TryFileClose(std::ifstream &file, std::string filename); // AddSensorReading adds sensor readings to the map builder. - void AddSensorReading( + bool AddSensorReading( cartographer::sensor::TimedPointCloudData measurement); // ProcessDataAndStartSavingMaps processes the data in the data directory From 3c646d21848a49a066e73b01ff23f2ec94c7a80d Mon Sep 17 00:00:00 2001 From: Kim Mishra Date: Tue, 30 May 2023 15:57:32 -0400 Subject: [PATCH 9/9] linter --- viam-cartographer/src/slam_service/slam_service.h | 1 - 1 file changed, 1 deletion(-) diff --git a/viam-cartographer/src/slam_service/slam_service.h b/viam-cartographer/src/slam_service/slam_service.h index bb50e372..cc0e5797 100644 --- a/viam-cartographer/src/slam_service/slam_service.h +++ b/viam-cartographer/src/slam_service/slam_service.h @@ -258,7 +258,6 @@ class SLAMServiceImpl final : public SLAMService::Service { // started. std::string latest_pointcloud_map; // --- - }; } // namespace viam