remove previous obstacles after a timer

ut-amrl · Apr 12, 2024 · 58ad0f1 · 58ad0f1
1 parent a731075
commit 58ad0f1
Show file tree

Hide file tree

Showing 4 changed files with 9 additions and 2 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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