feat(behavior_velocity_crosswalk_module): merge with forward stop point if it's in front of crosswalk

planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml

@@ -17,6 +17,8 @@
         far_object_threshold: 10.0 # [m] If objects cross X meters behind the stop line, the stop position is determined according to the object position (stop_distance_from_object meters before the object).
         # For the case where the stop position is determined according to the object position.
         stop_distance_from_object: 2.0 # [m] the vehicle decelerates to be able to stop in front of object with margin
+        # If the ahead stop position is already inserted, use the same position for the new stop point.
+        max_ahead_longitudinal_margin: 5.0 # [m]  specifies the margin between the current position and the stop point
 
       # param for ego's slow down velocity
       slow_down:

planning/behavior_velocity_crosswalk_module/src/manager.cpp

@@ -53,6 +53,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
     getOrDeclareParameter<double>(node, ns + ".stop_position.far_object_threshold");
   cp.stop_position_threshold =
     getOrDeclareParameter<double>(node, ns + ".stop_position.stop_position_threshold");
+  cp.max_ahead_longitudinal_margin =
+    getOrDeclareParameter<double>(node, ns + ".stop_position.max_ahead_longitudinal_margin");
 
   // param for ego velocity
   cp.min_slow_down_velocity =

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

@@ -1,4 +1,4 @@
 // Copyright 2020 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -313,6 +313,39 @@
   return {};
 }
 
+std::vector<Point> CrosswalkModule::searchAheadInsertedStopPoint(
+  const PathWithLaneId & ego_path, const Point & candidate_stop_point,
+  const double ahead_margin) const
+{
+  // If the stop point is not found, return empty vector.
+  std::vector<Point> inserted_forward_stop_point{};
+
+  const double epsilon = 1e-3;
+  const int candidate_stop_point_idx =
+    findNearestSegmentIndex(ego_path.points, candidate_stop_point);
+
+  // Start from the segment just before the nearest or from 0 if it's the first segment.
+  size_t start_idx = std::max(candidate_stop_point_idx - 1, 0);
+  for (size_t idx = start_idx; idx < ego_path.points.size() - 1; ++idx) {
+    if (calcSignedArcLength(ego_path.points, start_idx, idx) > ahead_margin) {
+      break;
+    }
+    if (std::abs(ego_path.points.at(idx).point.longitudinal_velocity_mps) < epsilon) {
+      RCLCPP_DEBUG_THROTTLE(
+        logger_, *clock_, 500,
+        "Stop point is found and distance from inserted stop point: %f module_id: %ld",
+        calcSignedArcLength(ego_path.points, candidate_stop_point_idx, idx), module_id_);
+      const auto stop_point = createPoint(
+        ego_path.points.at(idx).point.pose.position.x,
+        ego_path.points.at(idx).point.pose.position.y,
+        ego_path.points.at(idx).point.pose.position.z);
+      inserted_forward_stop_point.push_back(stop_point);
+      break;
+    }
+  }
+  return inserted_forward_stop_point;
+}
+
 std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
   const PathWithLaneId & ego_path, const PathWithLaneId & sparse_resample_path,
   const std::optional<std::pair<geometry_msgs::msg::Point, double>> & p_stop_line,
@@ -383,17 +416,51 @@
   if (!nearest_stop_info) {
     return {};
   }
+  std::vector<Point> inserted_forward_stop_point{};
+  bool merged_stop_point_is_front_of_crosswalk = false;
+
+  if (default_stop_pose.has_value()) {
+    const double ahead_margin = planner_param_.max_ahead_longitudinal_margin;
+    const Point default_stop_point = createPoint(
+      default_stop_pose->position.x, default_stop_pose->position.y, default_stop_pose->position.z);
+    // Search for the inserted stop point that is ahead of the ego path with the margin.
+    inserted_forward_stop_point =
+      searchAheadInsertedStopPoint(ego_path, default_stop_point, ahead_margin);
+
+    // If there are any inserted forward stop points
+    if (!inserted_forward_stop_point.empty()) {
+      const double dist_inserted_stop_point2crosswalk = calcSignedArcLength(
+        ego_path.points, path_intersects.front(), inserted_forward_stop_point.front());
+      // Check if the merged stop point is in front of the crosswalk
+      merged_stop_point_is_front_of_crosswalk =
+        dist_inserted_stop_point2crosswalk + base_link2front < 0.0;
+      RCLCPP_DEBUG_THROTTLE(
+        logger_, *clock_, 500, "Distance to crosswalk: %f", dist_inserted_stop_point2crosswalk);
+      if (!merged_stop_point_is_front_of_crosswalk) {
+        RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 500, "Stop point is behind crosswalk");
+      }
+    }
+  }
 
   // Check if the ego should stop beyond the stop line.
   const bool stop_at_stop_line =
     dist_ego_to_stop < nearest_stop_info->second &&
     nearest_stop_info->second < dist_ego_to_stop + planner_param_.far_object_threshold;
 
