From 60511d1f9e18ee3b8f8e0fb152133bbcb4187b42 Mon Sep 17 00:00:00 2001
From: Leonardo Brizi <leobrizi.96@gmail.com>
Date: Wed, 11 Sep 2024 18:40:44 -0400
Subject: [PATCH] realtime mode is now adaptive to sensor freq

---
 mad_icp/apps/mad_icp.py           | 3 +--
 mad_icp/src/odometry/pipeline.cpp | 3 ++-
 mad_icp/src/odometry/pipeline.h   | 1 +
 3 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/mad_icp/apps/mad_icp.py b/mad_icp/apps/mad_icp.py
index 5056d30..75bfae2 100755
--- 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
index 48a2812..715f02f 100755
--- 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
index f5e4edd..ad7e158 100755
--- 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;
 };