Skip to content

Commit

Permalink
style(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
@pre-commit-ci
pre-commit-ci[bot] committed Sep 22, 2023
1 parent 7ab7b3b commit ef3b082
Showing 1 changed file with 116 additions and 116 deletions.
232 changes: 116 additions & 116 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 @@ -239,123 +239,123 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation)
EXPECT_DOUBLE_EQ(yaw, M_PI_4 / 2);
}

TEST(TestLongitudinalControllerUtils, lerpTrajectoryPoint)
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);
}
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 ef3b082

Please sign in to comment.