feat(lane_departure_checker): add braking distance offset to avoid un…

…wanted move after stop (autowarefoundation#2712) Signed-off-by: Berkay Karaman <[email protected]>
Interplai · Jan 24, 2023 · dd022c4 · dd022c4
1 parent 403713d
commit dd022c4
Show file tree

Hide file tree

Showing 4 changed files with 7 additions and 2 deletions.
diff --git a/control/lane_departure_checker/config/lane_departure_checker.param.yaml b/control/lane_departure_checker/config/lane_departure_checker.param.yaml
@@ -13,3 +13,4 @@
     max_longitudinal_deviation: 2.0
     max_yaw_deviation_deg: 60.0
     delta_yaw_threshold_for_closest_point: 1.570 #M_PI/2.0, delta yaw thres for closest point
+    min_braking_distance: 0.0
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
@@ -58,6 +58,7 @@ struct Param
   double max_lateral_deviation;
   double max_longitudinal_deviation;
   double max_yaw_deviation_deg;
+  double min_braking_distance;
   // nearest search to ego
   double ego_nearest_dist_threshold;
   double ego_nearest_yaw_threshold;

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
@@ -109,8 +109,9 @@ Output LaneDepartureChecker::update(const Input & input)
     const auto & raw_abs_velocity = std::abs(input.current_odom->twist.twist.linear.x);
     const auto abs_velocity = raw_abs_velocity < min_velocity ? 0.0 : raw_abs_velocity;
 
-    const auto braking_distance =
-      calcBrakingDistance(abs_velocity, param_.max_deceleration, param_.delay_time);
+    const auto braking_distance = std::max(
+      param_.min_braking_distance,
+      calcBrakingDistance(abs_velocity, param_.max_deceleration, param_.delay_time));
 
     output.resampled_trajectory = cutTrajectory(
       resampleTrajectory(*input.predicted_trajectory, param_.resample_interval), braking_distance);

diff --git a/...ol/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/...ol/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp
@@ -144,6 +144,7 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o
   param_.max_yaw_deviation_deg = declare_parameter("max_yaw_deviation_deg", 30.0);
   param_.ego_nearest_dist_threshold = declare_parameter<double>("ego_nearest_dist_threshold");
   param_.ego_nearest_yaw_threshold = declare_parameter<double>("ego_nearest_yaw_threshold");
+  param_.min_braking_distance = declare_parameter<double>("min_braking_distance");
 
   // Parameter Callback
   set_param_res_ =
@@ -367,6 +368,7 @@ rcl_interfaces::msg::SetParametersResult LaneDepartureCheckerNode::onParameter(
     update_param(parameters, "resample_interval", param_.resample_interval);
     update_param(parameters, "max_deceleration", param_.max_deceleration);
     update_param(parameters, "delay_time", param_.delay_time);
+    update_param(parameters, "min_braking_distance", param_.min_braking_distance);
 
     if (lane_departure_checker_) {
       lane_departure_checker_->setParam(param_);