From 02453660899854c91f04705da2f22f5c048071ac Mon Sep 17 00:00:00 2001 From: Yuma Nihei Date: Tue, 1 Oct 2024 20:56:55 +0900 Subject: [PATCH] feat(lane_departure_checker): add parameter of footprint_extra_margin (#1565) * feat(lane_departure_checker): add parameter of footprint_extra_margin Signed-off-by: Yuma Nihei * ci(pre-commit): autofix * chore: update readme Co-authored-by: Shigekazu Fukuta <107168699+sfukuta@users.noreply.github.com> * ci(pre-commit): autofix --------- Signed-off-by: Yuma Nihei Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Shigekazu Fukuta <107168699+sfukuta@users.noreply.github.com> --- control/lane_departure_checker/README.md | 5 +++++ .../config/lane_departure_checker.param.yaml | 1 + .../lane_departure_checker/lane_departure_checker.hpp | 1 + .../lane_departure_checker/util/create_vehicle_footprint.hpp | 5 +++++ .../lane_departure_checker_node/lane_departure_checker.cpp | 3 ++- .../lane_departure_checker_node.cpp | 2 ++ 6 files changed, 16 insertions(+), 1 deletion(-) diff --git a/control/lane_departure_checker/README.md b/control/lane_departure_checker/README.md index 45ce9071140f4..422418412a9c5 100644 --- a/control/lane_departure_checker/README.md +++ b/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.[m] | 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 index 092a6765aa948..f07543feaca9a 100644 --- a/control/lane_departure_checker/config/lane_departure_checker.param.yaml +++ b/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 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 index 3c16b4db06dec..6390905c60753 100644 --- 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 @@ -50,6 +50,7 @@ using TrajectoryPoints = std::vector; 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/include/lane_departure_checker/util/create_vehicle_footprint.hpp b/control/lane_departure_checker/include/lane_departure_checker/util/create_vehicle_footprint.hpp index 1e0701eb403c8..13c9ea73a140e 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/util/create_vehicle_footprint.hpp +++ b/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_ 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 index ad3fca6f8504e..bdb210bf8d8cd 100644 --- 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 @@ -219,7 +219,8 @@ std::vector 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); 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 index 315458aaf04a8..f9a200f1d5531 100644 --- 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 @@ -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);