From 7c146803b02d683a46181696736bc9c4b627c7e5 Mon Sep 17 00:00:00 2001 From: Yuma Nihei Date: Wed, 30 Oct 2024 18:10:59 +0900 Subject: [PATCH] =?UTF-8?q?Revert=20"feat(lane=5Fdeparture=5Fchecker):=20a?= =?UTF-8?q?dd=20parameter=20of=20footprint=5Fextra=5Fmargin=E2=80=A6"?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This reverts commit 02453660899854c91f04705da2f22f5c048071ac. --- 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, 1 insertion(+), 16 deletions(-) diff --git a/control/lane_departure_checker/README.md b/control/lane_departure_checker/README.md index 422418412a9c5..45ce9071140f4 100644 --- a/control/lane_departure_checker/README.md +++ b/control/lane_departure_checker/README.md @@ -43,10 +43,6 @@ 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 @@ -76,7 +72,6 @@ Expand footprint based on the constant defined with `footprint_extra_margin`. | 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 f07543feaca9a..092a6765aa948 100644 --- a/control/lane_departure_checker/config/lane_departure_checker.param.yaml +++ b/control/lane_departure_checker/config/lane_departure_checker.param.yaml @@ -6,7 +6,6 @@ # 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 6390905c60753..3c16b4db06dec 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,7 +50,6 @@ 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 13c9ea73a140e..1e0701eb403c8 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,9 +89,4 @@ 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 bdb210bf8d8cd..ad3fca6f8504e 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,8 +219,7 @@ std::vector LaneDepartureChecker::createVehicleFootprints( const Param & param) { // Calculate longitudinal and lateral margin based on covariance - const auto margin = - calcFootprintMargin(covariance, param.footprint_margin_scale) + param.footprint_extra_margin; + const auto margin = calcFootprintMargin(covariance, param.footprint_margin_scale); // 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 f9a200f1d5531..315458aaf04a8 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,7 +128,6 @@ 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); @@ -353,7 +352,6 @@ 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);