diff --git a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp index d653d25bf911b..178385f2ef9ef 100644 --- a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp @@ -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 @@ -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) diff --git a/control/pid_longitudinal_controller/test/test_pid.cpp b/control/pid_longitudinal_controller/test/test_pid.cpp index 82d01e0028a9c..f0ceccd8499a3 100644 --- a/control/pid_longitudinal_controller/test/test_pid.cpp +++ b/control/pid_longitudinal_controller/test/test_pid.cpp @@ -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); @@ -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); diff --git a/control/pid_longitudinal_controller/test/test_smooth_stop.cpp b/control/pid_longitudinal_controller/test/test_smooth_stop.cpp index d6b8b8b731e01..d658a5271d249 100644 --- a/control/pid_longitudinal_controller/test/test_smooth_stop.cpp +++ b/control/pid_longitudinal_controller/test/test_smooth_stop.cpp @@ -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> 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> 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); + } }