feat(avoidance): return original lane as soon as possible if traffic is congested

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Author: satoshi-ota

planning/behavior_path_avoidance_module/config/avoidance.param.yaml

@@ -228,6 +228,9 @@
           traffic_light:
             enable: true                                # [-]
             buffer: 3.0                                 # [m]
+          traffic_jam:
+            enable: true                                # [-]
+            buffer: 8.0                                 # [m]
 
       # For cancel maneuver
       cancel:

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

@@ -134,10 +134,12 @@ struct AvoidanceParameters
   // consider avoidance return dead line
   bool enable_dead_line_for_goal{false};
   bool enable_dead_line_for_traffic_light{false};
+  bool enable_dead_line_for_traffic_jam{false};
 
   // module try to return original path to keep this distance from edge point of the path.
   double dead_line_buffer_for_goal{0.0};
   double dead_line_buffer_for_traffic_light{0.0};
+  double dead_line_buffer_for_traffic_jam{0.0};
 
   // max deceleration for
   double max_deceleration{0.0};

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp

@@ -283,9 +283,13 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
     p.enable_dead_line_for_goal = getOrDeclareParameter<bool>(*node, ns + "goal.enable");
     p.enable_dead_line_for_traffic_light =
       getOrDeclareParameter<bool>(*node, ns + "traffic_light.enable");
+    p.enable_dead_line_for_traffic_jam =
+      getOrDeclareParameter<bool>(*node, ns + "traffic_jam.enable");
     p.dead_line_buffer_for_goal = getOrDeclareParameter<double>(*node, ns + "goal.buffer");
     p.dead_line_buffer_for_traffic_light =
       getOrDeclareParameter<double>(*node, ns + "traffic_light.buffer");
+    p.dead_line_buffer_for_traffic_jam =
+      getOrDeclareParameter<double>(*node, ns + "traffic_jam.buffer");
   }
 
   // cancel

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp

@@ -166,7 +166,7 @@ DrivableLanes generateExpandedDrivableLanes(
 
 double calcDistanceToReturnDeadLine(
   const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path,
-  const std::shared_ptr<const PlannerData> & planner_data,
+  ObjectDataArray & other_objects, const std::shared_ptr<const PlannerData> & planner_data,
   const std::shared_ptr<AvoidanceParameters> & parameters);
 
 double calcDistanceToAvoidStartLine(

planning/behavior_path_avoidance_module/schema/avoidance.schema.json

@@ -1183,9 +1183,26 @@
                   },
                   "required": ["enable", "buffer"],
                   "additionalProperties": false
+                },
+                "traffic_jam": {
+                  "type": "object",
+                  "properties": {
+                    "enable": {
+                      "type": "boolean",
+                      "description": "Insert stop point in order to return original lane before reaching vehicles in traffic jam.",
+                      "default": "true"
+                    },
+                    "buffer": {
+                      "type": "number",
+                      "description": "Buffer distance to return original lane before reaching vehicles in traffic jam.",
+                      "default": 8.0
+                    }
+                  },
+                  "required": ["enable", "buffer"],
+                  "additionalProperties": false
                 }
               },
-              "required": ["goal", "traffic_light"],
+              "required": ["goal", "traffic_light", "traffic_jam"],
               "additionalProperties": false
             }
           },

planning/behavior_path_avoidance_module/src/scene.cpp

@@ -267,12 +267,6 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat
     data.reference_path, 0, data.reference_path.points.size(),
     motion_utils::calcSignedArcLength(data.reference_path.points, getEgoPosition(), 0));
 
-  data.to_return_point = utils::avoidance::calcDistanceToReturnDeadLine(
-    data.current_lanelets, data.reference_path_rough, planner_data_, parameters_);
-
-  data.to_start_point = utils::avoidance::calcDistanceToAvoidStartLine(
-    data.current_lanelets, data.reference_path_rough, planner_data_, parameters_);
-
   // target objects for avoidance
   fillAvoidanceTargetObjects(data, debug);
 
@@ -286,6 +280,17 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat
     return a.longitudinal < b.longitudinal;
   });
 
+  std::sort(data.other_objects.begin(), data.other_objects.end(), [](auto a, auto b) {
+    return a.longitudinal < b.longitudinal;
+  });
+
+  data.to_return_point = utils::avoidance::calcDistanceToReturnDeadLine(
+    data.current_lanelets, data.reference_path_rough, data.other_objects, planner_data_,
+    parameters_);
+
+  data.to_start_point = utils::avoidance::calcDistanceToAvoidStartLine(
+    data.current_lanelets, data.reference_path_rough, planner_data_, parameters_);
+
   // set base path
   path_shifter_.setPath(data.reference_path);
 }

planning/behavior_path_avoidance_module/src/utils.cpp

@@ -2263,7 +2263,7 @@ double calcDistanceToAvoidStartLine(
 
 double calcDistanceToReturnDeadLine(
   const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path,
-  const std::shared_ptr<const PlannerData> & planner_data,
+  ObjectDataArray & other_objects, const std::shared_ptr<const PlannerData> & planner_data,
   const std::shared_ptr<AvoidanceParameters> & parameters)
 {
   if (lanelets.empty()) {
@@ -2282,6 +2282,15 @@ double calcDistanceToReturnDeadLine(
     }
   }
 
+  // dead line stop factor(traffic jam)
+  if (parameters->enable_dead_line_for_traffic_jam && !other_objects.empty()) {
+    if (filtering_utils::isOnEgoLane(other_objects.front())) {
+      distance_to_return_dead_line = std::min(
+        distance_to_return_dead_line,
+        other_objects.front().longitudinal - parameters->dead_line_buffer_for_traffic_jam);
+    }
+  }
+
   // dead line for goal
   if (
     !utils::isAllowedGoalModification(planner_data->route_handler) &&