From 58ad0f1fc8dc389fdd985111e990b109afba4076 Mon Sep 17 00:00:00 2001
From: zdeng-UTexas <zdeng@utexas.edu>
Date: Fri, 12 Apr 2024 18:28:10 -0400
Subject: [PATCH] remove previous obstacles after a timer

---
 config/navigation.lua                  | 1 +
 src/navigation/navigation.cc           | 2 +-
 src/navigation/navigation_main.cc      | 2 ++
 src/navigation/navigation_parameters.h | 6 +++++-
 4 files changed, 9 insertions(+), 2 deletions(-)

diff --git a/config/navigation.lua b/config/navigation.lua
index 4ae3ead..ddd48bd 100644
--- a/config/navigation.lua
+++ b/config/navigation.lua
@@ -52,6 +52,7 @@ NavigationParameters = {
   range_min = 0.1;
   range_max = 10.0;
   replan_carrot_dist = 2;
+  object_lifespan = 5
 };
 
 AckermannSampler = {
diff --git a/src/navigation/navigation.cc b/src/navigation/navigation.cc
index 1376237..9a0f254 100644
--- a/src/navigation/navigation.cc
+++ b/src/navigation/navigation.cc
@@ -1243,7 +1243,7 @@ bool Navigation::Run(const double& time,
     bool in_map = costmap_.worldToMap(new_relative_point.x(), new_relative_point.y(), unsigned_mx, unsigned_my);
     uint32_t index = costmap_.getIndex(unsigned_mx, unsigned_my);
     //TODO: change expiration time to parameter
-    if (in_map && empty_cells.count(index) == 0 && std::time(nullptr) - obs.last_seen < 5){
+    if (in_map && empty_cells.count(index) == 0 && std::time(nullptr) - obs.last_seen < params_.object_lifespan){
       obstacle_cells.insert(index);
       if(index_to_obstacle.count(index) == 0){
         SeenObstacle new_obs;
diff --git a/src/navigation/navigation_main.cc b/src/navigation/navigation_main.cc
index 664b2dd..8c8212e 100644
--- a/src/navigation/navigation_main.cc
+++ b/src/navigation/navigation_main.cc
@@ -800,6 +800,7 @@ void LoadConfig(navigation::NavigationParameters* params) {
   REAL_PARAM(range_min);
   REAL_PARAM(range_max);
   REAL_PARAM(replan_carrot_dist);
+  REAL_PARAM(object_lifespan);
 
   config_reader::ConfigReader reader({FLAGS_robot_config});
   params->dt = CONFIG_dt;
@@ -842,6 +843,7 @@ void LoadConfig(navigation::NavigationParameters* params) {
   params->range_min = CONFIG_range_min;
   params->range_max = CONFIG_range_max;
   params->replan_carrot_dist = CONFIG_replan_carrot_dist;
+  params->object_lifespan = CONFIG_object_lifespan;
 
   // TODO Rather than loading camera homography from a file, compute it from camera transformation info
   LoadCameraCalibrationCV(CONFIG_camera_calibration_path, &params->K, &params->D, &params->H);
diff --git a/src/navigation/navigation_parameters.h b/src/navigation/navigation_parameters.h
index 284423e..e250808 100644
--- a/src/navigation/navigation_parameters.h
+++ b/src/navigation/navigation_parameters.h
@@ -123,6 +123,9 @@ struct NavigationParameters {
   // Distance between intermediate goal and global carrot before replanning
   float replan_carrot_dist;
 
+  // How long an object should stay in the costmap if not continuously observed
+  float object_lifespan;
+
 
   cv::Mat K;
   cv::Mat D;
@@ -161,7 +164,8 @@ struct NavigationParameters {
       global_costmap_origin_y(-100),
       range_min(0.1),
       range_max(10),
-      replan_carrot_dist(2){
+      replan_carrot_dist(2),
+      object_lifespan(5){
       }
 };
 }  // namespace navigation