Skip to content

Commit

Permalink
test
Browse files Browse the repository at this point in the history
  • Loading branch information
@brkay54
brkay54 committed Sep 18, 2023
1 parent 5fd7f12 commit 170e385
Showing 1 changed file with 117 additions and 229 deletions.
346 changes: 117 additions & 229 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 @@ -183,136 +183,6 @@ TEST(TestLongitudinalControllerUtils, calcElevationAngle)
EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), M_PI_4);
}

// TEST(TestLongitudinalControllerUtils, calcDistanceAfterTimeDelay)
//{
// using geometry_msgs::msg::Pose;
// const double abs_err = 1e-7;
// Pose current_pose;
// current_pose.position.x = 0.0;
// current_pose.position.y = 0.0;
// current_pose.position.z = 0.0;
// tf2::Quaternion quaternion_tf;
// quaternion_tf.setRPY(0.0, 0.0, 0.0);
// current_pose.orientation = tf2::toMsg(quaternion_tf);
//
// // With a delay acceleration and/or a velocity of 0.0 there is no change of position
// double delay_time = 0.0;
// double current_vel = 0.0;
// double current_acc = 0.0;
// Pose delayed_pose = longitudinal_utils::calcDistanceAfterTimeDelay(
// current_pose, delay_time, current_vel, current_acc);
// EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err);
// EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
// EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
//
// delay_time = 1.0;
// current_vel = 0.0;
// current_acc = 0.0;
// delayed_pose = longitudinal_utils::calcDistanceAfterTimeDelay(
// current_pose, delay_time, current_vel, current_acc);
// EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err);
// EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
// EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
//
// delay_time = 0.0;
// current_vel = 1.0;
// current_acc = 0.0;
// delayed_pose = longitudinal_utils::calcDistanceAfterTimeDelay(
// current_pose, delay_time, current_vel, current_acc);
// EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err);
// EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
// EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
//
// // With both delay and velocity: change of position
// delay_time = 1.0;
// current_vel = 1.0;
// current_acc = 0.0;
//
// delayed_pose = longitudinal_utils::calcDistanceAfterTimeDelay(
// current_pose, delay_time, current_vel, current_acc);
// EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x + current_vel * delay_time,
// abs_err); EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
// EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
//
// // With all, acceleration, delay and velocity: change of position
// delay_time = 1.0;
// current_vel = 1.0;
// current_acc = 1.0;
//
// delayed_pose = longitudinal_utils::calcDistanceAfterTimeDelay(
// current_pose, delay_time, current_vel, current_acc);
// EXPECT_NEAR(
// delayed_pose.position.x,
// current_pose.position.x + current_vel * delay_time +
// 0.5 * current_acc * delay_time * delay_time,
// abs_err);
// EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
// EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
//
// // Vary the yaw
// quaternion_tf.setRPY(0.0, 0.0, M_PI);
// current_pose.orientation = tf2::toMsg(quaternion_tf);
// delayed_pose = longitudinal_utils::calcDistanceAfterTimeDelay(
// current_pose, delay_time, current_vel, current_acc);
// EXPECT_NEAR(
// delayed_pose.position.x,
// current_pose.position.x - current_vel * delay_time -
// 0.5 * current_acc * delay_time * delay_time,
// abs_err);
// EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
// EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
//
// quaternion_tf.setRPY(0.0, 0.0, M_PI_2);
// current_pose.orientation = tf2::toMsg(quaternion_tf);
// delayed_pose = longitudinal_utils::calcDistanceAfterTimeDelay(
// current_pose, delay_time, current_vel, current_acc);
// EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err);
// EXPECT_NEAR(
// delayed_pose.position.y,
// current_pose.position.y + current_vel * delay_time +
// 0.5 * current_acc * delay_time * delay_time,
// abs_err);
// EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
//
// quaternion_tf.setRPY(0.0, 0.0, -M_PI_2);
// current_pose.orientation = tf2::toMsg(quaternion_tf);
// delayed_pose = longitudinal_utils::calcDistanceAfterTimeDelay(
// current_pose, delay_time, current_vel, current_acc);
// EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err);
// EXPECT_NEAR(
// delayed_pose.position.y,
// current_pose.position.y - current_vel * delay_time -
// 0.5 * current_acc * delay_time * delay_time,
// abs_err);
// EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
//
// // Vary the pitch : no effect /!\ NOTE: bug with roll of +-PI/2 which rotates the yaw by PI
// quaternion_tf.setRPY(0.0, M_PI_4, 0.0);
// current_pose.orientation = tf2::toMsg(quaternion_tf);
// delayed_pose = longitudinal_utils::calcDistanceAfterTimeDelay(
// current_pose, delay_time, current_vel, current_acc);
// EXPECT_NEAR(
// delayed_pose.position.x,
// current_pose.position.x + current_vel * delay_time +
// 0.5 * current_acc * delay_time * delay_time,
// abs_err);
// EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
// EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
//
// // Vary the roll : no effect
// quaternion_tf.setRPY(M_PI_2, 0.0, 0.0);
// current_pose.orientation = tf2::toMsg(quaternion_tf);
// delayed_pose = longitudinal_utils::calcDistanceAfterTimeDelay(
// current_pose, delay_time, current_vel, current_acc);
// EXPECT_NEAR(
// delayed_pose.position.x,
// current_pose.position.x + current_vel * delay_time +
// 0.5 * current_acc * delay_time * delay_time,
// abs_err);
// EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
// EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
// }

