RSDK-1753 - Fix localization mode (#163)

viamrobotics · Feb 24, 2023 · b293014 · b293014
1 parent ff6184c
commit b293014
Show file tree

Hide file tree

Showing 5 changed files with 28 additions and 7 deletions.
diff --git a/slam-libraries/viam-cartographer/lua_files/locating_in_map.lua b/slam-libraries/viam-cartographer/lua_files/locating_in_map.lua
@@ -1,7 +1,10 @@
 include "trajectory_builder.lua"
 include "map_builder.lua"
 
--- ++++ ALWAYS TRY TO TUNE FOR A PLATFORM, NOT A PARTICULAR BAG ++++ --
+-- !!!!! NOTE: DO NOT CHANGE THIS FILE !!!!! --
+
+-- ALWAYS TRY TO TUNE FOR A SENSOR SET/ROBOT SETUP,
+-- NOT A PARTICULAR ENVIRONMENT --
 
 -- ===== Local SLAM Options ======
 -- no reason to change these:

diff --git a/slam-libraries/viam-cartographer/lua_files/mapping_new_map.lua b/slam-libraries/viam-cartographer/lua_files/mapping_new_map.lua
@@ -1,7 +1,10 @@
 include "trajectory_builder.lua"
 include "map_builder.lua"
 
--- ++++ ALWAYS TRY TO TUNE FOR A PLATFORM, NOT A PARTICULAR BAG ++++ --
+-- !!!!! NOTE: DO NOT CHANGE THIS FILE !!!!! --
+
+-- ALWAYS TRY TO TUNE FOR A SENSOR SET/ROBOT SETUP,
+-- NOT A PARTICULAR ENVIRONMENT --
 
 -- ===== Local SLAM Options ======
 -- no reason to change these:

diff --git a/slam-libraries/viam-cartographer/lua_files/updating_a_map.lua b/slam-libraries/viam-cartographer/lua_files/updating_a_map.lua
@@ -1,7 +1,10 @@
 include "trajectory_builder.lua"
 include "map_builder.lua"
 
--- ++++ ALWAYS TRY TO TUNE FOR A PLATFORM, NOT A PARTICULAR BAG ++++ --
+-- !!!!! NOTE: DO NOT CHANGE THIS FILE !!!!! --
+
+-- ALWAYS TRY TO TUNE FOR A SENSOR SET/ROBOT SETUP,
+-- NOT A PARTICULAR ENVIRONMENT --
 
 -- ===== Local SLAM Options ======
 -- no reason to change these:

diff --git a/slam-libraries/viam-cartographer/src/slam_service/slam_service.cc b/slam-libraries/viam-cartographer/src/slam_service/slam_service.cc
@@ -755,11 +755,12 @@ void SLAMServiceImpl::PaintMarker(
     viam::io::DrawPoseOnSurface(painted_slices, global_pose, kPixelSize);
 }
 
-void SLAMServiceImpl::RunSLAM() {
-    LOG(INFO) << "Setting up cartographer";
+double SLAMServiceImpl::SetUpSLAM() {
+    // Setting the action mode has to happen before setting up the
+    // map builder.
+    SetActionMode();
     // Set up and build the MapBuilder
     SetUpMapBuilder();
-    SetActionMode();
 
     double data_start_time = 0;
     if (action_mode == ActionMode::UPDATING ||
@@ -793,7 +794,12 @@ void SLAMServiceImpl::RunSLAM() {
 
         CacheMapInLocalizationMode();
     }
+    return data_start_time;
+}
 
+void SLAMServiceImpl::RunSLAM() {
+    LOG(INFO) << "Setting up cartographer";
+    double data_start_time = SetUpSLAM();
     LOG(INFO) << "Starting to run cartographer";
     ProcessDataAndStartSavingMaps(data_start_time);
     LOG(INFO) << "Done running cartographer";

diff --git a/slam-libraries/viam-cartographer/src/slam_service/slam_service.h b/slam-libraries/viam-cartographer/src/slam_service/slam_service.h
@@ -254,9 +254,15 @@ class SLAMServiceImpl final : public SLAMService::Service {
     void ProcessDataAndStartSavingMaps(double data_cutoff_time);
 
     // SetUpMapBuilder loads the lua file with default cartographer config
-    // parameters depending on the action mode.
+    // parameters depending on the action mode. Setting the correct action
+    // mode has to happen before calling this function.
     void SetUpMapBuilder();
 
+    // SetUpSLAM sets the correct action mode, prepares the map builder and
+    // loads the right hyperparameters based on the action mode. Needs to be
+    // called before running slam.
+    double SetUpSLAM();
+
     // GetLatestPaintedMapSlices paints and returns the current map of
     // Cartographer
     cartographer::io::PaintSubmapSlicesResult GetLatestPaintedMapSlices();