realtime mode is now adaptive to sensor freq

rvp-group · Sep 11, 2024 · 60511d1 · 60511d1
1 parent 0784d80
commit 60511d1
Show file tree

Hide file tree

Showing 3 changed files with 4 additions and 3 deletions.
diff --git a/mad_icp/apps/mad_icp.py b/mad_icp/apps/mad_icp.py
@@ -94,8 +94,7 @@ def main(data_path: Annotated[
         visualizer = Visualizer()
 
     reader_type = InputDataInterface.kitti
-    print(data_path)
-    print(list(data_path.glob("*.db3")))
+
     if len(list(data_path.glob("*.bag"))) != 0:
         console.print("[yellow] The dataset is in ros bag format")
         reader_type = InputDataInterface.ros1

diff --git a/mad_icp/src/odometry/pipeline.cpp b/mad_icp/src/odometry/pipeline.cpp
@@ -59,6 +59,7 @@ Pipeline::Pipeline(double sensor_hz,
   seq_keyframe_   = 0;
   is_initialized_ = false;
   is_map_updated_ = false;
+  loop_time = (1. / sensor_hz_) * 1000;
 
   max_parallel_levels_ = static_cast<int>(std::log2(num_threads));
   omp_set_num_threads(num_threads);
@@ -163,7 +164,7 @@ void Pipeline::compute(const double& curr_stamp, ContainerType curr_cloud_mem) {
   struct timeval icp_start, icp_end, icp_delta;
 
   for (size_t icp_iteration = 0; icp_iteration < MAX_ICP_ITS; ++icp_iteration) {
-    const float remaining_time = 90.0 - (preprocessing_time + total_icp_time + icp_time);
+    const float remaining_time = loop_time - 5.0 - (preprocessing_time + total_icp_time + icp_time);
     if (realtime_ && remaining_time < 0)
       break;
 

diff --git a/mad_icp/src/odometry/pipeline.h b/mad_icp/src/odometry/pipeline.h
@@ -99,4 +99,5 @@ class Pipeline {
   size_t seq_keyframe_;
   bool is_initialized_;
   bool is_map_updated_;
+  float loop_time;
 };