From c218fd7a1af2263d510567e71e68a2f9d93df63e Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 7 Jul 2022 14:16:58 +0900 Subject: [PATCH] fix(behavior_path_planner): replace lost count to lost time (#1142) The PR aims to replace integer count to time-based. Signed-off-by: Muhammad Zulfaqar Azmi --- .../config/avoidance/avoidance.param.yaml | 2 +- .../avoidance/avoidance_module_data.hpp | 5 +++-- .../src/behavior_path_planner_node.cpp | 2 +- .../scene_module/avoidance/avoidance_module.cpp | 15 ++++++++++----- .../src/scene_module/avoidance/debug.cpp | 3 ++- 5 files changed, 17 insertions(+), 10 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 63f3e1438cdd5..b4adda24448e7 100644 --- 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/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp index ea891ce9bec89..1411ce8ae3aff 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_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 index 9867976adc648..45c31caefcce8 100644 --- 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 index 9d4ef23a6d596..9f20a2aa28be2 100644 --- 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 index afcf2b940c9d1..bde0b94ccb367 100644 --- 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); }