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 55755a7f3e9ba..38ecef431b59a 100644 --- a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp @@ -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) {