feat(start_planner,lane_departure_checker): add margin param for lane departure check (autowarefoundation#7193)

* add param for lane departure margin

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* json thing

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* docs

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* make separate param for lane departure margin expansion

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* update docs

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

---------

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

Author: danielsanchezaran

control/lane_departure_checker/README.md

@@ -93,6 +93,7 @@ This package includes the following features:
 | Name                       | Type   | Description                                                                        | Default value |
 | :------------------------- | :----- | :--------------------------------------------------------------------------------- | :------------ |
 | footprint_margin_scale     | double | Coefficient for expanding footprint margin. Multiplied by 1 standard deviation.    | 1.0           |
+| footprint_extra_margin     | double | Coefficient for expanding footprint margin. When checking for lane departure       | 0.0           |
 | resample_interval          | double | Minimum Euclidean distance between points when resample trajectory.[m]             | 0.3           |
 | max_deceleration           | double | Maximum deceleration when calculating braking distance.                            | 2.8           |
 | delay_time                 | double | Delay time which took to actuate brake when calculating braking distance. [second] | 1.3           |

control/lane_departure_checker/config/lane_departure_checker.param.yaml

@@ -17,6 +17,7 @@
 
     # Core
     footprint_margin_scale: 1.0
+    footprint_extra_margin: 0.0
     resample_interval: 0.3
     max_deceleration: 2.8
     delay_time: 1.3

control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp

@@ -60,6 +60,7 @@ 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;

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp

@@ -261,7 +261,8 @@ std::vector<LinearRing2d> LaneDepartureChecker::createVehicleFootprints(
   const PathWithLaneId & path) const
 {
   // Create vehicle footprint in base_link coordinate
-  const auto local_vehicle_footprint = vehicle_info_ptr_->createFootprint();
+  const auto local_vehicle_footprint =
+    vehicle_info_ptr_->createFootprint(param_.footprint_extra_margin);
 
   // Create vehicle footprint on each Path point
   std::vector<LinearRing2d> vehicle_footprints;

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp

@@ -150,6 +150,7 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o
 
   // Core Parameter
   param_.footprint_margin_scale = declare_parameter<double>("footprint_margin_scale");
+  param_.footprint_extra_margin = declare_parameter<double>("footprint_extra_margin");
   param_.resample_interval = declare_parameter<double>("resample_interval");
   param_.max_deceleration = declare_parameter<double>("max_deceleration");
   param_.delay_time = declare_parameter<double>("delay_time");
@@ -403,6 +404,7 @@ rcl_interfaces::msg::SetParametersResult LaneDepartureCheckerNode::onParameter(
 
     // Core
     update_param(parameters, "footprint_margin_scale", param_.footprint_margin_scale);
+    update_param(parameters, "footprint_extra_margin", param_.footprint_extra_margin);
     update_param(parameters, "resample_interval", param_.resample_interval);
     update_param(parameters, "max_deceleration", param_.max_deceleration);
     update_param(parameters, "delay_time", param_.delay_time);

planning/behavior_path_start_planner_module/config/start_planner.param.yaml

@@ -26,6 +26,7 @@
       geometric_pull_out_velocity: 1.0
       arc_path_interval: 1.0
       lane_departure_margin: 0.2
+      lane_departure_check_expansion_margin: 0.0
       backward_velocity: -1.0
       pull_out_max_steer_angle: 0.26  # 15deg
       # search start pose backward

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp

@@ -47,6 +47,7 @@ struct StartPlannerParameters
   double collision_check_distance_from_end{0.0};
   double th_moving_object_velocity{0.0};
   double center_line_path_interval{0.0};
+  double lane_departure_check_expansion_margin{0.0};
 
   // shift pull out
   bool enable_shift_pull_out{false};

planning/behavior_path_start_planner_module/src/manager.cpp

@@ -71,6 +71,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
     node->declare_parameter<double>(ns + "arc_path_interval");
   p.parallel_parking_parameters.pull_out_lane_departure_margin =
     node->declare_parameter<double>(ns + "lane_departure_margin");
+  p.lane_departure_check_expansion_margin =
+    node->declare_parameter<double>(ns + "lane_departure_check_expansion_margin");
   p.parallel_parking_parameters.pull_out_max_steer_angle =
     node->declare_parameter<double>(ns + "pull_out_max_steer_angle");  // 15deg
   p.parallel_parking_parameters.center_line_path_interval =

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -62,6 +62,11 @@ StartPlannerModule::StartPlannerModule(
 {
   lane_departure_checker_ = std::make_shared<LaneDepartureChecker>();
   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);
 
   // set enabled planner
   if (parameters_->enable_shift_pull_out) {