feat(intersection_module): add option to change the stopline position (…

…tier4#1364)

* use constexpr

* add stopline before intersection

Signed-off-by: 1222-takeshi <m.takeshi1995@gmail.com>

* feat(intersection_module): add update stopline index before intersection

Signed-off-by: 1222-takeshi <m.takeshi1995@gmail.com>

* modify to generate stop line to consider for the vehicle length

Signed-off-by: 1222-takeshi <m.takeshi1995@gmail.com>

* add generate stop line before intersection func

Signed-off-by: 1222-takeshi <m.takeshi1995@gmail.com>

* generate pass judge line when use_stuck_stopline

Signed-off-by: 1222-takeshi <m.takeshi1995@gmail.com>

* update param at launch

Signed-off-by: 1222-takeshi <m.takeshi1995@gmail.com>

* reduce nest

Signed-off-by: 1222-takeshi <m.takeshi1995@gmail.com>

* ci(pre-commit): autofix

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml

@@ -15,6 +15,7 @@
       min_predicted_path_confidence: 0.05
       collision_start_margin_time: 5.0 # [s]
       collision_end_margin_time: 2.0 # [s]
+      use_stuck_stopline: false # stopline generate before the intersection lanelet when is_stuck
 
     merge_from_private_road:
       stop_duration_sec: 1.0

planning/behavior_velocity_planner/config/intersection.param.yaml

@@ -15,6 +15,7 @@
       min_predicted_path_confidence: 0.05
       collision_start_margin_time: 5.0 # [s]
       collision_end_margin_time: 2.0 # [s]
+      use_stuck_stopline: false # stopline generate before the intersection lanelet when is_stuck
 
     merge_from_private_road:
       stop_duration_sec: 1.0

planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp

@@ -122,6 +122,7 @@ class IntersectionModule : public SceneModuleInterface
     double external_input_timeout;       //! used to disable external input
     double collision_start_margin_time;  //! start margin time to check collision
     double collision_end_margin_time;    //! end margin time to check collision
+    bool use_stuck_stopline;  //! stopline generate before the intersection lanelet when is_stuck.
   };
 
   IntersectionModule(

planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp

@@ -80,6 +80,21 @@ bool generateStopLine(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, int * stop_line_idx,
   int * pass_judge_line_idx, int * first_idx_inside_lane, const rclcpp::Logger logger);
 
+/**
+ * @brief If use_stuck_stopline is true, a stop line is generated before the intersection.
+ * @param input_path      input path
+ * @param output_path     output path
+ * @param stuck_stop_line_idx   generated stuck stop line index
+ * @param pass_judge_line_idx  generated pass judge line index
+ * @return false when generation failed
+ */
+bool generateStopLineBeforeIntersection(
+  const int lane_id, lanelet::LaneletMapConstPtr lanelet_map_ptr,
+  const std::shared_ptr<const PlannerData> & planner_data,
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path,
+  autoware_auto_planning_msgs::msg::PathWithLaneId * output_path, int * stuck_stop_line_idx,
+  int * pass_judge_line_idx, const rclcpp::Logger logger);
+
 /**
  * @brief Calculate first path index that is in the polygon.
  * @param path     target path

planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp

@@ -50,6 +50,7 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
   ip.external_input_timeout = node.declare_parameter(ns + ".walkway.external_input_timeout", 1.0);
   ip.collision_start_margin_time = node.declare_parameter(ns + ".collision_start_margin_time", 5.0);
   ip.collision_end_margin_time = node.declare_parameter(ns + ".collision_end_margin_time", 2.0);
+  ip.use_stuck_stopline = node.declare_parameter(ns + ".use_stuck_stopline", true);
 }
 
 MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node)

planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp

@@ -188,9 +188,18 @@ bool IntersectionModule::modifyPathVelocity(
     path->points.at(stop_line_idx).point.pose.position));
 
   if (!isActivated()) {
-    const double v = 0.0;
+    constexpr double v = 0.0;
+    if (planner_param_.use_stuck_stopline && is_stuck) {
+      int stuck_stop_line_idx = -1;
+      int stuck_pass_judge_line_idx = -1;
+      if (util::generateStopLineBeforeIntersection(
+            lane_id_, lanelet_map_ptr, planner_data_, *path, path, &stuck_stop_line_idx,
+            &stuck_pass_judge_line_idx, logger_.get_child("util"))) {
+        stop_line_idx = stuck_stop_line_idx;
+        pass_judge_line_idx = stuck_pass_judge_line_idx;
+      }
+    }
     util::setVelocityFrom(stop_line_idx, v, path);
-
     debug_data_.stop_required = true;
     debug_data_.stop_wall_pose = util::getAheadPose(stop_line_idx, base_link2front, *path);
     debug_data_.stop_point_pose = path->points.at(stop_line_idx).point.pose;

planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp

@@ -501,5 +501,90 @@ lanelet::ConstLanelet generateOffsetLanelet(
   return std::move(lanelet_with_margin);
 }
 
+bool generateStopLineBeforeIntersection(
+  const int lane_id, lanelet::LaneletMapConstPtr lanelet_map_ptr,
+  const std::shared_ptr<const PlannerData> & planner_data,
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path,
+  autoware_auto_planning_msgs::msg::PathWithLaneId * output_path, int * stuck_stop_line_idx,
+  int * pass_judge_line_idx, const rclcpp::Logger logger)
+{
+  /* set judge line dist */
+  const double current_vel = planner_data->current_velocity->twist.linear.x;
+  const double current_acc = planner_data->current_accel.get();
+  const double max_acc = planner_data->max_stop_acceleration_threshold;
+  const double max_jerk = planner_data->max_stop_jerk_threshold;
+  const double delay_response_time = planner_data->delay_response_time;
+  const double pass_judge_line_dist = planning_utils::calcJudgeLineDistWithJerkLimit(
+    current_vel, current_acc, max_acc, max_jerk, delay_response_time);
+
+  /* set parameters */
+  constexpr double interval = 0.2;
+  const int base2front_idx_dist =
+    std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / interval);
+  const int pass_judge_idx_dist = std::ceil(pass_judge_line_dist / interval);
+
+  /* spline interpolation */
+  autoware_auto_planning_msgs::msg::PathWithLaneId path_ip;
+  if (!splineInterpolate(input_path, interval, &path_ip, logger)) {
+    return false;
+  }
+  const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id);
+  for (size_t i = 0; i < path_ip.points.size(); i++) {
+    const auto & p = path_ip.points.at(i).point.pose;
+    if (lanelet::utils::isInLanelet(p, assigned_lanelet, 0.1)) {
+      if (static_cast<int>(i) <= 0) {
+        RCLCPP_DEBUG(logger, "generate stopline, but no within lanelet.");
+        return false;
+      }
+      int stop_idx_ip;  // stop point index for interpolated path.
+      stop_idx_ip = std::max(static_cast<int>(i) - base2front_idx_dist, 0);
+
+      /* insert stop_point */
+      const auto inserted_stop_point = path_ip.points.at(stop_idx_ip).point.pose;
+      // if path has too close (= duplicated) point to the stop point, do not insert it
+      // and consider the index of the duplicated point as *stuck_stop_line_idx
+      if (!util::hasDuplicatedPoint(
+            *output_path, inserted_stop_point.position, stuck_stop_line_idx)) {
+        *stuck_stop_line_idx = util::insertPoint(inserted_stop_point, output_path);
+      }
+
+      /* if another stop point exist before intersection stop_line, disable judge_line. */
+      bool has_prior_stopline = false;
+      for (int i = 0; i < *stuck_stop_line_idx; ++i) {
+        if (std::fabs(output_path->points.at(i).point.longitudinal_velocity_mps) < 0.1) {
+          has_prior_stopline = true;
+          break;
+        }
+      }
+
+      /* insert judge point */
+      const int pass_judge_idx_ip = std::min(
+        static_cast<int>(path_ip.points.size()) - 1,
+        std::max(stop_idx_ip - pass_judge_idx_dist, 0));
+      if (has_prior_stopline || stop_idx_ip == pass_judge_idx_ip) {
+        *pass_judge_line_idx = *stuck_stop_line_idx;
+      } else {
+        const auto inserted_pass_judge_point = path_ip.points.at(pass_judge_idx_ip).point.pose;
+        // if path has too close (= duplicated) point to the pass judge point, do not insert it
+        // and consider the index of the duplicated point as pass_judge_line_idx
+        if (!util::hasDuplicatedPoint(
+              *output_path, inserted_pass_judge_point.position, pass_judge_line_idx)) {
+          *pass_judge_line_idx = util::insertPoint(inserted_pass_judge_point, output_path);
+          ++(*stuck_stop_line_idx);  // stop index is incremented by judge line insertion
+        }
+      }
+
+      RCLCPP_DEBUG(
+        logger,
+        "generateStopLineBeforeIntersection() : stuck_stop_line_idx = %d, pass_judge_idx = %d,"
+        "stop_idx_ip = %d, pass_judge_idx_ip = %d, has_prior_stopline = %d",
+        *stuck_stop_line_idx, *pass_judge_line_idx, stop_idx_ip, pass_judge_idx_ip,
+        has_prior_stopline);
+      return true;
+    }
+  }
+  return false;
+}
+
 }  // namespace util
 }  // namespace behavior_velocity_planner