feat(lane_departure_checker): add parameter of footprint_extra_margin

Signed-off-by: Yuma Nihei <yuma.nihei@tier4.jp>

Author: yn-mrse

control/lane_departure_checker/README.md

@@ -43,6 +43,10 @@ This package includes the following features:
 
 2. Expand footprint based on the standard deviation multiplied with `footprint_margin_scale`.
 
+### How to extend footprint by constant
+
+Expand footprint based on the constant defined with `footprint_extra_margin`.
+
 ## Interface
 
 ### Input
@@ -72,6 +76,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

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

@@ -50,6 +50,7 @@ using TrajectoryPoints = std::vector<TrajectoryPoint>;
 struct Param
 {
   double footprint_margin_scale;
+  double footprint_extra_margin;
   double resample_interval;
   double max_deceleration;
   double delay_time;

control/lane_departure_checker/include/lane_departure_checker/util/create_vehicle_footprint.hpp

@@ -89,4 +89,9 @@ inline FootprintMargin calcFootprintMargin(
   return FootprintMargin{Cov_xy_vehicle(0, 0) * scale, Cov_xy_vehicle(1, 1) * scale};
 }
 
+inline FootprintMargin operator+(FootprintMargin fm, const double margin)
+{
+  return FootprintMargin{fm.lon + margin, fm.lat + margin};
+}
+
 #endif  // LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp

@@ -219,7 +219,7 @@ std::vector<LinearRing2d> LaneDepartureChecker::createVehicleFootprints(
   const Param & param)
 {
   // Calculate longitudinal and lateral margin based on covariance
-  const auto margin = calcFootprintMargin(covariance, param.footprint_margin_scale);
+  const auto margin = calcFootprintMargin(covariance, param.footprint_margin_scale) + param.footprint_extra_margin;
 
   // Create vehicle footprint in base_link coordinate
   const auto local_vehicle_footprint = createVehicleFootprint(*vehicle_info_ptr_, margin);

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp

@@ -128,6 +128,7 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o
   vehicle_length_m_ = vehicle_info.vehicle_length_m;
 
   param_.footprint_margin_scale = declare_parameter("footprint_margin_scale", 1.0);
+  param_.footprint_extra_margin = declare_parameter("footprint_extra_margin", 0.0);
   param_.resample_interval = declare_parameter("resample_interval", 0.3);
   param_.max_deceleration = declare_parameter("max_deceleration", 3.0);
   param_.delay_time = declare_parameter("delay_time", 0.3);
@@ -352,6 +353,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);