From 60e6ba6c00d159bc49df4957ec09688a05319ea0 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 31 Jul 2023 20:35:24 +0900 Subject: [PATCH] suppress chattering Signed-off-by: Takayuki Murooka --- .../src/scene_crosswalk.hpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 880f505e032d7..cd0f3b8c7f074 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -138,13 +138,25 @@ class CrosswalkModule : public SceneModuleInterface // Compare time to collision and vehicle if (collision_point) { + // Check if ego will pass first + const double ego_pass_first_additional_margin = + collision_state == CollisionState::EGO_PASS_FIRST + ? 0.0 + : planner_param.ego_pass_first_additional_margin; if ( - collision_point->time_to_collision + planner_param.ego_pass_first_margin < + collision_point->time_to_collision + planner_param.ego_pass_first_margin + + ego_pass_first_additional_margin < collision_point->time_to_vehicle) { collision_state = CollisionState::EGO_PASS_FIRST; } + // Check if ego will pass later + const double ego_pass_later_additional_margin = + collision_state == CollisionState::EGO_PASS_LATER + ? 0.0 + : planner_param.ego_pass_later_additional_margin; if ( - collision_point->time_to_vehicle + planner_param.ego_pass_later_margin < + collision_point->time_to_vehicle + planner_param.ego_pass_later_margin + + ego_pass_later_additional_margin < collision_point->time_to_collision) { collision_state = CollisionState::EGO_PASS_LATER; }