Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(vehicle_cmd_gate)!: add steer and steer_rate filter #5044

Merged
merged 2 commits into from
Sep 21, 2023
Merged

feat(vehicle_cmd_gate)!: add steer and steer_rate filter #5044

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
c365f26
feat(vehicle_cmd_gate): add steering angle and rate filter
TakaHoribe Sep 20, 2023
d0788bc
update test
TakaHoribe Sep 20, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml
View file
Open in desktop
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
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,8 @@
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) {

Check warning on line 33 in control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Conditional 

VehicleCmdFilter::setParameterWithValidation increases from 1 complex conditionals with 4 branches to 1 complex conditionals with 6 branches, threshold = 2. A complex conditional is an expression inside a branch (e.g. if, for, while) which consists of multiple, logical operators such as AND/OR. The more logical operators in an expression, the more severe the code smell.
std::cerr << "VehicleCmdFilter::setParam() There is a size mismatch in the parameter. "
"Parameter initialization failed."
<< std::endl;
Expand All @@ -39,7 +40,18 @@
param_ = p;
return true;
}

void VehicleCmdFilter::setSteerLim(LimitArray v)

Check warning on line 43 in control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp

View check run for this annotation

Codecov / codecov/patch

control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp#L43 

Added line #L43 was not covered by tests
{
auto tmp = param_;

Check warning on line 45 in control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp

View check run for this annotation

Codecov / codecov/patch

control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp#L45 

Added line #L45 was not covered by tests
tmp.steer_lim = v;
setParameterWithValidation(tmp);
}
void VehicleCmdFilter::setSteerRateLim(LimitArray v)

Check warning on line 49 in control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp

View check run for this annotation

Codecov / codecov/patch

control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp#L48-L49 

Added lines #L48 - L49 were not covered by tests
{
auto tmp = param_;

Check warning on line 51 in control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp

View check run for this annotation

Codecov / codecov/patch

control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp#L51 

Added line #L51 was not covered by tests
tmp.steer_rate_lim = v;
setParameterWithValidation(tmp);
}

Check warning on line 54 in control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp

View check run for this annotation

Codecov / codecov/patch

control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp#L54 

Added line #L54 was not covered by tests
void VehicleCmdFilter::setLonAccLim(LimitArray v)
{
auto tmp = param_;
Expand Down Expand Up @@ -151,12 +163,36 @@

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 @@
{
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 @@
{
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
Open in desktop
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
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -169,21 +169,25 @@
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");
p.lat_jerk_lim = declare_parameter<std::vector<double>>("nominal.lat_jerk_lim");
p.actual_steer_diff_lim =
declare_parameter<std::vector<double>>("nominal.actual_steer_diff_lim");
filter_.setParam(p);
}

{
VehicleCmdFilterParam p;
p.wheel_base = vehicle_info.wheel_base_m;
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");

Check warning on line 190 in control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Large Method 

VehicleCmdGate::VehicleCmdGate increases from 147 to 151 lines of code, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.
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
Open in desktop
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
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2021 Tier IV, Inc.

Check warning on line 1 in control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Missing Arguments Abstractions 

The average number of function arguments in this module is 4.44 across 9 functions. The average arguments threshold is 4.00. The functions in this file have too many arguments, indicating a lack of encapsulation or too many responsibilities in the same functions. Avoid adding more.

Check notice on line 1 in control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Primitive Obsession 

The ratio of primitive types in function arguments decreases from 58.33% to 57.50%, threshold = 30.0%. The functions in this file have too many primitive types (e.g. int, double, float) in their function argument lists. Using many primitive types lead to the code smell Primitive Obsession. Avoid adding more primitive arguments.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -31,12 +31,15 @@

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;

Check notice on line 42 in control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Excess Number of Function Arguments 

setFilterParams increases from 9 to 11 arguments, threshold = 4. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.
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 @@

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);

Check warning on line 113 in control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method 

test_1d_limit already has high cyclomatic complexity, and now it increases in Lines of Code from 89 to 90. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

Check notice on line 113 in control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Excess Number of Function Arguments 

test_1d_limit increases from 9 to 11 arguments, threshold = 4. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.
filter.setPrevCmd(prev_cmd);

// velocity filter
Expand Down Expand Up @@ -233,24 +237,31 @@
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 = {
genCmd(0.0, 0.0, 0.0, 0.0), genCmd(1.0, 1.0, 1.0, 1.0)};

const std::vector<AckermannControlCommand> raw_cmd_arr = {
genCmd(1.0, 1.0, 1.0, 1.0), genCmd(10.0, -1.0, -1.0, -1.0)};

for (const auto & v : v_arr) {
for (const auto & a : a_arr) {
for (const auto & j : j_arr) {
for (const auto & la : lat_a_arr) {
for (const auto & lj : lat_j_arr) {
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);
}
}

Check warning on line 264 in control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method 

TEST:VehicleCmdFilter:VehicleCmdFilter increases in cyclomatic complexity from 10 to 12, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

Check warning on line 264 in control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Deep, Nested Complexity 

TEST:VehicleCmdFilter:VehicleCmdFilter increases in nested complexity depth from 9 to 11, threshold = 4. This function contains deeply nested logic such as if statements and/or loops. The deeper the nesting, the lower the code health.
}
}
}
Expand All @@ -271,69 +282,151 @@
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};
p.lat_jerk_lim = std::vector<double>{0.9, 0.7, 0.1};
p.actual_steer_diff_lim = std::vector<double>{0.1, 0.3, 0.2};
filter.setParam(p);

const auto DT = 0.033;

const auto orig_cmd = []() {
AckermannControlCommand cmd;
cmd.lateral.steering_tire_angle = 0.5;
cmd.lateral.steering_tire_rotation_rate = 0.5;
cmd.longitudinal.speed = 30.0;
cmd.longitudinal.acceleration = 10.0;
cmd.longitudinal.jerk = 10.0;
return cmd;
}();

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);
return out;
};
const auto _limitLongitudinalWithAcc = [&](const auto & in) {
auto out = in;
filter.limitLongitudinalWithAcc(DT, out);
return out;
};
const auto _limitLongitudinalWithJerk = [&](const auto & in) {
auto out = in;
filter.limitLongitudinalWithJerk(DT, out);
return out;
};
const auto _limitLateralWithLatAcc = [&](const auto & in) {
auto out = in;
filter.limitLateralWithLatAcc(DT, out);
return out;
};
const auto _limitLateralWithLatJerk = [&](const auto & in) {
auto out = in;
filter.limitLateralWithLatJerk(DT, out);
return out;
};
const auto _limitActualSteerDiff = [&](const auto & in) {
auto out = in;
const auto prev_steering = 0.0;
filter.limitActualSteerDiff(prev_steering, out);
return out;
};

constexpr double ep = 1.0e-5;

// vel lim
{
set_speed_and_reset_prev(0.0);
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);
}

Check warning on line 429 in control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Large Method 

TEST:VehicleCmdFilter:VehicleCmdFilterInterpolate increases from 170 to 232 lines of code, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.
// longitudinal acc lim
{
set_speed_and_reset_prev(0.0);
Expand Down
Loading