Skip to content

Commit

Permalink
test update
Browse files Browse the repository at this point in the history
  • Loading branch information
@brkay54
brkay54 committed Sep 18, 2023
1 parent 73ebca7 commit 8fd87f5
Show file tree
Hide file tree
Showing 3 changed files with 108 additions and 108 deletions.
22 changes: 10 additions & 12 deletions control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -115,7 +115,7 @@ TEST(TestLongitudinalControllerUtils, getPitchByTraj)
{
using autoware_auto_planning_msgs::msg::Trajectory;
using autoware_auto_planning_msgs::msg::TrajectoryPoint;
const double wheel_base = 0.9;
const double wheel_base = 1.0;
/**
* Trajectory:
* 1 X
Expand All @@ -130,32 +130,30 @@ TEST(TestLongitudinalControllerUtils, getPitchByTraj)
point.pose.position.z = 0.0;
traj.points.push_back(point);
// non stopping trajectory: stop distance = trajectory length
point.pose.position.x = 1.0;
point.pose.position.x = 0.6;
point.pose.position.y = 0.0;
point.pose.position.z = 1.0;
point.pose.position.z = 0.8;
traj.points.push_back(point);
point.pose.position.x = 2.0;
point.pose.position.x = 1.2;
point.pose.position.y = 0.0;
point.pose.position.z = 0.0;
traj.points.push_back(point);
point.pose.position.x = 3.0;
point.pose.position.x = 1.8;
point.pose.position.y = 0.0;
point.pose.position.z = 0.5;
point.pose.position.z = 0.8;
traj.points.push_back(point);
size_t closest_idx = 0;
EXPECT_DOUBLE_EQ(
std::abs(longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base)), M_PI_4);
longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base), std::atan2(-0.8, 0.6));
closest_idx = 1;
EXPECT_DOUBLE_EQ(
std::abs(longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base)), M_PI_4);
longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base), std::atan2(0.8, 0.6));
closest_idx = 2;
EXPECT_DOUBLE_EQ(
std::abs(longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base)),
std::atan2(0.5, 1));
longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base), std::atan2(-0.8, 0.6));
closest_idx = 3;
EXPECT_DOUBLE_EQ(
std::abs(longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base)),
std::atan2(0.5, 1));
longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base), std::atan2(-0.8, 0.6));
}

TEST(TestLongitudinalControllerUtils, calcElevationAngle)
Expand Down
12 changes: 6 additions & 6 deletions control/pid_longitudinal_controller/test/test_pid.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,14 +29,14 @@ TEST(TestPID, calculate_pid_output)
PIDController pid;

// Cannot calculate before initializing gains and limits
EXPECT_THROW(pid.calculate(0.0, dt, enable_integration, contributions), std::runtime_error);
EXPECT_THROW(pid.calculate(0.0, 0.0, dt, enable_integration, contributions), std::runtime_error);

pid.setGains(1.0, 1.0, 1.0);
pid.setLimits(10.0, 0.0, 10.0, 0.0, 10.0, 0.0, 10.0, 0.0);
double error = target - current;
double prev_error = error;
while (current != target) {
current = pid.calculate(error, dt, enable_integration, contributions);
current = pid.calculate(error, error, dt, enable_integration, contributions);
EXPECT_EQ(contributions[0], error);
EXPECT_EQ(contributions[1], 0.0); // integration is deactivated
EXPECT_EQ(contributions[2], error - prev_error);
Expand All @@ -50,18 +50,18 @@ TEST(TestPID, calculate_pid_output)
enable_integration = true;

// High errors to force each component to its upper limit
EXPECT_EQ(pid.calculate(0.0, dt, enable_integration, contributions), 0.0);
EXPECT_EQ(pid.calculate(0.0, 0.0, dt, enable_integration, contributions), 0.0);
for (double error = 100.0; error < 1000.0; error += 100.0) {
EXPECT_EQ(pid.calculate(error, dt, enable_integration, contributions), 10.0);
EXPECT_EQ(pid.calculate(error, error, dt, enable_integration, contributions), 10.0);
EXPECT_EQ(contributions[0], 10.0);
EXPECT_EQ(contributions[1], 10.0); // integration is activated
EXPECT_EQ(contributions[2], 10.0);
}

// Low errors to force each component to its lower limit
EXPECT_EQ(pid.calculate(0.0, dt, enable_integration, contributions), 0.0);
EXPECT_EQ(pid.calculate(0.0, 0.0, dt, enable_integration, contributions), 0.0);
for (double error = -100.0; error > -1000.0; error -= 100.0) {
EXPECT_EQ(pid.calculate(error, dt, enable_integration, contributions), -10.0);
EXPECT_EQ(pid.calculate(error, error, dt, enable_integration, contributions), -10.0);
EXPECT_EQ(contributions[0], -10.0);
EXPECT_EQ(contributions[1], -10.0); // integration is activated
EXPECT_EQ(contributions[2], -10.0);
Expand Down
182 changes: 92 additions & 90 deletions control/pid_longitudinal_controller/test/test_smooth_stop.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,94 +21,96 @@

TEST(TestSmoothStop, calculate_stopping_acceleration)
{
// using ::autoware::motion::control::pid_longitudinal_controller::SmoothStop;
// using rclcpp::Duration;
// using rclcpp::Time;
//
// const double max_strong_acc = -0.5;
// const double min_strong_acc = -1.0;
// const double weak_acc = -0.3;
// const double weak_stop_acc = -0.8;
// const double strong_stop_acc = -3.4;
// const double max_fast_vel = 0.5;
// const double min_running_vel = 0.01;
// const double min_running_acc = 0.01;
// const double weak_stop_time = 0.8;
// const double weak_stop_dist = -0.3;
// const double strong_stop_dist = -0.5;
//
// const double delay_time = 0.17;
//
// SmoothStop ss;
//
// // Cannot calculate before setting parameters
// EXPECT_THROW(ss.calculate(0.0, 0.0, 0.0, {}, delay_time), std::runtime_error);
//
// ss.setParams(
// max_strong_acc, min_strong_acc, weak_acc, weak_stop_acc, strong_stop_acc, max_fast_vel,
// min_running_vel, min_running_acc, weak_stop_time, weak_stop_dist, strong_stop_dist);
//
// double vel_in_target;
// double stop_dist;
// double current_vel;
// double current_acc = 0.0;
// const Time now = rclcpp::Clock{RCL_ROS_TIME}.now();
// const std::vector<std::pair<Time, double>> velocity_history = {
// {now - Duration(3, 0), 3.0}, {now - Duration(2, 0), 2.0}, {now - Duration(1, 0), 1.0}};
// double accel;
//
// // strong stop when the stop distance is below the threshold
// vel_in_target = 5.0;
// stop_dist = strong_stop_dist - 0.1;
// current_vel = 2.0;
// ss.init(vel_in_target, stop_dist);
// accel = ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time);
// EXPECT_EQ(accel, strong_stop_acc);
//
// // weak stop when the stop distance is below the threshold (but not bellow the
// strong_stop_dist) stop_dist = weak_stop_dist - 0.1; current_vel = 2.0; ss.init(vel_in_target,
// stop_dist); accel = ss.calculate(stop_dist, current_vel, current_acc, velocity_history,
// delay_time); EXPECT_EQ(accel, weak_stop_acc);
//
// // if not running, weak accel for 0.5 seconds after the previous init or previous weak_acc
// rclcpp::Rate rate_quart(1.0 / 0.25);
// rclcpp::Rate rate_half(1.0 / 0.5);
// stop_dist = 0.0;
// current_vel = 0.0;
// EXPECT_EQ(
// ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), weak_acc);
// rate_quart.sleep();
// EXPECT_EQ(
// ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), weak_acc);
// rate_half.sleep();
// EXPECT_NE(
// ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), weak_acc);
//
// // strong stop when the car is not running (and is at least 0.5seconds after initialization)
// EXPECT_EQ(
// ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time),
// strong_stop_acc);
//
// // accel between min/max_strong_acc when the car is running:
// // not predicted to exceed the stop line and is predicted to stop after weak_stop_time + delay
// stop_dist = 1.0;
// current_vel = 1.0;
// vel_in_target = 1.0;
// ss.init(vel_in_target, stop_dist);
// EXPECT_EQ(
// ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time),
// max_strong_acc);
//
// vel_in_target = std::sqrt(2.0);
// ss.init(vel_in_target, stop_dist);
// EXPECT_EQ(
// ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time),
// min_strong_acc);
//
// for (double vel_in_target = 1.1; vel_in_target < std::sqrt(2.0); vel_in_target += 0.1) {
// ss.init(vel_in_target, stop_dist);
// accel = ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time);
// EXPECT_GT(accel, min_strong_acc);
// EXPECT_LT(accel, max_strong_acc);
// }
using ::autoware::motion::control::pid_longitudinal_controller::SmoothStop;
using rclcpp::Duration;
using rclcpp::Time;

const double max_strong_acc = -0.5;
const double min_strong_acc = -1.0;
const double weak_acc = -0.3;
const double weak_stop_acc = -0.8;
const double strong_stop_acc = -3.4;
const double max_fast_vel = 0.5;
const double min_running_vel = 0.01;
const double min_running_acc = 0.01;
const double weak_stop_time = 0.8;
const double weak_stop_dist = -0.3;
const double strong_stop_dist = -0.5;

const double delay_time = 0.17;

SmoothStop ss;

// Cannot calculate before setting parameters
EXPECT_THROW(ss.calculate(0.0, 0.0, 0.0, {}, delay_time), std::runtime_error);

ss.setParams(
max_strong_acc, min_strong_acc, weak_acc, weak_stop_acc, strong_stop_acc, max_fast_vel,
min_running_vel, min_running_acc, weak_stop_time, weak_stop_dist, strong_stop_dist);

double vel_in_target;
double stop_dist;
double current_vel;
double current_acc = 0.0;
const Time now = rclcpp::Clock{RCL_ROS_TIME}.now();
const std::vector<std::pair<Time, double>> velocity_history = {
{now - Duration(3, 0), 3.0}, {now - Duration(2, 0), 2.0}, {now - Duration(1, 0), 1.0}};
double accel;

// strong stop when the stop distance is below the threshold
vel_in_target = 5.0;
stop_dist = strong_stop_dist - 0.1;
current_vel = 2.0;
ss.init(vel_in_target, stop_dist);
accel = ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time);
EXPECT_EQ(accel, strong_stop_acc);

// weak stop when the stop distance is below the threshold (but not bellow the strong_stop_dist)
stop_dist = weak_stop_dist - 0.1;
current_vel = 2.0;
ss.init(vel_in_target, stop_dist);
accel = ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time);
EXPECT_EQ(accel, weak_stop_acc);

// if not running, weak accel for 0.5 seconds after the previous init or previous weak_acc
rclcpp::Rate rate_quart(1.0 / 0.25);
rclcpp::Rate rate_half(1.0 / 0.5);
stop_dist = 0.0;
current_vel = 0.0;
EXPECT_EQ(
ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), weak_acc);
rate_quart.sleep();
EXPECT_EQ(
ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), weak_acc);
rate_half.sleep();
EXPECT_NE(
ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), weak_acc);

// strong stop when the car is not running (and is at least 0.5seconds after initialization)
EXPECT_EQ(
ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time),
strong_stop_acc);

// accel between min/max_strong_acc when the car is running:
// not predicted to exceed the stop line and is predicted to stop after weak_stop_time + delay
stop_dist = 1.0;
current_vel = 1.0;
vel_in_target = 1.0;
ss.init(vel_in_target, stop_dist);
EXPECT_EQ(
ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time),
max_strong_acc);

vel_in_target = std::sqrt(2.0);
ss.init(vel_in_target, stop_dist);
EXPECT_EQ(
ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time),
min_strong_acc);

for (double vel_in_target = 1.1; vel_in_target < std::sqrt(2.0); vel_in_target += 0.1) {
ss.init(vel_in_target, stop_dist);
accel = ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time);
EXPECT_GT(accel, min_strong_acc);
EXPECT_LT(accel, max_strong_acc);
}
}

0 comments on commit 8fd87f5

Please sign in to comment.