feat/insert_ahead_stop_point

Signed-off-by: kyoichi-sugahara <[email protected]>

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: 3.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

@@ -313,6 +313,38 @@ std::optional<std::pair<geometry_msgs::msg::Point, double>> CrosswalkModule::get
   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> ahead_inserted_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_INFO_EXPRESSION(
+        logger_, "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);
+      ahead_inserted_stop_point.push_back(stop_point);
+      break;
+    }
+  }
+  return ahead_inserted_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,
@@ -384,14 +416,49 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
     return {};
   }
 
+  std::vector<Point> ahead_inserted_stop_point{};
+  bool stop_point_inserted = false;
+  bool inserted_stop_point_is_front_of_crosswalk = false;
+
+  // TBD
+  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);
+    ahead_inserted_stop_point =
+      searchAheadInsertedStopPoint(ego_path, default_stop_point, ahead_margin);
+
+    stop_point_inserted = !ahead_inserted_stop_point.empty();
+    // don't insert stop point if the stop point is inserted within ***[m] ahead
+    // NOTE: Make sure that the stop point is located before the crosswalk.
+    if (stop_point_inserted) {
+      const double dist_inserted_stop_point2crosswalk = calcSignedArcLength(
+        ego_path.points, path_intersects.front(), ahead_inserted_stop_point.front());
+      inserted_stop_point_is_front_of_crosswalk = dist_inserted_stop_point2crosswalk + base_link2front < 0.0;
+      RCLCPP_INFO_EXPRESSION(logger_, "Distance to crosswalk: %f", dist_inserted_stop_point2crosswalk);
+    }
+  }
+
   // 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;
 
+  // std::cerr << "stop_at_stop_line:" << stop_at_stop_line << std::endl;
+  // std::cerr << "default_stop_pose:" << default_stop_pose.has_value() << std::endl;
+  // std::cerr << "stop_line_found:" << stop_line_found << std::endl;
+  // std::cerr << "stop_point_inserted:" << stop_point_inserted << std::endl;
+  // std::cerr << "inserted_stop_point_is_front_of_crosswalk:"
+  //           << inserted_stop_point_is_front_of_crosswalk << std::endl;
   if (stop_at_stop_line) {
     // Stop at the stop line
     if (default_stop_pose) {
+      if (inserted_stop_point_is_front_of_crosswalk) {
+        RCLCPP_INFO_EXPRESSION(logger_, "Insert stop point ahead");
+        const auto inserted_stop_pose = calcLongitudinalOffsetPose(
+          ego_path.points, ahead_inserted_stop_point.front(), 0.0);
+        return createStopFactor(*inserted_stop_pose, ahead_inserted_stop_point);
+      }
       return createStopFactor(*default_stop_pose, stop_factor_points);
     }
   } else {

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,10 @@ class CrosswalkModule : public SceneModuleInterface
     const std::optional<StopFactor> & stop_factor_for_crosswalk_users,
     const std::optional<StopFactor> & stop_factor_for_stuck_vehicles);
 
+  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 +355,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,