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

danielsanchezaran · web-flow · commit 6fe9fc648eaa · 2024-06-01T08:06:08.000+09:00
* add param for lane departure margin

Signed-off-by: Daniel Sanchez &lt;danielsanchezaran@gmail.com&gt;

* json thing

Signed-off-by: Daniel Sanchez &lt;danielsanchezaran@gmail.com&gt;

* docs

Signed-off-by: Daniel Sanchez &lt;danielsanchezaran@gmail.com&gt;

* make separate param for lane departure margin expansion

Signed-off-by: Daniel Sanchez &lt;danielsanchezaran@gmail.com&gt;

* update docs

Signed-off-by: Daniel Sanchez &lt;danielsanchezaran@gmail.com&gt;

---------

Signed-off-by: Daniel Sanchez &lt;danielsanchezaran@gmail.com&gt;
diff --git a/control/lane_departure_checker/README.md b/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           |
diff --git a/control/lane_departure_checker/config/lane_departure_checker.param.yaml b/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
diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/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;
diff --git a/control/lane_departure_checker/schema/lane_departure_checker.json b/control/lane_departure_checker/schema/lane_departure_checker.json
@@ -11,6 +11,11 @@
           "default": 1.0,
           "description": "Coefficient for expanding footprint margin. Multiplied by 1 standard deviation."
         },
+        "footprint_extra_margin": {
+          "type": "number",
+          "default": 10.0,
+          "description": "Coefficient for expanding footprint margin."
+        },
         "resample_interval": {
           "type": "number",
           "default": 0.3,
@@ -53,6 +58,7 @@
       },
       "required": [
         "footprint_margin_scale",
+        "footprint_extra_margin",
         "resample_interval",
         "max_deceleration",
         "max_lateral_deviation",
diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/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;
diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/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);
diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md
@@ -494,14 +494,15 @@ See also [[1]](https://www.sciencedirect.com/science/article/pii/S14746670153474
 
 #### parameters for geometric pull out
 
-| Name                        | Unit  | Type   | Description                                                                                                                                               | Default value |
-| :-------------------------- | :---- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
-| enable_geometric_pull_out   | [-]   | bool   | flag whether to enable geometric pull out                                                                                                                 | true          |
-| divide_pull_out_path        | [-]   | bool   | flag whether to divide arc paths. The path is assumed to be divided because the curvature is not continuous. But it requires a stop during the departure. | false         |
-| geometric_pull_out_velocity | [m/s] | double | velocity of geometric pull out                                                                                                                            | 1.0           |
-| arc_path_interval           | [m]   | double | path points interval of arc paths of geometric pull out                                                                                                   | 1.0           |
-| lane_departure_margin       | [m]   | double | margin of deviation to lane right                                                                                                                         | 0.2           |
-| pull_out_max_steer_angle    | [rad] | double | maximum steer angle for path generation                                                                                                                   | 0.26          |
+| Name                                  | Unit  | Type   | Description                                                                                                                                               | Default value |
+| :------------------------------------ | :---- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
+| enable_geometric_pull_out             | [-]   | bool   | flag whether to enable geometric pull out                                                                                                                 | true          |
+| divide_pull_out_path                  | [-]   | bool   | flag whether to divide arc paths. The path is assumed to be divided because the curvature is not continuous. But it requires a stop during the departure. | false         |
+| geometric_pull_out_velocity           | [m/s] | double | velocity of geometric pull out                                                                                                                            | 1.0           |
+| arc_path_interval                     | [m]   | double | path points interval of arc paths of geometric pull out                                                                                                   | 1.0           |
+| lane_departure_margin                 | [m]   | double | margin of deviation to lane right                                                                                                                         | 0.2           |
+| lane_departure_check_expansion_margin | [m]   | double | margin to expand the ego vehicle footprint when doing lane departure checks                                                                               | 0.0           |
+| pull_out_max_steer_angle              | [rad] | double | maximum steer angle for path generation                                                                                                                   | 0.26          |
 
 ## **backward pull out start point search**
 
diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml
@@ -40,6 +40,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
diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp
@@ -71,6 +71,7 @@ struct StartPlannerParameters
   behavior_path_planner::utils::path_safety_checker::ObjectTypesToCheck
     object_types_to_check_for_path_generation{};
   double center_line_path_interval{0.0};
+  double lane_departure_check_expansion_margin{0.0};
 
   // shift pull out
   bool enable_shift_pull_out{false};
diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp
@@ -101,6 +101,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 =
@@ -435,6 +437,9 @@ void StartPlannerModuleManager::updateModuleParams(
     updateParam<double>(
       parameters, ns + "lane_departure_margin",
       p->parallel_parking_parameters.pull_out_lane_departure_margin);
+    updateParam<double>(
+      parameters, ns + "lane_departure_check_expansion_margin",
+      p->lane_departure_check_expansion_margin);
     updateParam<double>(
       parameters, ns + "pull_out_max_steer_angle",
       p->parallel_parking_parameters.pull_out_max_steer_angle);
diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp
@@ -65,6 +65,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) {

Original file line number	Diff line number	Diff line change
`@@ -60,6 +60,7 @@ typedef boost::geometry::index::rtree<Segment2d, boost::geometry::index::rstar<1`
`60`	`60`	`struct Param`
`61`	`61`	`{`
`62`	`62`	`double footprint_margin_scale;`
	`63`	`+ double footprint_extra_margin;`
`63`	`64`	`double resample_interval;`
`64`	`65`	`double max_deceleration;`
`65`	`66`	`double delay_time;`
Original file line number	Diff line number	Diff line change
`@@ -261,7 +261,8 @@ std::vector<LinearRing2d> LaneDepartureChecker::createVehicleFootprints(`
`261`	`261`	`const PathWithLaneId & path) const`
`262`	`262`	`{`
`263`	`263`	`// Create vehicle footprint in base_link coordinate`
`264`		`- const auto local_vehicle_footprint = vehicle_info_ptr_->createFootprint();`
	`264`	`+ const auto local_vehicle_footprint =`
	`265`	`+ vehicle_info_ptr_->createFootprint(param_.footprint_extra_margin);`
`265`	`266`
`266`	`267`	`// Create vehicle footprint on each Path point`
`267`	`268`	`std::vector<LinearRing2d> vehicle_footprints;`