Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(goal_planner): introduce bezier based pullover for bus stop area #10005

Closed
wants to merge 17 commits into from
+344 −122
Closed

feat(goal_planner): introduce bezier based pullover for bus stop area #10005

Show file tree
Hide file tree
Changes from 10 commits
Commits
Show all changes
17 commits
Select commit Hold shift + click to select a range
2cffe15
selected path chatters
soblin Dec 15, 2024
2fc6c18
fix chattering
soblin Jan 21, 2025
7a6ba86
improve
soblin Jan 22, 2025
03dc49b
remove rviz config
soblin Jan 22, 2025
20eb08d
fix
soblin Jan 22, 2025
7e1bdbd
revert align pose
soblin Jan 23, 2025
39ca583
fix to work as before
soblin Jan 23, 2025
2ac0e6f
only sort based on total curvature at bezier thread side
soblin Jan 23, 2025
b3ee2dd
divide param
soblin Jan 23, 2025
14977d7
fix turn signal
soblin Jan 23, 2025
bc8f304
reflect comment
soblin Jan 23, 2025
19d72ee
add prepare_length param, divide util
soblin Jan 24, 2025
f01e280
change trigger condition
soblin Jan 24, 2025
72a4a89
Merge branch 'main' into feat/bezier-pullover-bus
soblin Jan 24, 2025
b6c1fdc
Merge branch 'main' into feat/bezier-pullover-bus
soblin Jan 26, 2025
a5d9151
revert stop path
soblin Jan 27, 2025
b31d608
revert stop path(2)
soblin Jan 27, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
30 changes: 17 additions & 13 deletions ...or_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,8 +25,8 @@
high_curvature_threshold: 0.1
bus_stop_area:
use_bus_stop_area: false
goal_search_interval: 0.5
lateral_offset_interval: 0.25
goal_search_interval: 0.75
lateral_offset_interval: 0.3

# occupancy grid map
occupancy_grid:
Expand All @@ -51,15 +51,15 @@

# pull over
pull_over:
minimum_request_length: 0.0
minimum_request_length: 100.0
pull_over_velocity: 3.0
pull_over_minimum_velocity: 1.38
decide_path_distance: 10.0
maximum_deceleration: 1.0
maximum_jerk: 1.0
path_priority: "efficient_path" # "efficient_path" or "close_goal"
efficient_path_order: ["SHIFT", "ARC_FORWARD", "ARC_BACKWARD"] # only lane based pull over(exclude freespace parking)
lane_departure_check_expansion_margin: 0.0
lane_departure_check_expansion_margin: 0.2

# shift parking
shift_parking:
Expand All @@ -68,26 +68,26 @@
maximum_lateral_jerk: 2.0
minimum_lateral_jerk: 0.5
deceleration_interval: 15.0
after_shift_straight_distance: 1.0
after_shift_straight_distance: 1.5

# parallel parking path
parallel_parking:
path_interval: 1.0
max_steer_angle: 0.35 # 20deg
max_steer_angle: 0.4 #22.9deg
forward:
enable_arc_forward_parking: true
after_forward_parking_straight_distance: 2.0
forward_parking_velocity: 1.38
forward_parking_lane_departure_margin: 0.0
forward_parking_path_interval: 1.0
forward_parking_max_steer_angle: 0.35 # 20deg
forward_parking_max_steer_angle: 0.4 # 22.9deg
backward:
enable_arc_backward_parking: true
after_backward_parking_straight_distance: 2.0
backward_parking_velocity: -1.38
backward_parking_lane_departure_margin: 0.0
backward_parking_path_interval: 1.0
backward_parking_max_steer_angle: 0.35 # 20deg
backward_parking_max_steer_angle: 0.4 # 22.9deg

# freespace parking
freespace_parking:
Expand Down Expand Up @@ -130,6 +130,10 @@
neighbor_radius: 8.0
margin: 1.0

bezier_parking:
pull_over_azimuth_threshold: 0.5
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

nits:

when I hear azimuth in autoware I image

/**
 * @brief calculate azimuth angle of two points.
 * @details This function returns the azimuth angle of the position of the two input points
 *          with respect to the origin of their coordinate system.
 *          If x and y of the two points are the same, the calculation result will be unstable.
 * @param p_from source point
 * @param p_to target point
 * @return -pi < azimuth angle < pi.
 */
double calcAzimuthAngle(
  const geometry_msgs::msg::Point & p_from, const geometry_msgs::msg::Point & p_to)
{
  const double dx = p_to.x - p_from.x;
  const double dy = p_to.y - p_from.y;
  return std::atan2(dy, dx);
}

but here azimuth is two pose orientaion diff.
add explanation or rename it

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

fixed in bc8f304

after_shift_straight_distance: 1.5