-  if (stop_at_stop_line) {
-    // Stop at the stop line
-    if (default_stop_pose) {
-      return createStopFactor(*default_stop_pose, stop_factor_points);
+  RCLCPP_DEBUG(logger_, "stop_at_stop_line: %d", stop_at_stop_line);
+  RCLCPP_DEBUG(logger_, "default_stop_pose: %d", default_stop_pose.has_value());
+  RCLCPP_DEBUG(
+    logger_, "merged_stop_point_is_front_of_crosswalk: %d",
+    merged_stop_point_is_front_of_crosswalk);
+
+  if (stop_at_stop_line && default_stop_pose) {
+    if (merged_stop_point_is_front_of_crosswalk) {
+      RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 500, "merge with forward stop point");
+      const auto inserted_stop_pose =
+        calcLongitudinalOffsetPose(ego_path.points, inserted_forward_stop_point.front(), 0.0);
+      return createStopFactor(*inserted_stop_pose, inserted_forward_stop_point);
     }
+    return createStopFactor(*default_stop_pose, stop_factor_points);
   } else {
     // Stop beyond the stop line
     const auto stop_pose = calcLongitudinalOffsetPose(

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp

@@ -54,6 +54,7 @@ using autoware_auto_perception_msgs::msg::PredictedObject;
 using autoware_auto_perception_msgs::msg::PredictedObjects;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using autoware_perception_msgs::msg::TrafficSignalElement;
+using geometry_msgs::msg::Point;
 using lanelet::autoware::Crosswalk;
 using tier4_api_msgs::msg::CrosswalkStatus;
 using tier4_autoware_utils::Polygon2d;
@@ -115,6 +116,7 @@ class CrosswalkModule : public SceneModuleInterface
     double stop_distance_from_crosswalk;
     double far_object_threshold;
     double stop_position_threshold;
+    double max_ahead_longitudinal_margin;
     // param for ego velocity
     float min_slow_down_velocity;
     double max_slow_down_jerk;
@@ -160,7 +162,7 @@ class CrosswalkModule : public SceneModuleInterface
     CollisionState collision_state{};
     std::optional<rclcpp::Time> time_to_start_stopped{std::nullopt};
 
-    geometry_msgs::msg::Point position{};
+    Point position{};
     std::optional<CollisionPoint> collision_point{};
 
     void transitState(
@@ -311,21 +313,20 @@ class CrosswalkModule : public SceneModuleInterface
 private:
   // main functions
   void applySafetySlowDownSpeed(
-    PathWithLaneId & output, const std::vector<geometry_msgs::msg::Point> & path_intersects);
+    PathWithLaneId & output, const std::vector<Point> & path_intersects);
 
-  std::optional<std::pair<geometry_msgs::msg::Point, double>> getStopPointWithMargin(
-    const PathWithLaneId & ego_path,
-    const std::vector<geometry_msgs::msg::Point> & path_intersects) const;
+  std::optional<std::pair<Point, double>> getStopPointWithMargin(
+    const PathWithLaneId & ego_path, const std::vector<Point> & path_intersects) const;
 
   std::optional<StopFactor> checkStopForCrosswalkUsers(
     const PathWithLaneId & ego_path, const PathWithLaneId & sparse_resample_path,
-    const std::optional<std::pair<geometry_msgs::msg::Point, double>> & p_stop_line,
-    const std::vector<geometry_msgs::msg::Point> & path_intersects,
+    const std::optional<std::pair<Point, double>> & p_stop_line,
+    const std::vector<Point> & path_intersects,
     const std::optional<geometry_msgs::msg::Pose> & default_stop_pose);
 
   std::optional<StopFactor> checkStopForStuckVehicles(
     const PathWithLaneId & ego_path, const std::vector<PredictedObject> & objects,
-    const std::vector<geometry_msgs::msg::Point> & path_intersects,
+    const std::vector<Point> & path_intersects,
     const std::optional<geometry_msgs::msg::Pose> & stop_pose) const;
 
   std::optional<CollisionPoint> getCollisionPoint(
@@ -337,6 +338,28 @@ class CrosswalkModule : public SceneModuleInterface
     const std::optional<StopFactor> & stop_factor_for_crosswalk_users,
     const std::optional<StopFactor> & stop_factor_for_stuck_vehicles);
 
+  /**
+   * @brief Searches for stop points ahead of a given candidate stop point in the ego path.
+   *
+   * This function searches for stop points that are ahead of the given candidate stop point
+   * within a specified margin. If a stop point is found within the margin and its longitudinal
+   * velocity is approximately zero, it is considered as a valid stop point and added to the result.
+   * If no such point is found, an empty vector is returned.
+   *
+   * @param ego_path The path of the ego vehicle witch contains velocity information.
+   * @param candidate_stop_point The reference point to start searching for stop points.
+   * @param ahead_margin The distance margin ahead of the candidate stop point within which to
+   * search for stop points.
+   * @return A vector containing the found stop points. If no stop point is found, returns an empty
+   * vector.
+   *
+   * @note The function uses a small epsilon value to check if the longitudinal velocity of a point
+   * is approximately zero.
+   */
+  std::vector<Point> searchAheadInsertedStopPoint(
+    const PathWithLaneId & ego_path, const Point & candidate_stop_point,
+    const double ahead_margin) const;
+
   void setDistanceToStop(
     const PathWithLaneId & ego_path,
     const std::optional<geometry_msgs::msg::Pose> & default_stop_pose,
@@ -350,32 +373,28 @@ class CrosswalkModule : public SceneModuleInterface
 
   // minor functions
   std::pair<double, double> getAttentionRange(
-    const PathWithLaneId & ego_path,
-    const std::vector<geometry_msgs::msg::Point> & path_intersects);
+    const PathWithLaneId & ego_path, const std::vector<Point> & path_intersects);
 
   void insertDecelPointWithDebugInfo(
-    const geometry_msgs::msg::Point & stop_point, const float target_velocity,
-    PathWithLaneId & output) const;
+    const Point & stop_point, const float target_velocity, PathWithLaneId & output) const;
 
   std::pair<double, double> clampAttentionRangeByNeighborCrosswalks(
     const PathWithLaneId & ego_path, const double near_attention_range,
     const double far_attention_range);
 
   CollisionPoint createCollisionPoint(
-    const geometry_msgs::msg::Point & nearest_collision_point, const double dist_ego2cp,
-    const double dist_obj2cp, const geometry_msgs::msg::Vector3 & ego_vel,
-    const geometry_msgs::msg::Vector3 & obj_vel) const;
+    const Point & nearest_collision_point, const double dist_ego2cp, const double dist_obj2cp,
+    const geometry_msgs::msg::Vector3 & ego_vel, const geometry_msgs::msg::Vector3 & obj_vel) const;
 
-  float calcTargetVelocity(
-    const geometry_msgs::msg::Point & stop_point, const PathWithLaneId & ego_path) const;
+  float calcTargetVelocity(const Point & stop_point, const PathWithLaneId & ego_path) const;
 
   Polygon2d getAttentionArea(
     const PathWithLaneId & sparse_resample_path,
     const std::pair<double, double> & crosswalk_attention_range) const;
 
   bool isStuckVehicle(
     const PathWithLaneId & ego_path, const std::vector<PredictedObject> & objects,
-    const std::vector<geometry_msgs::msg::Point> & path_intersects) const;
+    const std::vector<Point> & path_intersects) const;
 
   void updateObjectState(
     const double dist_ego_to_stop, const PathWithLaneId & sparse_resample_path,