From a84418ac2e4c8fd3baa98420b468b903726b6390 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Thu, 21 Sep 2023 14:13:15 +0900 Subject: [PATCH] feat(vehicle_cmd_gate)!: add steer and steer_rate filter (#5044) * feat(vehicle_cmd_gate): add steering angle and rate filter Signed-off-by: Takamasa Horibe * update test Signed-off-by: Takamasa Horibe --------- Signed-off-by: Takamasa Horibe --- .../config/vehicle_cmd_gate.param.yaml | 4 + .../src/vehicle_cmd_filter.cpp | 57 +++++++++- .../src/vehicle_cmd_filter.hpp | 7 ++ .../vehicle_cmd_gate/src/vehicle_cmd_gate.cpp | 4 + .../test_filter_in_vehicle_cmd_gate_node.cpp | 4 + .../test/src/test_vehicle_cmd_filter.cpp | 107 ++++++++++++++++-- 6 files changed, 170 insertions(+), 13 deletions(-) diff --git a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml index 92844c61f6f4f..191e894622af7 100644 --- a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml +++ b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml @@ -15,6 +15,8 @@ nominal: vel_lim: 25.0 reference_speed_points: [20.0, 30.0] + steer_lim: [1.0, 0.8] + steer_rate_lim: [1.0, 0.8] lon_acc_lim: [5.0, 4.0] lon_jerk_lim: [5.0, 4.0] lat_acc_lim: [5.0, 4.0] @@ -23,6 +25,8 @@ on_transition: vel_lim: 50.0 reference_speed_points: [20.0, 30.0] + steer_lim: [1.0, 0.8] + steer_rate_lim: [1.0, 0.8] lon_acc_lim: [1.0, 0.9] lon_jerk_lim: [0.5, 0.4] lat_acc_lim: [2.0, 1.8] diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp index 6a33855fb4d0a..4431534a76a5e 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp @@ -29,7 +29,8 @@ bool VehicleCmdFilter::setParameterWithValidation(const VehicleCmdFilterParam & const auto s = p.reference_speed_points.size(); if ( p.lon_acc_lim.size() != s || p.lon_jerk_lim.size() != s || p.lat_acc_lim.size() != s || - p.lat_jerk_lim.size() != s || p.actual_steer_diff_lim.size() != s) { + p.lat_jerk_lim.size() != s || p.actual_steer_diff_lim.size() != s || p.steer_lim.size() != s || + p.steer_rate_lim.size() != s) { std::cerr << "VehicleCmdFilter::setParam() There is a size mismatch in the parameter. " "Parameter initialization failed." << std::endl; @@ -39,7 +40,18 @@ bool VehicleCmdFilter::setParameterWithValidation(const VehicleCmdFilterParam & param_ = p; return true; } - +void VehicleCmdFilter::setSteerLim(LimitArray v) +{ + auto tmp = param_; + tmp.steer_lim = v; + setParameterWithValidation(tmp); +} +void VehicleCmdFilter::setSteerRateLim(LimitArray v) +{ + auto tmp = param_; + tmp.steer_rate_lim = v; + setParameterWithValidation(tmp); +} void VehicleCmdFilter::setLonAccLim(LimitArray v) { auto tmp = param_; @@ -151,12 +163,36 @@ void VehicleCmdFilter::limitActualSteerDiff( void VehicleCmdFilter::limitLateralSteer(AckermannControlCommand & input) const { - // TODO(Horibe): parametrize the max steering angle. - // TODO(Horibe): support steering greater than PI/2. Now the lateral acceleration - // calculation does not support bigger steering value than PI/2 due to tan/atan calculation. - constexpr float steer_limit = M_PI_2; + const float steer_limit = getSteerLim(); + input.lateral.steering_tire_angle = std::clamp(input.lateral.steering_tire_angle, -steer_limit, steer_limit); + + // TODO(Horibe): support steering greater than PI/2. Now the lateral acceleration + // calculation does not support bigger steering value than PI/2 due to tan/atan calculation. + if (std::abs(input.lateral.steering_tire_angle) > M_PI_2f) { + std::cerr << "[vehicle_Cmd_gate] limitLateralSteer(): steering limit is set to pi/2 since the " + "current filtering logic can not handle the steering larger than pi/2. Please " + "check the steering angle limit." + << std::endl; + + std::clamp(input.lateral.steering_tire_angle, -M_PI_2f, M_PI_2f); + } +} + +void VehicleCmdFilter::limitLateralSteerRate(const double dt, AckermannControlCommand & input) const +{ + const float steer_rate_limit = getSteerRateLim(); + + // for steering angle rate + input.lateral.steering_tire_rotation_rate = + std::clamp(input.lateral.steering_tire_rotation_rate, -steer_rate_limit, steer_rate_limit); + + // for steering angle + const float steer_diff_limit = steer_rate_limit * dt; + float ds = input.lateral.steering_tire_angle - prev_cmd_.lateral.steering_tire_angle; + ds = std::clamp(ds, -steer_diff_limit, steer_diff_limit); + input.lateral.steering_tire_angle = prev_cmd_.lateral.steering_tire_angle + ds; } void VehicleCmdFilter::filterAll( @@ -165,6 +201,7 @@ void VehicleCmdFilter::filterAll( { const auto cmd_orig = cmd; limitLateralSteer(cmd); + limitLateralSteerRate(dt, cmd); limitLongitudinalWithJerk(dt, cmd); limitLongitudinalWithAcc(dt, cmd); limitLongitudinalWithVel(cmd); @@ -267,6 +304,14 @@ double VehicleCmdFilter::getLatJerkLim() const { return interpolateFromSpeed(param_.lat_jerk_lim); } +double VehicleCmdFilter::getSteerLim() const +{ + return interpolateFromSpeed(param_.steer_lim); +} +double VehicleCmdFilter::getSteerRateLim() const +{ + return interpolateFromSpeed(param_.steer_rate_lim); +} double VehicleCmdFilter::getSteerDiffLim() const { return interpolateFromSpeed(param_.actual_steer_diff_lim); diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp index a79b8a38bae8d..cf7a1f627a68a 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp @@ -37,6 +37,8 @@ struct VehicleCmdFilterParam LimitArray lon_jerk_lim; LimitArray lat_acc_lim; LimitArray lat_jerk_lim; + LimitArray steer_lim; + LimitArray steer_rate_lim; LimitArray actual_steer_diff_lim; }; class VehicleCmdFilter @@ -47,6 +49,8 @@ class VehicleCmdFilter void setWheelBase(double v) { param_.wheel_base = v; } void setVelLim(double v) { param_.vel_lim = v; } + void setSteerLim(LimitArray v); + void setSteerRateLim(LimitArray v); void setLonAccLim(LimitArray v); void setLonJerkLim(LimitArray v); void setLatAccLim(LimitArray v); @@ -64,6 +68,7 @@ class VehicleCmdFilter void limitActualSteerDiff( const double current_steer_angle, AckermannControlCommand & input) const; void limitLateralSteer(AckermannControlCommand & input) const; + void limitLateralSteerRate(const double dt, AckermannControlCommand & input) const; void filterAll( const double dt, const double current_steer_angle, AckermannControlCommand & input, IsFilterActivated & is_activated) const; @@ -90,6 +95,8 @@ class VehicleCmdFilter double getLonJerkLim() const; double getLatAccLim() const; double getLatJerkLim() const; + double getSteerLim() const; + double getSteerRateLim() const; double getSteerDiffLim() const; }; } // namespace vehicle_cmd_gate diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index 6d3893cbb9ca9..2f2083f626f6b 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -169,6 +169,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) p.vel_lim = declare_parameter("nominal.vel_lim"); p.reference_speed_points = declare_parameter>("nominal.reference_speed_points"); + p.steer_lim = declare_parameter>("nominal.steer_lim"); + p.steer_rate_lim = declare_parameter>("nominal.steer_rate_lim"); p.lon_acc_lim = declare_parameter>("nominal.lon_acc_lim"); p.lon_jerk_lim = declare_parameter>("nominal.lon_jerk_lim"); p.lat_acc_lim = declare_parameter>("nominal.lat_acc_lim"); @@ -184,6 +186,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) p.vel_lim = declare_parameter("on_transition.vel_lim"); p.reference_speed_points = declare_parameter>("on_transition.reference_speed_points"); + p.steer_lim = declare_parameter>("on_transition.steer_lim"); + p.steer_rate_lim = declare_parameter>("on_transition.steer_rate_lim"); p.lon_acc_lim = declare_parameter>("on_transition.lon_acc_lim"); p.lon_jerk_lim = declare_parameter>("on_transition.lon_jerk_lim"); p.lat_acc_lim = declare_parameter>("on_transition.lat_acc_lim"); diff --git a/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp b/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp index 1156656385117..f02235ed1ecbf 100644 --- a/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp +++ b/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp @@ -45,6 +45,8 @@ void print_values(int i, const T1 & name, const T2 & a, const T3 &... b) // global params const std::vector reference_speed_points = {5., 10., 15., 20.}; +const std::vector steer_lim = {0.5, 0.3, 0.2, 0.1}; +const std::vector steer_rate_lim = {0.5, 0.3, 0.2, 0.1}; const std::vector lon_acc_lim = {1.5, 1.0, 0.8, 0.6}; const std::vector lon_jerk_lim = {1.4, 0.9, 0.7, 0.5}; const std::vector lat_acc_lim = {2.0, 1.6, 1.2, 0.8}; @@ -336,6 +338,8 @@ std::shared_ptr generateNode() node_options.append_parameter_override("wheel_base", wheelbase); override("nominal.reference_speed_points", reference_speed_points); override("nominal.reference_speed_points", reference_speed_points); + override("nominal.steer_lim", steer_lim); + override("nominal.steer_rate_lim", steer_rate_lim); override("nominal.lon_acc_lim", lon_acc_lim); override("nominal.lon_jerk_lim", lon_jerk_lim); override("nominal.lat_acc_lim", lat_acc_lim); diff --git a/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp b/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp index 1791493aeb17b..b8200490dd1d5 100644 --- a/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp +++ b/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp @@ -31,12 +31,15 @@ constexpr double NOMINAL_INTERVAL = 1.0; void setFilterParams( vehicle_cmd_gate::VehicleCmdFilter & f, double v, LimitArray speed_points, LimitArray a, - LimitArray j, LimitArray lat_a, LimitArray lat_j, LimitArray steer_diff, const double wheelbase) + LimitArray j, LimitArray lat_a, LimitArray lat_j, LimitArray steer_diff, LimitArray steer_lim, + LimitArray steer_rate_lim, const double wheelbase) { vehicle_cmd_gate::VehicleCmdFilterParam p; p.vel_lim = v; p.wheel_base = wheelbase; p.reference_speed_points = speed_points; + p.steer_lim = steer_lim; + p.steer_rate_lim = steer_rate_lim; p.lat_acc_lim = lat_a; p.lat_jerk_lim = lat_j; p.lon_acc_lim = a; @@ -97,8 +100,8 @@ double calcLatJerk( void test_1d_limit( double ego_v, double V_LIM, double A_LIM, double J_LIM, double LAT_A_LIM, double LAT_J_LIM, - double STEER_DIFF, const AckermannControlCommand & prev_cmd, - const AckermannControlCommand & raw_cmd) + double STEER_DIFF, double STEER_LIM, double STEER_RATE_LIM, + const AckermannControlCommand & prev_cmd, const AckermannControlCommand & raw_cmd) { const double WHEELBASE = 3.0; const double DT = 0.1; // [s] @@ -106,7 +109,8 @@ void test_1d_limit( vehicle_cmd_gate::VehicleCmdFilter filter; filter.setCurrentSpeed(ego_v); setFilterParams( - filter, V_LIM, {0.0}, {A_LIM}, {J_LIM}, {LAT_A_LIM}, {LAT_J_LIM}, {STEER_DIFF}, WHEELBASE); + filter, V_LIM, {0.0}, {A_LIM}, {J_LIM}, {LAT_A_LIM}, {LAT_J_LIM}, {STEER_DIFF}, {STEER_LIM}, + {STEER_RATE_LIM}, WHEELBASE); filter.setPrevCmd(prev_cmd); // velocity filter @@ -233,6 +237,8 @@ TEST(VehicleCmdFilter, VehicleCmdFilter) const std::vector lat_a_arr = {0.01, 1.0, 100.0}; const std::vector lat_j_arr = {0.01, 1.0, 100.0}; const std::vector steer_diff_arr = {0.01, 1.0, 100.0}; + const std::vector steer_lim_arr = {0.01, 1.0, 100.0}; + const std::vector steer_rate_lim_arr = {0.01, 1.0, 100.0}; const std::vector ego_v_arr = {0.0, 0.1, 1.0, 3.0, 15.0}; const std::vector prev_cmd_arr = { @@ -249,8 +255,13 @@ TEST(VehicleCmdFilter, VehicleCmdFilter) for (const auto & prev_cmd : prev_cmd_arr) { for (const auto & raw_cmd : raw_cmd_arr) { for (const auto & steer_diff : steer_diff_arr) { - for (const auto & ego_v : ego_v_arr) { - test_1d_limit(ego_v, v, a, j, la, lj, steer_diff, prev_cmd, raw_cmd); + for (const auto & steer : steer_lim_arr) { + for (const auto & steer_rate : steer_rate_lim_arr) { + for (const auto & ego_v : ego_v_arr) { + test_1d_limit( + ego_v, v, a, j, la, lj, steer_diff, steer, steer_rate, prev_cmd, raw_cmd); + } + } } } } @@ -271,6 +282,8 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) p.wheel_base = WHEELBASE; p.vel_lim = 20.0; p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; + p.steer_lim = std::vector{0.1, 0.2, 0.3}; + p.steer_rate_lim = std::vector{0.2, 0.1, 0.05}; p.lon_acc_lim = std::vector{0.3, 0.4, 0.5}; p.lon_jerk_lim = std::vector{0.4, 0.4, 0.7}; p.lat_acc_lim = std::vector{0.1, 0.2, 0.3}; @@ -293,7 +306,16 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) const auto set_speed_and_reset_prev = [&](const auto & current_vel) { filter.setCurrentSpeed(current_vel); }; - + const auto _limitSteer = [&](const auto & in) { + auto out = in; + filter.limitLateralSteer(out); + return out; + }; + const auto _limitSteerRate = [&](const auto & in) { + auto out = in; + filter.limitLateralSteerRate(DT, out); + return out; + }; const auto _limitLongitudinalWithVel = [&](const auto & in) { auto out = in; filter.limitLongitudinalWithVel(out); @@ -334,6 +356,77 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) EXPECT_NEAR(_limitLongitudinalWithVel(orig_cmd).longitudinal.speed, 20.0, ep); } + // steer angle lim + // p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; + // p.steer_lim = std::vector{0.1, 0.2, 0.3}; + { + set_speed_and_reset_prev(0.0); + EXPECT_NEAR(_limitSteer(orig_cmd).lateral.steering_tire_angle, 0.1, ep); + + set_speed_and_reset_prev(2.0); + EXPECT_NEAR(_limitSteer(orig_cmd).lateral.steering_tire_angle, 0.1, ep); + + set_speed_and_reset_prev(3.0); + EXPECT_NEAR(_limitSteer(orig_cmd).lateral.steering_tire_angle, 0.15, ep); + + set_speed_and_reset_prev(5.0); + EXPECT_NEAR(_limitSteer(orig_cmd).lateral.steering_tire_angle, 0.2 + 0.1 / 6.0, ep); + + set_speed_and_reset_prev(8.0); + EXPECT_NEAR(_limitSteer(orig_cmd).lateral.steering_tire_angle, 0.2 + 0.1 * 4.0 / 6.0, ep); + + set_speed_and_reset_prev(10.0); + EXPECT_NEAR(_limitSteer(orig_cmd).lateral.steering_tire_angle, 0.3, ep); + + set_speed_and_reset_prev(15.0); + EXPECT_NEAR(_limitSteer(orig_cmd).lateral.steering_tire_angle, 0.3, ep); + } + + // steer angle rate lim + // p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; + // p.steer_rate_lim = std::vector{0.2, 0.1, 0.05}; + { + const auto calcSteerRateFromAngle = [&](const auto & cmd) { + return (cmd.steering_tire_angle - 0.0) / DT; + }; + autoware_auto_control_msgs::msg::AckermannLateralCommand filtered; + + set_speed_and_reset_prev(0.0); + filtered = _limitSteerRate(orig_cmd).lateral; + EXPECT_NEAR(calcSteerRateFromAngle(filtered), 0.2, ep); + EXPECT_NEAR(filtered.steering_tire_rotation_rate, 0.2, ep); + + set_speed_and_reset_prev(2.0); + filtered = _limitSteerRate(orig_cmd).lateral; + EXPECT_NEAR(calcSteerRateFromAngle(filtered), 0.2, ep); + EXPECT_NEAR(filtered.steering_tire_rotation_rate, 0.2, ep); + + set_speed_and_reset_prev(3.0); + filtered = _limitSteerRate(orig_cmd).lateral; + EXPECT_NEAR(calcSteerRateFromAngle(filtered), 0.15, ep); + EXPECT_NEAR(filtered.steering_tire_rotation_rate, 0.15, ep); + + set_speed_and_reset_prev(5.0); + filtered = _limitSteerRate(orig_cmd).lateral; + EXPECT_NEAR(calcSteerRateFromAngle(filtered), 0.1 - 0.05 * 1.0 / 6.0, ep); + EXPECT_NEAR(filtered.steering_tire_rotation_rate, 0.1 - 0.05 * 1.0 / 6.0, ep); + + set_speed_and_reset_prev(8.0); + filtered = _limitSteerRate(orig_cmd).lateral; + EXPECT_NEAR(calcSteerRateFromAngle(filtered), 0.1 - 0.05 * 4.0 / 6.0, ep); + EXPECT_NEAR(filtered.steering_tire_rotation_rate, 0.1 - 0.05 * 4.0 / 6.0, ep); + + set_speed_and_reset_prev(10.0); + filtered = _limitSteerRate(orig_cmd).lateral; + EXPECT_NEAR(calcSteerRateFromAngle(filtered), 0.05, ep); + EXPECT_NEAR(filtered.steering_tire_rotation_rate, 0.05, ep); + + set_speed_and_reset_prev(15.0); + filtered = _limitSteerRate(orig_cmd).lateral; + EXPECT_NEAR(calcSteerRateFromAngle(filtered), 0.05, ep); + EXPECT_NEAR(filtered.steering_tire_rotation_rate, 0.05, ep); + } + // longitudinal acc lim { set_speed_and_reset_prev(0.0);