fix(behavior_path_planner): replace lost count to lost time (autoware…

…foundation#1142) The PR aims to replace integer count to time-based. Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
tzhong518 · Oct 14, 2022 · 41f7a1c · 41f7a1c
1 parent 492149c
commit 41f7a1c
Show file tree

Hide file tree

Showing 5 changed files with 17 additions and 10 deletions.
diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml
@@ -32,7 +32,7 @@
       nominal_lateral_jerk: 0.2   # [m/s3]
       max_lateral_jerk: 1.0       # [m/s3]
 
-      object_hold_max_count: 20
+      object_last_seen_threshold: 2.0
 
       # For prevention of large acceleration while avoidance
       min_avoidance_speed_for_acc_prevention: 3.0  # [m/s]

diff --git a/...th_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp b/...th_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp
@@ -134,7 +134,7 @@ struct AvoidanceParameters
   // For the compensation of the detection lost. Once an object is observed, it is registered and
   // will be used for planning from the next time. If the object is not observed, it counts up the
   // lost_count and the registered object will be removed when the count exceeds this max count.
-  int object_hold_max_count;
+  double object_last_seen_threshold;
 
   // For velocity planning to avoid acceleration during avoidance.
   // Speeds smaller than this are not inserted.
@@ -194,7 +194,8 @@ struct ObjectData  // avoidance target
   double overhang_dist;
 
   // count up when object disappeared. Removed when it exceeds threshold.
-  int lost_count = 0;
+  rclcpp::Time last_seen;
+  double lost_time{0.0};
 
   // store the information of the lanelet which the object's overhang is currently occupying
   lanelet::ConstLanelet overhang_lanelet;

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -271,7 +271,7 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
     dp("longitudinal_collision_margin_min_distance", 0.0);
   p.longitudinal_collision_margin_time = dp("longitudinal_collision_margin_time", 0.0);
 
-  p.object_hold_max_count = dp("object_hold_max_count", 0);
+  p.object_last_seen_threshold = dp("object_last_seen_threshold", 2.0);
 
   p.min_avoidance_speed_for_acc_prevention = dp("min_avoidance_speed_for_acc_prevention", 3.0);
   p.max_avoidance_acceleration = dp("max_avoidance_acceleration", 0.5);

diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
@@ -291,6 +291,8 @@ ObjectDataArray AvoidanceModule::calcAvoidanceTargetObjects(
       continue;
     }
 
+    object_data.last_seen = clock_->now();
+
     // set data
     target_objects.push_back(object_data);
   }
@@ -2475,12 +2477,15 @@ void AvoidanceModule::updateRegisteredObject(const ObjectDataArray & now_objects
 
     // registered object is not detected this time. lost count up.
     if (!updateIfDetectedNow(r)) {
-      ++r.lost_count;
+      r.lost_time = (clock_->now() - r.last_seen).seconds();
+    } else {
+      r.last_seen = clock_->now();
+      r.lost_time = 0.0;
+    }
 
-      // lost count exceeds threshold. remove object from register.
-      if (r.lost_count > parameters_.object_hold_max_count) {
-        registered_objects_.erase(registered_objects_.begin() + i);
-      }
+    // lost count exceeds threshold. remove object from register.
+    if (r.lost_time > parameters_.object_last_seen_threshold) {
+      registered_objects_.erase(registered_objects_.begin() + i);
     }
   }
 

diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp
@@ -96,7 +96,8 @@ MarkerArray createAvoidanceObjectsMarkerArray(
   for (const auto & object : objects) {
     marker.id = ++i;
     marker.pose = object.object.kinematics.initial_pose_with_covariance.pose;
-    marker.color = object.lost_count == 0 ? normal_color : disappearing_color;
+    marker.scale = tier4_autoware_utils::createMarkerScale(3.0, 1.5, 1.5);
+    marker.color = std::fabs(object.lost_time) < 1e-2 ? normal_color : disappearing_color;
     msg.markers.push_back(marker);
   }