From 8272a56a8b5d68dbc9753f0be819c80667c6d37b Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Thu, 5 Oct 2023 02:40:08 +0900 Subject: [PATCH] add description Signed-off-by: kyoichi-sugahara --- .../src/scene_crosswalk.cpp | 3 +++ .../src/scene_crosswalk.hpp | 18 ++++++++++++++++++ 2 files changed, 21 insertions(+) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 4c288f725baf1..dfa5f55441817 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -423,12 +423,15 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( 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( diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 06a939a16a306..c9e3950d837a7 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -338,6 +338,24 @@ class CrosswalkModule : public SceneModuleInterface const std::optional & stop_factor_for_crosswalk_users, const std::optional & 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 searchAheadInsertedStopPoint( const PathWithLaneId & ego_path, const Point & candidate_stop_point, const double ahead_margin) const;