stop_condition:
maximum_deceleration_for_stop: 1.0
maximum_jerk_for_stop: 1.0
Expand All @@ -139,13 +143,13 @@
min_velocity: 1.0
min_acceleration: 1.0
max_velocity: 1.0
time_horizon_for_front_object: 10.0
time_horizon_for_rear_object: 10.0
time_horizon_for_front_object: 5.0
time_horizon_for_rear_object: 5.0
time_resolution: 0.5
delay_until_departure: 1.0
# For target object filtering
target_filtering:
safety_check_time_horizon: 10.0
safety_check_time_horizon: 5.0
safety_check_time_resolution: 1.0
# detection range
object_check_forward_distance: 100.0
Expand Down Expand Up @@ -179,7 +183,7 @@
# safety check configuration
enable_safety_check: true
method: "integral_predicted_polygon"
keep_unsafe_time: 3.0
keep_unsafe_time: 0.5
# collision check parameters
publish_debug_marker: false
rss_params:
Expand All @@ -192,7 +196,7 @@
forward_margin: 1.0
backward_margin: 1.0
lat_margin: 1.0
time_horizon: 10.0
time_horizon: 5.0
# hysteresis factor to expand/shrink polygon with the value
hysteresis_factor_expand_rate: 1.0
collision_check_yaw_diff_threshold: 3.1416
Expand Down
27 changes: 22 additions & 5 deletions ...planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#include "autoware/behavior_path_goal_planner_module/decision_state.hpp"
#include "autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp"
#include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp"
#include "autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp"
#include "autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp"
#include "autoware/behavior_path_goal_planner_module/thread_data.hpp"
#include "autoware/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp"
Expand Down Expand Up @@ -86,8 +87,8 @@ struct PullOverContextData
explicit PullOverContextData(
const bool is_stable_safe_path, const PredictedObjects & static_objects,
const PredictedObjects & dynamic_objects, const PathDecisionState & prev_state,
const bool is_stopped, const LaneParkingResponse & lane_parking_response,
const FreespaceParkingResponse & freespace_parking_response)
const bool is_stopped, LaneParkingResponse && lane_parking_response,
FreespaceParkingResponse && freespace_parking_response)
: is_stable_safe_path(is_stable_safe_path),
static_target_objects(static_objects),
dynamic_target_objects(dynamic_objects),
Expand All @@ -112,8 +113,8 @@ struct PullOverContextData
void update(
const bool is_stable_safe_path_, const PredictedObjects static_target_objects_,
const PredictedObjects dynamic_target_objects_, const PathDecisionState prev_state_for_debug_,
const bool is_stopped_, const LaneParkingResponse & lane_parking_response_,
const FreespaceParkingResponse & freespace_parking_response_)
const bool is_stopped_, LaneParkingResponse && lane_parking_response_,
FreespaceParkingResponse && freespace_parking_response_)
{
is_stable_safe_path = is_stable_safe_path_;
static_target_objects = static_target_objects_;
Expand Down Expand Up @@ -196,6 +197,21 @@ class LaneParkingPlanner
original_upstream_module_output_; //<! upstream_module_output used for generating last
// pull_over_path_candidates(only updated when new candidates
// are generated)
std::shared_ptr<BezierPullOver> bezier_pull_over_planner_;
const double pull_over_azimuth_threshold;

bool switch_bezier_{false};
void shift_planning_helper(
const std::shared_ptr<PlannerData> planner_data, const GoalCandidates & goal_candidates,
const BehaviorModuleOutput & upstream_module_output,
const lanelet::ConstLanelets current_lanelets, std::optional<Pose> & closest_start_pose,
std::vector<PullOverPath> & path_candidates);
void bezier_planning_helper(
const std::shared_ptr<PlannerData> planner_data, const GoalCandidates & goal_candidates,
const BehaviorModuleOutput & upstream_module_output,
const lanelet::ConstLanelets current_lanelets, std::optional<Pose> & closest_start_pose,
std::vector<PullOverPath> & path_candidates,
std::optional<std::vector<size_t>> & sorted_indices) const;
};

class FreespaceParkingPlanner
Expand Down Expand Up @@ -433,7 +449,8 @@ class GoalPlannerModule : public SceneModuleInterface
std::optional<PullOverPath> selectPullOverPath(
const PullOverContextData & context_data,
const std::vector<PullOverPath> & pull_over_path_candidates,
const GoalCandidates & goal_candidates) const;
const GoalCandidates & goal_candidates,
const std::optional<std::vector<size_t>> sorted_bezier_indices_opt) const;

// lanes and drivable area
std::vector<DrivableLanes> generateDrivableLanes() const;
Expand Down
6 changes: 6 additions & 0 deletions ...ner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -119,6 +119,12 @@ struct GoalPlannerParameters
AstarParam astar_parameters{};
RRTStarParam rrt_star_parameters{};

struct BezierParking
{
double pull_over_azimuth_threshold;
double after_shift_straight_distance;
} bezier_parking;

// stop condition
double maximum_deceleration_for_stop{0.0};
double maximum_jerk_for_stop{0.0};
Expand Down
7 changes: 6 additions & 1 deletion ...e/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,6 +68,10 @@ struct PullOverPath
const std::vector<double> & parking_path_curvatures() const { return parking_path_curvatures_; }
double full_path_max_curvature() const { return full_path_max_curvature_; }
double parking_path_max_curvature() const { return parking_path_max_curvature_; }
double parking_path_curvature_total_derivative() const
{
return parking_path_curvature_total_derivative_;
}
size_t path_idx() const { return path_idx_; }

bool incrementPathIndex();
Expand All @@ -94,7 +98,7 @@ struct PullOverPath
const PathWithLaneId & full_path, const PathWithLaneId & parking_path,
const std::vector<double> & full_path_curvatures,
const std::vector<double> & parking_path_curvatures, const double full_path_max_curvature,
const double parking_path_max_curvature,
const double parking_path_max_curvature, const double parking_path_curvature_total_derivative,
const std::vector<std::pair<double, double>> & pairs_terminal_velocity_and_accel);

PullOverPlannerType type_;
Expand All @@ -109,6 +113,7 @@ struct PullOverPath
std::vector<double> parking_path_curvatures_;
double full_path_max_curvature_;
double parking_path_max_curvature_;
double parking_path_curvature_total_derivative_;

// accelerate with constant acceleration to the target velocity
size_t path_idx_;
Expand Down
1 change: 1 addition & 0 deletions ...th_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,7 @@ struct LaneParkingResponse
{
std::vector<PullOverPath> pull_over_path_candidates;
std::optional<Pose> closest_start_pose;
std::optional<std::vector<size_t>> sorted_bezier_indices_opt;
};

class FreespaceParkingRequest
Expand Down
Loading
Loading