From a0aa598868d9bdc4670fc08738fb3a307ae4c5fc Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Fri, 11 Oct 2024 11:51:11 +0900 Subject: [PATCH] doxygen comment Signed-off-by: Zulfaqar Azmi --- .../utils/utils.hpp | 41 ++++++++++++++++++- .../src/scene.cpp | 7 ++-- .../src/utils/utils.cpp | 17 ++++---- 3 files changed, 52 insertions(+), 13 deletions(-) diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp index 1dd33d13cfc7e..48b940093c2cd 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp @@ -290,12 +290,51 @@ double calcPhaseLength( const double velocity, const double maximum_velocity, const double acceleration, const double time); +/** + * @brief Calculates the minimum distance to a stationary object in the current lanes. + * + * This function determines the closest distance from the ego vehicle to a stationary object + * in the current lanes. It checks if the object is within the stopping criteria based on its + * velocity and calculates the distance while accounting for the object's size. Only objects + * positioned after the specified distance to the target lane's start are considered. + * + * @param filtered_objects A collection of lane change target objects, including those in the + * current lane. + * @param bpp_param Parameters for the behavior path planner, such as vehicle dimensions. + * @param lc_param Parameters for the lane change process, including the velocity threshold for + * stopping. + * @param dist_to_target_lane_start The distance from the ego vehicle to the start of the target + * lane. + * @param ego_pose The current pose of the ego vehicle. + * @param path The current path of the ego vehicle, containing path points and lane information. + * @return The minimum distance to a stationary object in the current lanes. If no valid object is + * found, returns the maximum possible double value. + */ double get_min_dist_to_current_lanes_obj( const LaneChangeTargetObjects & filtered_objects, const BehaviorPathPlannerParameters & bpp_param, const LaneChangeParameters & lc_param, const double dist_to_target_lane_start, const Pose & pose, const PathWithLaneId & path); -bool has_blocking_target_object_for_stopping( +/** + * @brief Checks if there is any stationary object in the target lanes that would affect the lane + * change stop decision. + * + * This function determines whether there are any stationary objects in the target lanes that could + * impact the decision to insert a stop point for the ego vehicle. It checks each object's velocity, + * position relative to the ego vehicle, and overlap with the target lanes to identify if any object + * meets the criteria for being a blocking obstacle. + * + * @param target_lanes A collection of lanelets representing the target lanes for the lane change. + * @param filtered_objects A collection of lane change target objects, including those in the target + * lanes. + * @param lc_param Parameters for the lane change process, such as the stop velocity threshold. + * @param stop_arc_length The arc length at which the ego vehicle is expected to stop. + * @param ego_pose The current pose of the ego vehicle. + * @param path The current path of the ego vehicle, containing path points and lane information. + * @return true if there is a stationary object in the target lanes that meets the criteria for + * being a blocking object; otherwise, false. + */ +bool has_blocking_target_object( const lanelet::ConstLanelets & target_lanes, const LaneChangeTargetObjects & filtered_objects, const LaneChangeParameters & lc_param, const double stop_arc_length, const Pose & ego_pose, const PathWithLaneId & path); diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 1cf66c0b3354a..284ea9409e924 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -316,10 +316,9 @@ void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path) // ---------------------------------------------------------- // [ego]> | <--- stop margin ---> [obj]> // ---------------------------------------------------------- - const auto has_blocking_target_lane_obj = - utils::lane_change::has_blocking_target_object_for_stopping( - status_.target_lanes, filtered_objects, *lane_change_parameters_, stop_arc_length_behind_obj, - getEgoPose(), path); + const auto has_blocking_target_lane_obj = utils::lane_change::has_blocking_target_object( + status_.target_lanes, filtered_objects, *lane_change_parameters_, stop_arc_length_behind_obj, + getEgoPose(), path); if (has_blocking_target_lane_obj || stop_arc_length_behind_obj <= 0.0) { set_stop_pose(dist_to_terminal_start, path); diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index 965544e4bcdfe..6ad5404fedbae 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -1268,7 +1268,7 @@ double get_min_dist_to_current_lanes_obj( return min_dist_to_obj; } -bool has_blocking_target_object_for_stopping( +bool has_blocking_target_object( const lanelet::ConstLanelets & target_lanes, const LaneChangeTargetObjects & filtered_objects, const LaneChangeParameters & lc_param, const double stop_arc_length, const Pose & ego_pose, const PathWithLaneId & path) @@ -1277,30 +1277,31 @@ bool has_blocking_target_object_for_stopping( lanelet::utils::combineLaneletsShape(target_lanes).polygon2d().basicPolygon(); return std::any_of( filtered_objects.target_lane.begin(), filtered_objects.target_lane.end(), - [&](const ExtendedPredictedObject & o) { - const auto v = std::abs(o.initial_twist.twist.linear.x); + [&](const auto & object) { + const auto v = std::abs(object.initial_twist.twist.linear.x); if (v > lc_param.stop_velocity_threshold) { return false; } const auto arc_length_to_ego = motion_utils::calcSignedArcLength( - path.points, ego_pose.position, o.initial_pose.pose.position) - - (o.shape.dimensions.x / 2); + path.points, ego_pose.position, object.initial_pose.pose.position) - + (object.shape.dimensions.x / 2); if (arc_length_to_ego < 0.0) { return false; } - const auto obj_poly = tier4_autoware_utils::toPolygon2d(o.initial_pose.pose, o.shape); + const auto obj_poly = + tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape); // filtered_objects includes objects out of target lanes, so filter them out if (boost::geometry::disjoint(obj_poly, target_lane_poly)) { return false; } const auto arc_length_to_target_lane_obj = motion_utils::calcSignedArcLength( - path.points, path.points.front().point.pose.position, o.initial_pose.pose.position); - const auto width_margin = o.shape.dimensions.x / 2; + path.points, path.points.front().point.pose.position, object.initial_pose.pose.position); + const auto width_margin = object.shape.dimensions.x / 2; return (arc_length_to_target_lane_obj - width_margin) >= stop_arc_length; }); }