TEST(TestLongitudinalControllerUtils, lerpOrientation)
{
geometry_msgs::msg::Quaternion result;
Expand Down Expand Up @@ -369,105 +239,123 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation)
EXPECT_DOUBLE_EQ(yaw, M_PI_4 / 2);
}

// TEST(TestLongitudinalControllerUtils, lerpTrajectoryPoint)
//{
// using autoware_auto_planning_msgs::msg::TrajectoryPoint;
// using geometry_msgs::msg::Pose;
// const double abs_err = 1e-15;
// decltype(autoware_auto_planning_msgs::msg::Trajectory::points) points;
// TrajectoryPoint p;
// p.pose.position.x = 0.0;
// p.pose.position.y = 0.0;
// p.longitudinal_velocity_mps = 10.0;
// p.acceleration_mps2 = 10.0;
// points.push_back(p);
// p.pose.position.x = 1.0;
// p.pose.position.y = 0.0;
// p.longitudinal_velocity_mps = 20.0;
// p.acceleration_mps2 = 20.0;
// points.push_back(p);
// p.pose.position.x = 1.0;
// p.pose.position.y = 1.0;
// p.longitudinal_velocity_mps = 30.0;
// p.acceleration_mps2 = 30.0;
// points.push_back(p);
// p.pose.position.x = 2.0;
// p.pose.position.y = 1.0;
// p.longitudinal_velocity_mps = 40.0;
// p.acceleration_mps2 = 40.0;
// points.push_back(p);
// TrajectoryPoint result;
// Pose pose;
// double max_dist = 3.0;
// double max_yaw = 0.7;
// // Points on the trajectory gives back the original trajectory points values
// pose.position.x = 0.0;
// pose.position.y = 0.0;
// result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
// EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err);
// EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err);
// EXPECT_NEAR(result.longitudinal_velocity_mps, 10.0, abs_err);
// EXPECT_NEAR(result.acceleration_mps2, 10.0, abs_err);
//
// pose.position.x = 1.0;
// pose.position.y = 0.0;
// result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
// EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err);
// EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err);
// EXPECT_NEAR(result.longitudinal_velocity_mps, 20.0, abs_err);
// EXPECT_NEAR(result.acceleration_mps2, 20.0, abs_err);
//
// pose.position.x = 1.0;
// pose.position.y = 1.0;
// result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
// EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err);
// EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err);
// EXPECT_NEAR(result.longitudinal_velocity_mps, 30.0, abs_err);
// EXPECT_NEAR(result.acceleration_mps2, 30.0, abs_err);
//
// pose.position.x = 2.0;
// pose.position.y = 1.0;
// result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
// EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err);
// EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err);
// EXPECT_NEAR(result.longitudinal_velocity_mps, 40.0, abs_err);
// EXPECT_NEAR(result.acceleration_mps2, 40.0, abs_err);
//
// // Interpolate between trajectory points
// pose.position.x = 0.5;
// pose.position.y = 0.0;
// result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
// EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err);
// EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err);
// EXPECT_NEAR(result.longitudinal_velocity_mps, 15.0, abs_err);
// EXPECT_NEAR(result.acceleration_mps2, 15.0, abs_err);
// pose.position.x = 0.75;
// pose.position.y = 0.0;
// result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
//
// EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err);
// EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err);
// EXPECT_NEAR(result.longitudinal_velocity_mps, 17.5, abs_err);
// EXPECT_NEAR(result.acceleration_mps2, 17.5, abs_err);
//
// // Interpolate away from the trajectory (interpolated point is projected)
// pose.position.x = 0.5;
// pose.position.y = -1.0;
// result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
// EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err);
// EXPECT_NEAR(result.pose.position.y, 0.0, abs_err);
// EXPECT_NEAR(result.longitudinal_velocity_mps, 15.0, abs_err);
// EXPECT_NEAR(result.acceleration_mps2, 15.0, abs_err);
//
// // Ambiguous projections: possibility with the lowest index is used
// pose.position.x = 0.5;
// pose.position.y = 0.5;
// result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
// EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err);
// EXPECT_NEAR(result.pose.position.y, 0.0, abs_err);
// EXPECT_NEAR(result.longitudinal_velocity_mps, 15.0, abs_err);
// EXPECT_NEAR(result.acceleration_mps2, 15.0, abs_err);
// }
TEST(TestLongitudinalControllerUtils, lerpTrajectoryPoint)
{
using autoware_auto_planning_msgs::msg::TrajectoryPoint;
using geometry_msgs::msg::Pose;
const double abs_err = 1e-15;
decltype(autoware_auto_planning_msgs::msg::Trajectory::points) points;
TrajectoryPoint p;
p.pose.position.x = 0.0;
p.pose.position.y = 0.0;
p.pose.position.z = 0.0;
p.longitudinal_velocity_mps = 10.0;
p.acceleration_mps2 = 10.0;
points.push_back(p);
p.pose.position.x = 1.0;
p.pose.position.y = 0.0;
p.pose.position.z = 0.0;
p.longitudinal_velocity_mps = 20.0;
p.acceleration_mps2 = 20.0;
points.push_back(p);
p.pose.position.x = 1.0;
p.pose.position.y = 1.0;
p.pose.position.z = 1.0;
p.longitudinal_velocity_mps = 30.0;
p.acceleration_mps2 = 30.0;
points.push_back(p);
p.pose.position.x = 2.0;
p.pose.position.y = 1.0;
p.pose.position.z = 2.0;
p.longitudinal_velocity_mps = 40.0;
p.acceleration_mps2 = 40.0;
points.push_back(p);
Pose pose;
double max_dist = 3.0;
double max_yaw = 0.7;
// Points on the trajectory gives back the original trajectory points values
pose.position.x = 0.0;
pose.position.y = 0.0;
pose.position.z = 0.0;

auto result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err);
EXPECT_NEAR(result.first.pose.position.y, pose.position.y, abs_err);
EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err);
EXPECT_NEAR(result.first.longitudinal_velocity_mps, 10.0, abs_err);
EXPECT_NEAR(result.first.acceleration_mps2, 10.0, abs_err);

