feat(intersection): use different expected deceleration for bike/car (a…

…utowarefoundation#6328) Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
tier4 · Feb 8, 2024 · 8ada280 · 8ada280
1 parent cc4ceaa
commit 8ada280
Show file tree

Hide file tree

Showing 4 changed files with 25 additions and 9 deletions.
diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml
@@ -56,7 +56,9 @@
           duration: 3.0
           object_dist_to_stopline: 10.0
         ignore_on_amber_traffic_light:
-          object_expected_deceleration: 2.0
+          object_expected_deceleration:
+            car: 2.0
+            bike: 5.0
         ignore_on_red_traffic_light:
           object_margin_to_path: 2.0
         avoid_collision_by_acceleration:

diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp
@@ -127,9 +127,14 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
   ip.collision_detection.yield_on_green_traffic_light.object_dist_to_stopline =
     getOrDeclareParameter<double>(
       node, ns + ".collision_detection.yield_on_green_traffic_light.object_dist_to_stopline");
-  ip.collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration =
+  ip.collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration.car =
     getOrDeclareParameter<double>(
-      node, ns + ".collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration");
+      node,
+      ns + ".collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration.car");
+  ip.collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration.bike =
+    getOrDeclareParameter<double>(
+      node,
+      ns + ".collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration.bike");
   ip.collision_detection.ignore_on_red_traffic_light.object_margin_to_path =
     getOrDeclareParameter<double>(
       node, ns + ".collision_detection.ignore_on_red_traffic_light.object_margin_to_path");

diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
@@ -123,7 +123,11 @@ class IntersectionModule : public SceneModuleInterface
       } yield_on_green_traffic_light;
       struct IgnoreOnAmberTrafficLight
       {
-        double object_expected_deceleration;
+        struct ObjectExpectedDeceleration
+        {
+          double car;
+          double bike;
+        } object_expected_deceleration;
       } ignore_on_amber_traffic_light;
       struct IgnoreOnRedTrafficLight
       {

diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp
@@ -176,18 +176,23 @@ void IntersectionModule::updateObjectInfoManagerCollision(
   for (auto & object_info : object_info_manager_.attentionObjects()) {
     const auto & predicted_object = object_info->predicted_object();
     bool safe_under_traffic_control = false;
+    const auto label = predicted_object.classification.at(0).label;
+    const auto expected_deceleration =
+      (label == autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE ||
+       label == autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE)
+        ? planner_param_.collision_detection.ignore_on_amber_traffic_light
+            .object_expected_deceleration.bike
+        : planner_param_.collision_detection.ignore_on_amber_traffic_light
+            .object_expected_deceleration.car;
     if (
       traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED &&
-      object_info->can_stop_before_stopline(
-        planner_param_.collision_detection.ignore_on_amber_traffic_light
-          .object_expected_deceleration)) {
+      object_info->can_stop_before_stopline(expected_deceleration)) {
       safe_under_traffic_control = true;
     }
     if (
       traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED &&
       object_info->can_stop_before_ego_lane(
-        planner_param_.collision_detection.ignore_on_amber_traffic_light
-          .object_expected_deceleration,
+        expected_deceleration,
         planner_param_.collision_detection.ignore_on_red_traffic_light.object_margin_to_path,
         ego_lane)) {
       safe_under_traffic_control = true;