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 7486b59b15191..3560dfc740ef8 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -270,7 +270,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 fcd9c8e3d9eb0..9306fc14056da 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); } @@ -2463,12 +2465,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 15711c91a7547..982d4683099e2 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp @@ -337,7 +337,7 @@ MarkerArray createAvoidanceObjectsMarkerArray( marker.action = Marker::ADD; marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; marker.scale = tier4_autoware_utils::createMarkerScale(3.0, 1.5, 1.5); - marker.color = object.lost_count == 0 ? normal_color : disappearing_color; + marker.color = std::fabs(object.lost_time) < 1e-2 ? normal_color : disappearing_color; msg.markers.push_back(marker); }