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..9512ed388daaf 100644 --- 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 index fd21d18b98a64..9605f53cde1d3 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 @@ -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 index e3e753d9e4023..1d594180a638b 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 @@ -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/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 f0274e5c4712f..661a189ef20bb 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 @@ -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("ego_nearest_dist_threshold"); param_.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); + param_.min_braking_distance = declare_parameter("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_);