Skip to content

Commit

Permalink
feat(vehicle_cmd_gate)!: add steer and steer_rate filter (#5044)
Browse files Browse the repository at this point in the history 
* feat(vehicle_cmd_gate): add steering angle and rate filter

Signed-off-by: Takamasa Horibe <[email protected]>

* update test

Signed-off-by: Takamasa Horibe <[email protected]>

---------

Signed-off-by: Takamasa Horibe <[email protected]>
  • Loading branch information
@TakaHoribe
TakaHoribe authored Sep 21, 2023
1 parent 08c809c commit a84418a
Show file tree
Hide file tree
Showing 6 changed files with 170 additions and 13 deletions.
4 changes: 4 additions & 0 deletions control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand All @@ -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]
Expand Down
57 changes: 51 additions & 6 deletions control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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_;
Expand Down Expand Up @@ -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(
Expand All @@ -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);
Expand Down Expand Up @@ -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);
Expand Down
7 changes: 7 additions & 0 deletions control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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);
Expand All @@ -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;
Expand All @@ -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
Expand Down
4 changes: 4 additions & 0 deletions control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -169,6 +169,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
p.vel_lim = declare_parameter<double>("nominal.vel_lim");
p.reference_speed_points =
declare_parameter<std::vector<double>>("nominal.reference_speed_points");
p.steer_lim = declare_parameter<std::vector<double>>("nominal.steer_lim");
p.steer_rate_lim = declare_parameter<std::vector<double>>("nominal.steer_rate_lim");
p.lon_acc_lim = declare_parameter<std::vector<double>>("nominal.lon_acc_lim");
p.lon_jerk_lim = declare_parameter<std::vector<double>>("nominal.lon_jerk_lim");
p.lat_acc_lim = declare_parameter<std::vector<double>>("nominal.lat_acc_lim");
Expand All @@ -184,6 +186,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
p.vel_lim = declare_parameter<double>("on_transition.vel_lim");
p.reference_speed_points =
declare_parameter<std::vector<double>>("on_transition.reference_speed_points");
p.steer_lim = declare_parameter<std::vector<double>>("on_transition.steer_lim");
p.steer_rate_lim = declare_parameter<std::vector<double>>("on_transition.steer_rate_lim");
p.lon_acc_lim = declare_parameter<std::vector<double>>("on_transition.lon_acc_lim");
p.lon_jerk_lim = declare_parameter<std::vector<double>>("on_transition.lon_jerk_lim");
p.lat_acc_lim = declare_parameter<std::vector<double>>("on_transition.lat_acc_lim");
Expand Down
4 changes: 4 additions & 0 deletions control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,8 @@ void print_values(int i, const T1 & name, const T2 & a, const T3 &... b)

// global params
const std::vector<double> reference_speed_points = {5., 10., 15., 20.};
const std::vector<double> steer_lim = {0.5, 0.3, 0.2, 0.1};
const std::vector<double> steer_rate_lim = {0.5, 0.3, 0.2, 0.1};
const std::vector<double> lon_acc_lim = {1.5, 1.0, 0.8, 0.6};
const std::vector<double> lon_jerk_lim = {1.4, 0.9, 0.7, 0.5};
const std::vector<double> lat_acc_lim = {2.0, 1.6, 1.2, 0.8};
Expand Down Expand Up @@ -336,6 +338,8 @@ std::shared_ptr<VehicleCmdGate> 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);
Expand Down
107 changes: 100 additions & 7 deletions control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -97,16 +100,17 @@ 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]

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
Expand Down Expand Up @@ -233,6 +237,8 @@ TEST(VehicleCmdFilter, VehicleCmdFilter)
const std::vector<double> lat_a_arr = {0.01, 1.0, 100.0};
const std::vector<double> lat_j_arr = {0.01, 1.0, 100.0};
const std::vector<double> steer_diff_arr = {0.01, 1.0, 100.0};
const std::vector<double> steer_lim_arr = {0.01, 1.0, 100.0};
const std::vector<double> steer_rate_lim_arr = {0.01, 1.0, 100.0};
const std::vector<double> ego_v_arr = {0.0, 0.1, 1.0, 3.0, 15.0};

const std::vector<AckermannControlCommand> prev_cmd_arr = {
Expand All @@ -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);
}
}
}
}
}
Expand All @@ -271,6 +282,8 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate)
p.wheel_base = WHEELBASE;
p.vel_lim = 20.0;
p.reference_speed_points = std::vector<double>{2.0, 4.0, 10.0};
p.steer_lim = std::vector<double>{0.1, 0.2, 0.3};
p.steer_rate_lim = std::vector<double>{0.2, 0.1, 0.05};
p.lon_acc_lim = std::vector<double>{0.3, 0.4, 0.5};
p.lon_jerk_lim = std::vector<double>{0.4, 0.4, 0.7};
p.lat_acc_lim = std::vector<double>{0.1, 0.2, 0.3};
Expand All @@ -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);
Expand Down Expand Up @@ -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<double>{2.0, 4.0, 10.0};
// p.steer_lim = std::vector<double>{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<double>{2.0, 4.0, 10.0};
// p.steer_rate_lim = std::vector<double>{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);
Expand Down

0 comments on commit a84418a

Please sign in to comment.