feat(intersection): use own max acc/jerk param (autowarefoundation#5370)

Signed-off-by: Mamoru Sobue <[email protected]>
zulfaqar-azmi-t4 · Oct 23, 2023 · 1502e66 · 1502e66
1 parent 98c24b8
commit 1502e66
Show file tree

Hide file tree

Showing 6 changed files with 17 additions and 7 deletions.
diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml
@@ -12,6 +12,9 @@
         use_intersection_area: false
         path_interpolation_ds: 0.1 # [m]
         consider_wrong_direction_vehicle: false
+        max_accel: -2.8
+        max_jerk: -5.0
+        delay_response_time: 0.5
       stuck_vehicle:
         turn_direction:
           left: true

diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp
@@ -62,6 +62,10 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
     getOrDeclareParameter<double>(node, ns + ".common.path_interpolation_ds");
   ip.common.consider_wrong_direction_vehicle =
     getOrDeclareParameter<bool>(node, ns + ".common.consider_wrong_direction_vehicle");
+  ip.common.max_accel = getOrDeclareParameter<double>(node, ns + ".common.max_accel");
+  ip.common.max_jerk = getOrDeclareParameter<double>(node, ns + ".common.max_jerk");
+  ip.common.delay_response_time =
+    getOrDeclareParameter<double>(node, ns + ".common.delay_response_time");
 
   ip.stuck_vehicle.turn_direction.left =
     getOrDeclareParameter<bool>(node, ns + ".stuck_vehicle.turn_direction.left");

diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
@@ -987,7 +987,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
   const auto intersection_stop_lines_opt = util::generateIntersectionStopLines(
     first_conflicting_area, dummy_first_attention_area, planner_data_, interpolated_path_info,
     planner_param_.stuck_vehicle.use_stuck_stopline, planner_param_.common.stop_line_margin,
-    peeking_offset, path);
+    planner_param_.common.max_accel, planner_param_.common.max_jerk,
+    planner_param_.common.delay_response_time, peeking_offset, path);
   if (!intersection_stop_lines_opt) {
     return IntersectionModule::Indecisive{"failed to generate intersection_stop_lines"};
   }

diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
@@ -58,6 +58,9 @@ class IntersectionModule : public SceneModuleInterface
       bool use_intersection_area;
       bool consider_wrong_direction_vehicle;
       double path_interpolation_ds;
+      double max_accel;
+      double max_jerk;
+      double delay_response_time;
     } common;
     struct StuckVehicle
     {

diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp
@@ -269,7 +269,8 @@ std::optional<IntersectionStopLines> generateIntersectionStopLines(
   const lanelet::CompoundPolygon3d & first_detection_area,
   const std::shared_ptr<const PlannerData> & planner_data,
   const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline,
-  const double stop_line_margin, const double peeking_offset,
+  const double stop_line_margin, const double peeking_offset, const double max_accel,
+  const double max_jerk, const double delay_response_time,
   autoware_auto_planning_msgs::msg::PathWithLaneId * original_path)
 {
   const auto & path_ip = interpolated_path_info.path;
@@ -350,11 +351,8 @@ std::optional<IntersectionStopLines> generateIntersectionStopLines(
   // (4) pass judge line position on interpolated path
   const double velocity = planner_data->current_velocity->twist.linear.x;
   const double acceleration = planner_data->current_acceleration->accel.accel.linear.x;
-  const double max_stop_acceleration = planner_data->max_stop_acceleration_threshold;
-  const double max_stop_jerk = planner_data->max_stop_jerk_threshold;
-  const double delay_response_time = planner_data->delay_response_time;
   const double braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit(
-    velocity, acceleration, max_stop_acceleration, max_stop_jerk, delay_response_time);
+    velocity, acceleration, max_accel, max_jerk, delay_response_time);
   int pass_judge_ip_int =
     static_cast<int>(first_footprint_inside_detection_ip) - std::ceil(braking_dist / ds);
   const auto pass_judge_line_ip = static_cast<size_t>(

diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp
@@ -70,7 +70,8 @@ std::optional<IntersectionStopLines> generateIntersectionStopLines(
   const lanelet::CompoundPolygon3d & first_detection_area,
   const std::shared_ptr<const PlannerData> & planner_data,
   const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline,
-  const double stop_line_margin, const double peeking_offset,
+  const double stop_line_margin, const double peeking_offset, const double max_accel,
+  const double max_jerk, const double delay_response_time,
   autoware_auto_planning_msgs::msg::PathWithLaneId * original_path);
 
 std::optional<size_t> getFirstPointInsidePolygon(