pose.position.x = 1.0;
pose.position.y = 0.0;
pose.position.z = 0.0;
result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err);
EXPECT_NEAR(result.first.pose.position.y, pose.position.y, abs_err);
EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err);
EXPECT_NEAR(result.first.longitudinal_velocity_mps, 20.0, abs_err);
EXPECT_NEAR(result.first.acceleration_mps2, 20.0, abs_err);

pose.position.x = 1.0;
pose.position.y = 1.0;
pose.position.z = 1.0;
result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err);
EXPECT_NEAR(result.first.pose.position.y, pose.position.y, abs_err);
EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err);
EXPECT_NEAR(result.first.longitudinal_velocity_mps, 30.0, abs_err);
EXPECT_NEAR(result.first.acceleration_mps2, 30.0, abs_err);

pose.position.x = 2.0;
pose.position.y = 1.0;
pose.position.z = 2.0;
result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err);
EXPECT_NEAR(result.first.pose.position.y, pose.position.y, abs_err);
EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err);
EXPECT_NEAR(result.first.longitudinal_velocity_mps, 40.0, abs_err);
EXPECT_NEAR(result.first.acceleration_mps2, 40.0, abs_err);

// Interpolate between trajectory points
pose.position.x = 0.5;
pose.position.y = 0.0;
pose.position.z = 0.0;
result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err);
EXPECT_NEAR(result.first.pose.position.y, pose.position.y, abs_err);
EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err);
EXPECT_NEAR(result.first.longitudinal_velocity_mps, 15.0, abs_err);
EXPECT_NEAR(result.first.acceleration_mps2, 15.0, abs_err);
pose.position.x = 0.75;
pose.position.y = 0.0;
pose.position.z = 0.0;
result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);

EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err);
EXPECT_NEAR(result.first.pose.position.y, pose.position.y, abs_err);
EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err);
EXPECT_NEAR(result.first.longitudinal_velocity_mps, 17.5, abs_err);
EXPECT_NEAR(result.first.acceleration_mps2, 17.5, abs_err);

// Interpolate away from the trajectory (interpolated point is projected)
pose.position.x = 0.5;
pose.position.y = -1.0;
result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err);
EXPECT_NEAR(result.first.pose.position.y, 0.0, abs_err);
EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err);
EXPECT_NEAR(result.first.longitudinal_velocity_mps, 15.0, abs_err);
EXPECT_NEAR(result.first.acceleration_mps2, 15.0, abs_err);

// Ambiguous projections: possibility with the lowest index is used
pose.position.x = 0.5;
pose.position.y = 0.5;
result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err);
EXPECT_NEAR(result.first.pose.position.y, 0.0, abs_err);
EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err);
EXPECT_NEAR(result.first.longitudinal_velocity_mps, 15.0, abs_err);
EXPECT_NEAR(result.first.acceleration_mps2, 15.0, abs_err);
}

TEST(TestLongitudinalControllerUtils, applyDiffLimitFilter)
{
Expand Down

0 comments on commit 170e385

Please sign in to comment.