From 769e091e2408a3dbaab073f9a3284b9ee3a5ab75 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 14 Aug 2024 01:10:51 +0900 Subject: [PATCH 1/2] fix(goal_planner): fix lane departure check not working correctly due to uninitialized variable (#8449) Signed-off-by: kosuke55 --- .../README.md | 19 ++++++++++--------- .../config/goal_planner.param.yaml | 1 + .../goal_planner_parameters.hpp | 1 + .../src/goal_planner_module.cpp | 4 ++++ .../src/manager.cpp | 5 +++++ 5 files changed, 21 insertions(+), 9 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md index 3e155aba0af2e..5e93c86c9be1c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md @@ -206,15 +206,16 @@ The main thread will be the one called from the planner manager flow. - The path candidates generated there are referred to by the main thread, and the one judged to be valid for the current planner data (e.g. ego and object information) is selected from among them. valid means no sudden deceleration, no collision with obstacles, etc. The selected path will be the output of this module. - If there is no path selected, or if the selected path is collision and ego is stuck, a separate thread(freespace path generation thread) will generate a path using freespace planning algorithm. If a valid free space path is found, it will be the output of the module. If the object moves and the pull over path generated along the lane is collision-free, the path is used as output again. See also the section on freespace parking for more information on the flow of generating freespace paths. -| Name | Unit | Type | Description | Default value | -| :------------------------------- | :----- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :--------------------------------------- | -| pull_over_minimum_request_length | [m] | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, pull over is activated. | 100.0 | -| pull_over_velocity | [m/s] | double | decelerate to this speed by the goal search area | 3.0 | -| pull_over_minimum_velocity | [m/s] | double | speed of pull_over after stopping once. this prevents excessive acceleration. | 1.38 | -| decide_path_distance | [m] | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0 | -| maximum_deceleration | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly | 1.0 | -| path_priority | [-] | string | In case `efficient_path` use a goal that can generate an efficient path which is set in `efficient_path_order`. In case `close_goal` use the closest goal to the original one. | efficient_path | -| efficient_path_order | [-] | string | efficient order of pull over planner along lanes excluding freespace pull over | ["SHIFT", "ARC_FORWARD", "ARC_BACKWARD"] | +| Name | Unit | Type | Description | Default value | +| :------------------------------------ | :----- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :--------------------------------------- | +| pull_over_minimum_request_length | [m] | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, pull over is activated. | 100.0 | +| pull_over_velocity | [m/s] | double | decelerate to this speed by the goal search area | 3.0 | +| pull_over_minimum_velocity | [m/s] | double | speed of pull_over after stopping once. this prevents excessive acceleration. | 1.38 | +| decide_path_distance | [m] | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0 | +| maximum_deceleration | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly | 1.0 | +| path_priority | [-] | string | In case `efficient_path` use a goal that can generate an efficient path which is set in `efficient_path_order`. In case `close_goal` use the closest goal to the original one. | efficient_path | +| efficient_path_order | [-] | string | efficient order of pull over planner along lanes excluding freespace pull over | ["SHIFT", "ARC_FORWARD", "ARC_BACKWARD"] | +| lane_departure_check_expansion_margin | [m] | double | margin to expand the ego vehicle footprint when doing lane departure checks | 0.0 | ### **shift parking** diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml index e6f84e7eece0c..156483df65f20 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml @@ -54,6 +54,7 @@ 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 # shift parking shift_parking: diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp index 6a10c0fb183f6..1272ea38451ee 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp @@ -89,6 +89,7 @@ struct GoalPlannerParameters double maximum_jerk{0.0}; std::string path_priority; // "efficient_path" or "close_goal" std::vector efficient_path_order{}; + double lane_departure_check_expansion_margin{0.0}; // shift path bool enable_shift_parking{false}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index dc7a76031a3c1..ed2e3f30540b6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -68,6 +68,10 @@ GoalPlannerModule::GoalPlannerModule( { LaneDepartureChecker lane_departure_checker{}; lane_departure_checker.setVehicleInfo(vehicle_info_); + lane_departure_checker::Param lane_departure_checker_params; + lane_departure_checker_params.footprint_extra_margin = + parameters->lane_departure_check_expansion_margin; + lane_departure_checker.setParam(lane_departure_checker_params); occupancy_grid_map_ = std::make_shared(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp index b86bbbca7d98b..2d565607ec610 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -137,6 +137,8 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node) p.path_priority = node->declare_parameter(ns + "path_priority"); p.efficient_path_order = node->declare_parameter>(ns + "efficient_path_order"); + p.lane_departure_check_expansion_margin = + node->declare_parameter(ns + "lane_departure_check_expansion_margin"); } // shift parking @@ -526,6 +528,9 @@ void GoalPlannerModuleManager::updateModuleParams( updateParam(parameters, ns + "deceleration_interval", p->deceleration_interval); updateParam( parameters, ns + "after_shift_straight_distance", p->after_shift_straight_distance); + updateParam( + parameters, ns + "lane_departure_check_expansion_margin", + p->lane_departure_check_expansion_margin); } // parallel parking common From 60356be1ed4bee51f8d7f3d96b976f0a29d3c770 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 14 Aug 2024 10:56:03 +0900 Subject: [PATCH 2/2] fix(lane_departure_checker): fix uninitialized variables (#8451) fix(lane_departure_checker): fix uninitialized_variables Signed-off-by: kosuke55 --- .../lane_departure_checker.hpp | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp index b43a7d0d0369a..7aeedd096862d 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp @@ -59,18 +59,18 @@ typedef boost::geometry::index::rtree