From 0e539d1999af50b5688b88baad2f22c7e4e1d1ed Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Thu, 1 Aug 2024 09:25:27 +0900 Subject: [PATCH] feat(out_of_lane): also apply lat buffer between the lane and stop pose (#1438) feat(out_of_lane): also apply lat buffer between the lane and stop pose (#7918) Signed-off-by: Maxime CLEMENT --- .../config/out_of_lane.param.yaml | 3 ++- .../src/calculate_slowdown_points.cpp | 6 ++++-- .../src/calculate_slowdown_points.hpp | 2 +- .../src/out_of_lane_module.cpp | 7 +++++-- .../src/types.hpp | 3 ++- 5 files changed, 14 insertions(+), 7 deletions(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml index cc8f6f65b272d..a0cf69ee71027 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml @@ -28,7 +28,8 @@ action: # action to insert in the trajectory if an object causes a conflict at an overlap skip_if_over_max_decel: true # if true, do not take an action that would cause more deceleration than the maximum allowed precision: 0.1 # [m] precision when inserting a stop pose in the trajectory - distance_buffer: 1.5 # [m] buffer distance to try to keep between the ego footprint and lane + longitudinal_distance_buffer: 1.5 # [m] safety distance buffer to keep in front of the ego vehicle + lateral_distance_buffer: 1.0 # [m] safety distance buffer to keep on the side of the ego vehicle min_duration: 1.0 # [s] minimum duration needed before a decision can be canceled slowdown: distance_threshold: 30.0 # [m] insert a slowdown when closer than this distance from an overlap diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp index 0c793c9f5798e..8b03fa66eab55 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp @@ -70,9 +70,11 @@ std::optional calculate_last_in_lane_pose( std::optional calculate_slowdown_point( const EgoData & ego_data, const std::vector & decisions, - const std::optional prev_slowdown_point, PlannerParam params) + const std::optional & prev_slowdown_point, PlannerParam params) { - params.extra_front_offset += params.dist_buffer; + params.extra_front_offset += params.lon_dist_buffer; + params.extra_right_offset += params.lat_dist_buffer; + params.extra_left_offset += params.lat_dist_buffer; const auto base_footprint = make_base_footprint(params); // search for the first slowdown decision for which a stop point can be inserted diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp index ee4897de86c5b..145f21c94c0d0 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp @@ -53,6 +53,6 @@ std::optional calculate_last_in_lane_pose( /// @return optional slowdown point to insert in the trajectory std::optional calculate_slowdown_point( const EgoData & ego_data, const std::vector & decisions, - const std::optional prev_slowdown_point, PlannerParam params); + const std::optional & prev_slowdown_point, PlannerParam params); } // namespace autoware::motion_velocity_planner::out_of_lane #endif // CALCULATE_SLOWDOWN_POINTS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index a7f7cbf5f2242..0afedcd7f9c9a 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -94,7 +94,9 @@ void OutOfLaneModule::init_parameters(rclcpp::Node & node) getOrDeclareParameter(node, ns_ + ".action.skip_if_over_max_decel"); pp.precision = getOrDeclareParameter(node, ns_ + ".action.precision"); pp.min_decision_duration = getOrDeclareParameter(node, ns_ + ".action.min_duration"); - pp.dist_buffer = getOrDeclareParameter(node, ns_ + ".action.distance_buffer"); + pp.lon_dist_buffer = + getOrDeclareParameter(node, ns_ + ".action.longitudinal_distance_buffer"); + pp.lat_dist_buffer = getOrDeclareParameter(node, ns_ + ".action.lateral_distance_buffer"); pp.slow_velocity = getOrDeclareParameter(node, ns_ + ".action.slowdown.velocity"); pp.slow_dist_threshold = getOrDeclareParameter(node, ns_ + ".action.slowdown.distance_threshold"); @@ -140,7 +142,8 @@ void OutOfLaneModule::update_parameters(const std::vector & p updateParam(parameters, ns_ + ".action.skip_if_over_max_decel", pp.skip_if_over_max_decel); updateParam(parameters, ns_ + ".action.precision", pp.precision); updateParam(parameters, ns_ + ".action.min_duration", pp.min_decision_duration); - updateParam(parameters, ns_ + ".action.distance_buffer", pp.dist_buffer); + updateParam(parameters, ns_ + ".action.longitudinal_distance_buffer", pp.lon_dist_buffer); + updateParam(parameters, ns_ + ".action.lateral_distance_buffer", pp.lat_dist_buffer); updateParam(parameters, ns_ + ".action.slowdown.velocity", pp.slow_velocity); updateParam(parameters, ns_ + ".action.slowdown.distance_threshold", pp.slow_dist_threshold); updateParam(parameters, ns_ + ".action.stop.distance_threshold", pp.stop_dist_threshold); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp index 852d3ab2d2bc1..9c68a0bf23a92 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp @@ -62,7 +62,8 @@ struct PlannerParam double overlap_min_dist; // [m] min distance inside another lane to consider an overlap // action to insert in the trajectory if an object causes a conflict at an overlap bool skip_if_over_max_decel; // if true, skip the action if it causes more than the max decel - double dist_buffer; + double lon_dist_buffer; // [m] safety distance buffer to keep in front of the ego vehicle + double lat_dist_buffer; // [m] safety distance buffer to keep on the side of the ego vehicle double slow_velocity; double slow_dist_threshold; double stop_dist_threshold;