suppress chattering

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
autowarefoundation · Aug 1, 2023 · 60e6ba6 · 60e6ba6
1 parent 58354db
commit 60e6ba6
Showing 1 changed file with 14 additions and 2 deletions.
diff --git 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;
         }