Merge pull request #1465 from tier4/fix/goal_planner_0.29

fix(goal_planner): fix lane departure check not working correctly due to uninitialized variable

control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp

@@ -59,18 +59,18 @@ typedef boost::geometry::index::rtree<Segment2d, boost::geometry::index::rstar<1
 
 struct Param
 {
-  double footprint_margin_scale;
-  double footprint_extra_margin;
-  double resample_interval;
-  double max_deceleration;
-  double delay_time;
-  double max_lateral_deviation;
-  double max_longitudinal_deviation;
-  double max_yaw_deviation_deg;
-  double min_braking_distance;
+  double footprint_margin_scale{0.0};
+  double footprint_extra_margin{0.0};
+  double resample_interval{0.0};
+  double max_deceleration{0.0};
+  double delay_time{0.0};
+  double max_lateral_deviation{0.0};
+  double max_longitudinal_deviation{0.0};
+  double max_yaw_deviation_deg{0.0};
+  double min_braking_distance{0.0};
   // nearest search to ego
-  double ego_nearest_dist_threshold;
-  double ego_nearest_yaw_threshold;
+  double ego_nearest_dist_threshold{0.0};
+  double ego_nearest_yaw_threshold{0.0};
 };
 
 struct Input

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**
 

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:

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<std::string> efficient_path_order{};
+  double lane_departure_check_expansion_margin{0.0};
 
   // shift path
   bool enable_shift_parking{false};

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<OccupancyGridBasedCollisionDetector>();
 

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<std::string>(ns + "path_priority");
     p.efficient_path_order =
       node->declare_parameter<std::vector<std::string>>(ns + "efficient_path_order");
+    p.lane_departure_check_expansion_margin =
+      node->declare_parameter<double>(ns + "lane_departure_check_expansion_margin");
   }
 
   // shift parking
@@ -526,6 +528,9 @@ void GoalPlannerModuleManager::updateModuleParams(
     updateParam<double>(parameters, ns + "deceleration_interval", p->deceleration_interval);
     updateParam<double>(
       parameters, ns + "after_shift_straight_distance", p->after_shift_straight_distance);
+    updateParam<double>(
+      parameters, ns + "lane_departure_check_expansion_margin",
+      p->lane_departure_check_expansion_margin);
   }
 
   // parallel parking common