diff --git a/planning/planning_validator/README.md b/planning/planning_validator/README.md index da1e572bb383a..a260254159947 100644 --- a/planning/planning_validator/README.md +++ b/planning/planning_validator/README.md @@ -18,6 +18,8 @@ The following features are supported for trajectory validation and can have thre - **Steering angle rate** : invalid if the expected steering rate value is too large - **Velocity deviation** : invalid if the planning velocity is too far from the ego velocity - **Distance deviation** : invalid if the ego is too far from the trajectory +- **Longitudinal distance deviation** : invalid if the trajectory is too far from ego in longitudinal direction +- **Forward trajectory length** : invalid if the trajectory length is not enough to stop within a given deceleration The following features are to be implemented. @@ -63,15 +65,23 @@ The following parameters can be set for the `planning_validator`: The input trajectory is detected as invalid if the index exceeds the following thresholds. -| Name | Type | Description | Default value | -| :-------------------------------- | :----- | :----------------------------------------------------------------------------------------------------------------- | :------------ | -| `thresholds.interval` | double | invalid threshold of the distance of two neighboring trajectory points [m] | 100.0 | -| `thresholds.relative_angle` | double | invalid threshold of the relative angle of two neighboring trajectory points [rad] | 2.0 | -| `thresholds.curvature` | double | invalid threshold of the curvature in each trajectory point [1/m] | 1.0 | -| `thresholds.lateral_acc` | double | invalid threshold of the lateral acceleration in each trajectory point [m/ss] | 9.8 | -| `thresholds.longitudinal_max_acc` | double | invalid threshold of the maximum longitudinal acceleration in each trajectory point [m/ss] | 9.8 | -| `thresholds.longitudinal_min_acc` | double | invalid threshold of the minimum longitudinal deceleration in each trajectory point [m/ss] | -9.8 | -| `thresholds.steering` | double | invalid threshold of the steering angle in each trajectory point [rad] | 1.414 | -| `thresholds.steering_rate` | double | invalid threshold of the steering angle rate in each trajectory point [rad/s] | 10.0 | -| `thresholds.velocity_deviation` | double | invalid threshold of the velocity deviation between the ego velocity and the trajectory point closest to ego [m/s] | 100.0 | -| `thresholds.distance_deviation` | double | invalid threshold of the distance deviation between the ego position and the trajectory point closest to ego [m] | 100.0 | +| Name | Type | Description | Default value | +| :------------------------------------------- | :----- | :----------------------------------------------------------------------------------------------------------------- | :------------ | +| `thresholds.interval` | double | invalid threshold of the distance of two neighboring trajectory points [m] | 100.0 | +| `thresholds.relative_angle` | double | invalid threshold of the relative angle of two neighboring trajectory points [rad] | 2.0 | +| `thresholds.curvature` | double | invalid threshold of the curvature in each trajectory point [1/m] | 1.0 | +| `thresholds.lateral_acc` | double | invalid threshold of the lateral acceleration in each trajectory point [m/ss] | 9.8 | +| `thresholds.longitudinal_max_acc` | double | invalid threshold of the maximum longitudinal acceleration in each trajectory point [m/ss] | 9.8 | +| `thresholds.longitudinal_min_acc` | double | invalid threshold of the minimum longitudinal deceleration in each trajectory point [m/ss] | -9.8 | +| `thresholds.steering` | double | invalid threshold of the steering angle in each trajectory point [rad] | 1.414 | +| `thresholds.steering_rate` | double | invalid threshold of the steering angle rate in each trajectory point [rad/s] | 10.0 | +| `thresholds.velocity_deviation` | double | invalid threshold of the velocity deviation between the ego velocity and the trajectory point closest to ego [m/s] | 100.0 | +| `thresholds.distance_deviation` | double | invalid threshold of the distance deviation between the ego position and the trajectory point closest to ego [m] | 100.0 | +| `parameters.longitudinal_distance_deviation` | double | invalid threshold of the longitudinal distance deviation between the ego position and the trajectory [m] | 2.0 | + +#### Parameters + +For parameters used e.g. to calculate threshold. + +| `parameters.forward_trajectory_length_acceleration` | double | This value is used to calculate required trajectory length. | -5.0 | +| `parameters.forward_trajectory_length_margin` | double | A margin of the required trajectory length not to raise an error when the ego slightly exceeds the trajectory end point. | 2.0 | diff --git a/planning/planning_validator/config/planning_validator.param.yaml b/planning/planning_validator/config/planning_validator.param.yaml index 658a968906187..da337d70b1078 100644 --- a/planning/planning_validator/config/planning_validator.param.yaml +++ b/planning/planning_validator/config/planning_validator.param.yaml @@ -26,3 +26,14 @@ steering_rate: 10.0 velocity_deviation: 100.0 distance_deviation: 100.0 + longitudinal_distance_deviation: 1.0 + + parameters: + # The required trajectory length is calculated as the distance needed + # to stop from the current speed at this deceleration. + forward_trajectory_length_acceleration: -3.0 + + # An error is raised if the required trajectory length is less than this distance. + # Setting it to 0 means an error will occur if even slightly exceeding the end of the path, + # therefore, a certain margin is necessary. + forward_trajectory_length_margin: 2.0 diff --git a/planning/planning_validator/include/planning_validator/planning_validator.hpp b/planning/planning_validator/include/planning_validator/planning_validator.hpp index a12eb7dedcf88..4e27def784b12 100644 --- a/planning/planning_validator/include/planning_validator/planning_validator.hpp +++ b/planning/planning_validator/include/planning_validator/planning_validator.hpp @@ -45,6 +45,7 @@ using tier4_debug_msgs::msg::Float64Stamped; struct ValidationParams { + // thresholds double interval_threshold; double relative_angle_threshold; double curvature_threshold; @@ -55,6 +56,11 @@ struct ValidationParams double steering_rate_threshold; double velocity_deviation_threshold; double distance_deviation_threshold; + double longitudinal_distance_deviation_threshold; + + // parameters + double forward_trajectory_length_acceleration; + double forward_trajectory_length_margin; }; class PlanningValidator : public rclcpp::Node @@ -64,7 +70,7 @@ class PlanningValidator : public rclcpp::Node void onTrajectory(const Trajectory::ConstSharedPtr msg); - bool checkValidSize(const Trajectory & trajectory) const; + bool checkValidSize(const Trajectory & trajectory); bool checkValidFiniteValue(const Trajectory & trajectory); bool checkValidInterval(const Trajectory & trajectory); bool checkValidRelativeAngle(const Trajectory & trajectory); @@ -76,6 +82,8 @@ class PlanningValidator : public rclcpp::Node bool checkValidSteeringRate(const Trajectory & trajectory); bool checkValidVelocityDeviation(const Trajectory & trajectory); bool checkValidDistanceDeviation(const Trajectory & trajectory); + bool checkValidLongitudinalDistanceDeviation(const Trajectory & trajectory); + bool checkValidForwardTrajectoryLength(const Trajectory & trajectory); private: void setupDiag(); diff --git a/planning/planning_validator/msg/PlanningValidatorStatus.msg b/planning/planning_validator/msg/PlanningValidatorStatus.msg index b218e7996888a..cf127347a6021 100644 --- a/planning/planning_validator/msg/PlanningValidatorStatus.msg +++ b/planning/planning_validator/msg/PlanningValidatorStatus.msg @@ -13,8 +13,11 @@ bool is_valid_steering bool is_valid_steering_rate bool is_valid_velocity_deviation bool is_valid_distance_deviation +bool is_valid_longitudinal_distance_deviation +bool is_valid_forward_trajectory_length # values +int64 trajectory_size float64 max_interval_distance float64 max_relative_angle float64 max_curvature @@ -25,5 +28,8 @@ float64 max_steering float64 max_steering_rate float64 velocity_deviation float64 distance_deviation +float64 longitudinal_distance_deviation +float64 forward_trajectory_length_required +float64 forward_trajectory_length_measured int64 invalid_count diff --git a/planning/planning_validator/src/planning_validator.cpp b/planning/planning_validator/src/planning_validator.cpp index 6185163811614..8b8b7d4566f08 100644 --- a/planning/planning_validator/src/planning_validator.cpp +++ b/planning/planning_validator/src/planning_validator.cpp @@ -80,6 +80,14 @@ void PlanningValidator::setupParameters() p.steering_rate_threshold = declare_parameter(t + "steering_rate"); p.velocity_deviation_threshold = declare_parameter(t + "velocity_deviation"); p.distance_deviation_threshold = declare_parameter(t + "distance_deviation"); + p.longitudinal_distance_deviation_threshold = + declare_parameter(t + "longitudinal_distance_deviation"); + + const std::string ps = "parameters."; + p.forward_trajectory_length_acceleration = + declare_parameter(ps + "forward_trajectory_length_acceleration"); + p.forward_trajectory_length_margin = + declare_parameter(ps + "forward_trajectory_length_margin"); } try { @@ -148,6 +156,20 @@ void PlanningValidator::setupDiag() setStatus( stat, validation_status_.is_valid_velocity_deviation, "velocity deviation is too large"); }); + d->add(ns + "distance_deviation", [&](auto & stat) { + setStatus( + stat, validation_status_.is_valid_distance_deviation, "distance deviation is too large"); + }); + d->add(ns + "longitudinal_distance_deviation", [&](auto & stat) { + setStatus( + stat, validation_status_.is_valid_longitudinal_distance_deviation, + "longitudinal distance deviation is too large"); + }); + d->add(ns + "forward_trajectory_length", [&](auto & stat) { + setStatus( + stat, validation_status_.is_valid_forward_trajectory_length, + "trajectory length is too short"); + }); } bool PlanningValidator::isDataReady() @@ -280,6 +302,8 @@ void PlanningValidator::validate(const Trajectory & trajectory) s.is_valid_longitudinal_min_acc = checkValidMinLongitudinalAcceleration(trajectory); s.is_valid_velocity_deviation = checkValidVelocityDeviation(trajectory); s.is_valid_distance_deviation = checkValidDistanceDeviation(trajectory); + s.is_valid_longitudinal_distance_deviation = checkValidLongitudinalDistanceDeviation(trajectory); + s.is_valid_forward_trajectory_length = checkValidForwardTrajectoryLength(trajectory); // use resampled trajectory because the following metrics can not be evaluated for closed points. // Note: do not interpolate to keep original trajectory shape. @@ -294,8 +318,9 @@ void PlanningValidator::validate(const Trajectory & trajectory) s.invalid_count = isAllValid(s) ? 0 : s.invalid_count + 1; } -bool PlanningValidator::checkValidSize(const Trajectory & trajectory) const +bool PlanningValidator::checkValidSize(const Trajectory & trajectory) { + validation_status_.trajectory_size = trajectory.points.size(); return trajectory.points.size() >= 2; } @@ -447,13 +472,81 @@ bool PlanningValidator::checkValidDistanceDeviation(const Trajectory & trajector return true; } +bool PlanningValidator::checkValidLongitudinalDistanceDeviation(const Trajectory & trajectory) +{ + if (trajectory.points.size() < 2) { + RCLCPP_ERROR(get_logger(), "Trajectory size is invalid to calculate distance deviation."); + return false; + } + + const auto ego_pose = current_kinematics_->pose.pose; + const size_t idx = + motion_utils::findFirstNearestIndexWithSoftConstraints(trajectory.points, ego_pose); + + if (0 < idx && idx < trajectory.points.size() - 1) { + return true; // ego-nearest point exists between trajectory points. + } + + // Check if the valid longitudinal deviation for given segment index + const auto HasValidLongitudinalDeviation = [&](const size_t seg_idx, const bool is_last) { + auto long_offset = + motion_utils::calcLongitudinalOffsetToSegment(trajectory.points, seg_idx, ego_pose.position); + + // for last, need to remove distance for the last segment. + if (is_last) { + const auto size = trajectory.points.size(); + long_offset -= tier4_autoware_utils::calcDistance2d( + trajectory.points.at(size - 1), trajectory.points.at(size - 2)); + } + + validation_status_.longitudinal_distance_deviation = long_offset; + return std::abs(validation_status_.longitudinal_distance_deviation) < + validation_params_.longitudinal_distance_deviation_threshold; + }; + + // Make sure the trajectory is far AHEAD from ego. + if (idx == 0) { + const auto seg_idx = 0; + return HasValidLongitudinalDeviation(seg_idx, false); + } + + // Make sure the trajectory is far BEHIND from ego. + if (idx == trajectory.points.size() - 1) { + const auto seg_idx = trajectory.points.size() - 2; + return HasValidLongitudinalDeviation(seg_idx, true); + } + + return true; +} + +bool PlanningValidator::checkValidForwardTrajectoryLength(const Trajectory & trajectory) +{ + const auto ego_speed = std::abs(current_kinematics_->twist.twist.linear.x); + if (ego_speed < 1.0 / 3.6) { + return true; // Ego is almost stopped. + } + + const auto forward_length = motion_utils::calcSignedArcLength( + trajectory.points, current_kinematics_->pose.pose.position, trajectory.points.size() - 1); + + const auto acc = validation_params_.forward_trajectory_length_acceleration; + const auto forward_length_required = ego_speed * ego_speed / (2.0 * std::abs(acc)) - + validation_params_.forward_trajectory_length_margin; + + validation_status_.forward_trajectory_length_required = forward_length_required; + validation_status_.forward_trajectory_length_measured = forward_length; + + return forward_length > forward_length_required; +} + bool PlanningValidator::isAllValid(const PlanningValidatorStatus & s) const { return s.is_valid_size && s.is_valid_finite_value && s.is_valid_interval && s.is_valid_relative_angle && s.is_valid_curvature && s.is_valid_lateral_acc && s.is_valid_longitudinal_max_acc && s.is_valid_longitudinal_min_acc && s.is_valid_steering && s.is_valid_steering_rate && s.is_valid_velocity_deviation && - s.is_valid_distance_deviation; + s.is_valid_distance_deviation && s.is_valid_longitudinal_distance_deviation && + s.is_valid_forward_trajectory_length; } void PlanningValidator::displayStatus() @@ -470,7 +563,6 @@ void PlanningValidator::displayStatus() warn(s.is_valid_size, "planning trajectory size is invalid, too small."); warn(s.is_valid_curvature, "planning trajectory curvature is too large!!"); - warn(s.is_valid_distance_deviation, "planning trajectory is too far from ego!!"); warn(s.is_valid_finite_value, "planning trajectory has invalid value!!"); warn(s.is_valid_interval, "planning trajectory interval is too long!!"); warn(s.is_valid_lateral_acc, "planning trajectory lateral acceleration is too high!!"); @@ -480,6 +572,11 @@ void PlanningValidator::displayStatus() warn(s.is_valid_steering, "planning trajectory expected steering angle is too high!!"); warn(s.is_valid_steering_rate, "planning trajectory expected steering angle rate is too high!!"); warn(s.is_valid_velocity_deviation, "planning trajectory velocity deviation is too high!!"); + warn(s.is_valid_distance_deviation, "planning trajectory is too far from ego!!"); + warn( + s.is_valid_longitudinal_distance_deviation, + "planning trajectory is too far from ego in longitudinal direction!!"); + warn(s.is_valid_forward_trajectory_length, "planning trajectory forward length is not enough!!"); } } // namespace planning_validator diff --git a/planning/planning_validator/src/utils.cpp b/planning/planning_validator/src/utils.cpp index 3155a6f2e0bc8..aada0bea2da9b 100644 --- a/planning/planning_validator/src/utils.cpp +++ b/planning/planning_validator/src/utils.cpp @@ -296,7 +296,7 @@ std::pair calcMaxSteeringRates( const auto steer_next = steering_array.at(i + 1); const auto steer_rate = (steer_next - steer_prev) / dt; - takeBigger(max_steering_rate, max_index, steer_rate, i); + takeBigger(max_steering_rate, max_index, std::abs(steer_rate), i); } return {max_steering_rate, max_index}; diff --git a/planning/planning_validator/test/src/test_parameter.hpp b/planning/planning_validator/test/src/test_parameter.hpp new file mode 100644 index 0000000000000..55bbbdffbb4f3 --- /dev/null +++ b/planning/planning_validator/test/src/test_parameter.hpp @@ -0,0 +1,38 @@ + +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_PARAMETER_HPP_ +#define TEST_PARAMETER_HPP_ + +constexpr double deg2rad = 3.14159265 / 180.0; +constexpr double kmph2mps = 1.0 / 3.6; + +constexpr double WHEELBASE = 3.5; + +constexpr double THRESHOLD_INTERVAL = 1.0; +constexpr double THRESHOLD_RELATIVE_ANGLE = 115.0 * deg2rad; +constexpr double THRESHOLD_CURVATURE = 0.3; +constexpr double THRESHOLD_LATERAL_ACC = 5.0; +constexpr double THRESHOLD_LONGITUDINAL_MAX_ACC = 3.0; +constexpr double THRESHOLD_LONGITUDINAL_MIN_ACC = -6.0; +constexpr double THRESHOLD_STEERING = 35.0 * deg2rad; +constexpr double THRESHOLD_STEERING_RATE = 7.0 * deg2rad; +constexpr double THRESHOLD_VELOCITY_DEVIATION = 15.0 * kmph2mps; +constexpr double THRESHOLD_DISTANCE_DEVIATION = 3.0; +constexpr double THRESHOLD_LONGITUDINAL_DISTANCE_DEVIATION = 2.0; +constexpr double PARAMETER_FORWARD_TRAJECTORY_LENGTH_ACCELERATION = -5.0; +constexpr double PARAMETER_FORWARD_TRAJECTORY_LENGTH_MARGIN = 2.0; + +#endif // TEST_PARAMETER_HPP_ diff --git a/planning/planning_validator/test/src/test_planning_validator_functions.cpp b/planning/planning_validator/test/src/test_planning_validator_functions.cpp index 241ed3b8831d5..482996c4c040d 100644 --- a/planning/planning_validator/test/src/test_planning_validator_functions.cpp +++ b/planning/planning_validator/test/src/test_planning_validator_functions.cpp @@ -14,6 +14,7 @@ #include "planning_validator/debug_marker.hpp" #include "planning_validator/planning_validator.hpp" +#include "test_parameter.hpp" #include "test_planning_validator_helper.hpp" #include @@ -29,7 +30,7 @@ TEST(PlanningValidatorTestSuite, checkValidFiniteValueFunction) // Valid Trajectory { - Trajectory valid_traj = generateTrajectory(NOMINAL_INTERVAL); + Trajectory valid_traj = generateTrajectory(THRESHOLD_INTERVAL * 0.9); ASSERT_TRUE(validator->checkValidFiniteValue(valid_traj)); } @@ -52,7 +53,7 @@ TEST(PlanningValidatorTestSuite, checkValidIntervalFunction) // Normal Trajectory { - Trajectory valid_traj = generateTrajectory(NOMINAL_INTERVAL); + Trajectory valid_traj = generateTrajectory(THRESHOLD_INTERVAL * 0.9); ASSERT_TRUE(validator->checkValidInterval(valid_traj)); } @@ -61,16 +62,16 @@ TEST(PlanningValidatorTestSuite, checkValidIntervalFunction) // Note: too small value is not supported like numerical_limits::epsilon const auto ep = 1.0e-5; - Trajectory ok_bound_traj = generateTrajectory(ERROR_INTERVAL - ep); + Trajectory ok_bound_traj = generateTrajectory(THRESHOLD_INTERVAL - ep); ASSERT_TRUE(validator->checkValidInterval(ok_bound_traj)); - Trajectory ng_bound_traj = generateTrajectory(ERROR_INTERVAL + ep); + Trajectory ng_bound_traj = generateTrajectory(THRESHOLD_INTERVAL + ep); ASSERT_FALSE(validator->checkValidInterval(ng_bound_traj)); } // Long Interval Trajectory { - Trajectory long_interval_traj = generateTrajectory(ERROR_INTERVAL * 2.0); + Trajectory long_interval_traj = generateTrajectory(THRESHOLD_INTERVAL * 2.0); ASSERT_FALSE(validator->checkValidInterval(long_interval_traj)); } } @@ -81,7 +82,7 @@ TEST(PlanningValidatorTestSuite, checkValidCurvatureFunction) // Normal Trajectory { - Trajectory valid_traj = generateTrajectory(NOMINAL_INTERVAL); + Trajectory valid_traj = generateTrajectory(THRESHOLD_INTERVAL * 2.0); ASSERT_TRUE(validator->checkValidCurvature(valid_traj)); } diff --git a/planning/planning_validator/test/src/test_planning_validator_helper.cpp b/planning/planning_validator/test/src/test_planning_validator_helper.cpp index ec584e7ee1a06..19e0d5a7f73ee 100644 --- a/planning/planning_validator/test/src/test_planning_validator_helper.cpp +++ b/planning/planning_validator/test/src/test_planning_validator_helper.cpp @@ -14,40 +14,150 @@ #include "test_planning_validator_helper.hpp" +#include "test_parameter.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + #include using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using tier4_autoware_utils::createQuaternionFromYaw; -Trajectory generateTrajectory(double interval_distance) +Trajectory generateTrajectoryWithConstantAcceleration( + const double interval_distance, const double speed, const double yaw, const size_t size, + const double acceleration) { - Trajectory traj; - for (double s = 0.0; s <= 10.0 * interval_distance; s += interval_distance) { + Trajectory trajectory; + double s = 0.0, v = speed, a = acceleration; + constexpr auto MAX_DT = 10.0; + for (size_t i = 0; i < size; ++i) { TrajectoryPoint p; - p.pose.position.x = s; - p.longitudinal_velocity_mps = 1.0; - traj.points.push_back(p); + p.pose.position.x = s * std::cos(yaw); + p.pose.position.y = s * std::sin(yaw); + p.pose.orientation = createQuaternionFromYaw(yaw); + p.longitudinal_velocity_mps = v; + p.acceleration_mps2 = a; + p.front_wheel_angle_rad = 0.0; + trajectory.points.push_back(p); + s += interval_distance; + + const auto dt = std::abs(v) > 0.1 ? interval_distance / v : MAX_DT; + v += acceleration * dt; + if (v < 0.0) { + v = 0.0; + a = 0.0; + } } - return traj; + return trajectory; +} + +Trajectory generateTrajectory( + const double interval_distance, const double speed, const double yaw, const size_t size) +{ + constexpr auto acceleration = 0.0; + return generateTrajectoryWithConstantAcceleration( + interval_distance, speed, yaw, size, acceleration); +} + +Trajectory generateTrajectoryWithConstantCurvature( + const double interval_distance, const double speed, const double curvature, const size_t size, + const double wheelbase) +{ + if (std::abs(curvature) < 1.0e-5) { + return generateTrajectory(interval_distance, speed, 0.0, size); + } + + const auto steering = std::atan(curvature * wheelbase); + const auto radius = 1.0 / curvature; + + Trajectory trajectory; + double x = 0.0, y = 0.0, yaw = 0.0; + + for (size_t i = 0; i <= size; ++i) { + TrajectoryPoint p; + p.pose.position.x = x; + p.pose.position.y = y; + p.pose.orientation = createQuaternionFromYaw(yaw); + p.longitudinal_velocity_mps = speed; + p.front_wheel_angle_rad = steering; + trajectory.points.push_back(p); + + // Update x, y, yaw for the next point + const auto prev_yaw = yaw; + double delta_yaw = curvature * interval_distance; + yaw += delta_yaw; + x += radius * (std::sin(yaw) - std::sin(prev_yaw)); + y -= radius * (std::cos(yaw) - std::cos(prev_yaw)); + } + return trajectory; +} + +Trajectory generateTrajectoryWithConstantSteering( + const double interval_distance, const double speed, const double steering_angle_rad, + const size_t size, const double wheelbase) +{ + const auto curvature = std::tan(steering_angle_rad) / wheelbase; + return generateTrajectoryWithConstantCurvature( + interval_distance, speed, curvature, size, wheelbase); +} + +Trajectory generateTrajectoryWithConstantSteeringRate( + const double interval_distance, const double speed, const double steering_rate, const size_t size, + const double wheelbase) +{ + Trajectory trajectory; + double x = 0.0, y = 0.0, yaw = 0.0, steering_angle_rad = 0.0; + + constexpr double MAX_STEERING_ANGLE_RAD = M_PI / 3.0; + + for (size_t i = 0; i <= size; ++i) { + // Limit the steering angle to the maximum value + steering_angle_rad = + std::clamp(steering_angle_rad, -MAX_STEERING_ANGLE_RAD, MAX_STEERING_ANGLE_RAD); + + TrajectoryPoint p; + p.pose.position.x = x; + p.pose.position.y = y; + p.pose.orientation = createQuaternionFromYaw(yaw); + p.longitudinal_velocity_mps = speed; + p.front_wheel_angle_rad = steering_angle_rad; + p.acceleration_mps2 = 0.0; + + trajectory.points.push_back(p); + + // Update x, y, yaw, and steering_angle for the next point + const auto curvature = std::tan(steering_angle_rad) / wheelbase; + double delta_yaw = curvature * interval_distance; + yaw += delta_yaw; + x += interval_distance * cos(yaw); + y += interval_distance * sin(yaw); + if (std::abs(speed) > 0.01) { + steering_angle_rad += steering_rate * interval_distance / speed; + } else { + steering_angle_rad = steering_rate > 0.0 ? MAX_STEERING_ANGLE_RAD : -MAX_STEERING_ANGLE_RAD; + } + } + + return trajectory; } Trajectory generateNanTrajectory() { - Trajectory traj = generateTrajectory(1.0); - traj.points.front().pose.position.x = NAN; - return traj; + Trajectory trajectory = generateTrajectory(1.0); + trajectory.points.front().pose.position.x = NAN; + return trajectory; } Trajectory generateInfTrajectory() { - Trajectory traj = generateTrajectory(1.0); - traj.points.front().pose.position.x = INFINITY; - return traj; + Trajectory trajectory = generateTrajectory(1.0); + trajectory.points.front().pose.position.x = INFINITY; + return trajectory; } Trajectory generateBadCurvatureTrajectory() { - Trajectory traj; + Trajectory trajectory; double y = 1.5; for (double s = 0.0; s <= 10.0; s += 1.0) { @@ -56,10 +166,10 @@ Trajectory generateBadCurvatureTrajectory() p.pose.position.x = s; p.pose.position.y = y; y *= -1.0; // invert sign - traj.points.push_back(p); + trajectory.points.push_back(p); } - return traj; + return trajectory; } Odometry generateDefaultOdometry(const double x, const double y, const double vx) @@ -79,22 +189,33 @@ rclcpp::NodeOptions getNodeOptionsWithDefaultParams() node_options.append_parameter_override("publish_diag", true); node_options.append_parameter_override("invalid_trajectory_handling_type", 0); node_options.append_parameter_override("diag_error_count_threshold", 0); - node_options.append_parameter_override("display_on_terminal", false); - node_options.append_parameter_override("thresholds.interval", ERROR_INTERVAL); - node_options.append_parameter_override("thresholds.relative_angle", 1.0); - node_options.append_parameter_override("thresholds.curvature", ERROR_CURVATURE); - node_options.append_parameter_override("thresholds.lateral_acc", 100.0); - node_options.append_parameter_override("thresholds.longitudinal_max_acc", 100.0); - node_options.append_parameter_override("thresholds.longitudinal_min_acc", -100.0); - node_options.append_parameter_override("thresholds.steering", 100.0); - node_options.append_parameter_override("thresholds.steering_rate", 100.0); - node_options.append_parameter_override("thresholds.velocity_deviation", 100.0); - node_options.append_parameter_override("thresholds.distance_deviation", 100.0); + node_options.append_parameter_override("display_on_terminal", true); + node_options.append_parameter_override("thresholds.interval", THRESHOLD_INTERVAL); + node_options.append_parameter_override("thresholds.relative_angle", THRESHOLD_RELATIVE_ANGLE); + node_options.append_parameter_override("thresholds.curvature", THRESHOLD_CURVATURE); + node_options.append_parameter_override("thresholds.lateral_acc", THRESHOLD_LATERAL_ACC); + node_options.append_parameter_override( + "thresholds.longitudinal_max_acc", THRESHOLD_LONGITUDINAL_MAX_ACC); + node_options.append_parameter_override( + "thresholds.longitudinal_min_acc", THRESHOLD_LONGITUDINAL_MIN_ACC); + node_options.append_parameter_override("thresholds.steering", THRESHOLD_STEERING); + node_options.append_parameter_override("thresholds.steering_rate", THRESHOLD_STEERING_RATE); + node_options.append_parameter_override( + "thresholds.velocity_deviation", THRESHOLD_VELOCITY_DEVIATION); + node_options.append_parameter_override( + "thresholds.distance_deviation", THRESHOLD_DISTANCE_DEVIATION); + node_options.append_parameter_override( + "thresholds.longitudinal_distance_deviation", THRESHOLD_LONGITUDINAL_DISTANCE_DEVIATION); + node_options.append_parameter_override( + "parameters.forward_trajectory_length_acceleration", + PARAMETER_FORWARD_TRAJECTORY_LENGTH_ACCELERATION); + node_options.append_parameter_override( + "parameters.forward_trajectory_length_margin", PARAMETER_FORWARD_TRAJECTORY_LENGTH_MARGIN); // for vehicle info node_options.append_parameter_override("wheel_radius", 0.5); node_options.append_parameter_override("wheel_width", 0.2); - node_options.append_parameter_override("wheel_base", 3.0); + node_options.append_parameter_override("wheel_base", WHEELBASE); node_options.append_parameter_override("wheel_tread", 2.0); node_options.append_parameter_override("front_overhang", 1.0); node_options.append_parameter_override("rear_overhang", 1.0); diff --git a/planning/planning_validator/test/src/test_planning_validator_helper.hpp b/planning/planning_validator/test/src/test_planning_validator_helper.hpp index c3890475e808d..d2e6472fb04bc 100644 --- a/planning/planning_validator/test/src/test_planning_validator_helper.hpp +++ b/planning/planning_validator/test/src/test_planning_validator_helper.hpp @@ -21,14 +21,28 @@ #include #include -constexpr double NOMINAL_INTERVAL = 1.0; -constexpr double ERROR_INTERVAL = 1000.0; -constexpr double ERROR_CURVATURE = 2.0; - using autoware_auto_planning_msgs::msg::Trajectory; using nav_msgs::msg::Odometry; -Trajectory generateTrajectory(double interval_distance); +Trajectory generateTrajectoryWithConstantAcceleration( + const double interval_distance, const double speed, const double yaw, const size_t size, + const double acceleration); + +Trajectory generateTrajectory( + const double interval_distance, const double speed = 1.0, const double yaw = 0.0, + const size_t size = 10); + +Trajectory generateTrajectoryWithConstantCurvature( + const double interval_distance, const double speed, const double curvature, const size_t size, + const double wheelbase); + +Trajectory generateTrajectoryWithConstantSteering( + const double interval_distance, const double speed, const double steering_angle_rad, + const size_t size, const double wheelbase); + +Trajectory generateTrajectoryWithConstantSteeringRate( + const double interval_distance, const double speed, const double steering_rate, const size_t size, + const double wheelbase); Trajectory generateNanTrajectory(); diff --git a/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp b/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp index 889ba5780af1f..a7b90be4471cd 100644 --- a/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp +++ b/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "planning_validator/planning_validator.hpp" +#include "test_parameter.hpp" #include "test_planning_validator_helper.hpp" #include @@ -34,19 +35,22 @@ using diagnostic_msgs::msg::DiagnosticStatus; using nav_msgs::msg::Odometry; using planning_validator::PlanningValidator; +constexpr double epsilon = 0.001; +constexpr double scale_margin = 1.1; + class PubSubManager : public rclcpp::Node { public: PubSubManager() : Node("test_pub_sub") { - traj_pub_ = create_publisher("/planning_validator/input/trajectory", 1); + trajectory_pub_ = create_publisher("/planning_validator/input/trajectory", 1); kinematics_pub_ = create_publisher("/planning_validator/input/kinematics", 1); diag_sub_ = create_subscription( "/diagnostics", 1, [this](const DiagnosticArray::ConstSharedPtr msg) { received_diags_.push_back(msg); }); } - rclcpp::Publisher::SharedPtr traj_pub_; + rclcpp::Publisher::SharedPtr trajectory_pub_; rclcpp::Publisher::SharedPtr kinematics_pub_; rclcpp::Subscription::SharedPtr diag_sub_; @@ -55,7 +59,7 @@ class PubSubManager : public rclcpp::Node void spinSome(rclcpp::Node::SharedPtr node_ptr) { - for (int i = 0; i < 50; ++i) { + for (int i = 0; i < 10; ++i) { rclcpp::spin_some(node_ptr); rclcpp::WallRate(100).sleep(); } @@ -85,53 +89,424 @@ bool hasError(const std::vector & diags) return false; } -void runWithOKTrajectory(const Trajectory & trajectory) +bool hasError(const std::vector & diags, const std::string & name) +{ + for (const auto & diag : diags) { + for (const auto & status : diag->status) { + if (status.name == name) { + return status.level == DiagnosticStatus::ERROR; + } + } + } + throw std::runtime_error(name + " is not contained in the diagnostic message."); +} + +std::pair, std::shared_ptr> +prepareTest(const Trajectory & trajectory, const Odometry & ego_odom) { auto validator = std::make_shared(getNodeOptionsWithDefaultParams()); auto manager = std::make_shared(); - EXPECT_GE(manager->traj_pub_->get_subscription_count(), 1U) << "topic is not connected."; + EXPECT_GE(manager->trajectory_pub_->get_subscription_count(), 1U) << "topic is not connected."; - manager->traj_pub_->publish(trajectory); - manager->kinematics_pub_->publish(generateDefaultOdometry()); + manager->trajectory_pub_->publish(trajectory); + manager->kinematics_pub_->publish(ego_odom); spinSome(validator); spinSome(manager); + return {validator, manager}; +} + +void runWithOKTrajectory(const Trajectory & trajectory, const Odometry & ego_odom) +{ + auto [validator, manager] = prepareTest(trajectory, ego_odom); + EXPECT_GE(manager->received_diags_.size(), 1U) << "diag has not received!"; EXPECT_TRUE(isAllOK(manager->received_diags_)); } -void runWithBadTrajectory(const Trajectory & trajectory) +void runWithOKTrajectory( + const Trajectory & trajectory, const Odometry & ego_odom, const std::string & name) { - auto validator = std::make_shared(getNodeOptionsWithDefaultParams()); - auto manager = std::make_shared(); - EXPECT_GE(manager->traj_pub_->get_subscription_count(), 1U) << "topic is not connected."; + auto [validator, manager] = prepareTest(trajectory, ego_odom); - manager->traj_pub_->publish(trajectory); - manager->kinematics_pub_->publish(generateDefaultOdometry()); - spinSome(validator); - spinSome(manager); + EXPECT_GE(manager->received_diags_.size(), 1U) << "diag has not received!"; + EXPECT_FALSE(hasError(manager->received_diags_, name)); +} + +void runWithBadTrajectory(const Trajectory & trajectory, const Odometry & ego_odom) +{ + auto [validator, manager] = prepareTest(trajectory, ego_odom); EXPECT_GE(manager->received_diags_.size(), 1U) << "diag has not received!"; EXPECT_TRUE(hasError(manager->received_diags_)); } +void runWithBadTrajectory( + const Trajectory & trajectory, const Odometry & ego_odom, const std::string & name) +{ + auto [validator, manager] = prepareTest(trajectory, ego_odom); + + EXPECT_GE(manager->received_diags_.size(), 1U) << "diag has not received!"; + EXPECT_TRUE(hasError(manager->received_diags_, name)); +} + +// ============================================================= +// Overall tests +// ============================================================= + // OK cases TEST(PlanningValidator, DiagCheckForNominalTrajectory) { - runWithOKTrajectory(generateTrajectory(NOMINAL_INTERVAL)); + runWithOKTrajectory(generateTrajectory(THRESHOLD_INTERVAL * 0.5), generateDefaultOdometry()); } // Bad cases TEST(PlanningValidator, DiagCheckForNaNTrajectory) { - runWithBadTrajectory(generateNanTrajectory()); + runWithBadTrajectory(generateNanTrajectory(), generateDefaultOdometry()); } TEST(PlanningValidator, DiagCheckForInfTrajectory) { - runWithBadTrajectory(generateInfTrajectory()); + runWithBadTrajectory(generateInfTrajectory(), generateDefaultOdometry()); } TEST(PlanningValidator, DiagCheckForTooLongIntervalTrajectory) { constexpr double ep = 0.001; - runWithBadTrajectory(generateTrajectory(ERROR_INTERVAL + ep)); + runWithBadTrajectory(generateTrajectory(THRESHOLD_INTERVAL + ep), generateDefaultOdometry()); +} + +// ============================================================= +// Specific diag tests +// ============================================================= + +TEST(PlanningValidator, DiagCheckSize) +{ + const auto diag_name = "planning_validator: trajectory_validation_size"; + const auto odom = generateDefaultOdometry(); + runWithBadTrajectory(generateTrajectory(1.0, 1.0, 0.0, 0), odom, diag_name); + runWithBadTrajectory(generateTrajectory(1.0, 1.0, 0.0, 1), odom, diag_name); + runWithOKTrajectory(generateTrajectory(1.0, 1.0, 0.0, 2), odom); + runWithOKTrajectory(generateTrajectory(1.0, 1.0, 0.0, 3), odom); +} + +TEST(PlanningValidator, DiagCheckInterval) +{ + const auto diag_name = "planning_validator: trajectory_validation_interval"; + const auto odom = generateDefaultOdometry(); + + // Larger interval than threshold -> must be NG + { + auto trajectory = generateTrajectory(1.0, 1.0, 0.0, 10); + auto tp = trajectory.points.back(); + tp.pose.position.x += THRESHOLD_INTERVAL + 0.001; + trajectory.points.push_back(tp); + runWithBadTrajectory(trajectory, odom, diag_name); + } + + // Smaller interval than threshold -> must be OK + { + auto trajectory = generateTrajectory(1.0, 1.0, 0.0, 10); + auto tp = trajectory.points.back(); + tp.pose.position.x += THRESHOLD_INTERVAL - 0.001; + trajectory.points.push_back(tp); + runWithOKTrajectory(trajectory, odom, diag_name); + } +} + +TEST(PlanningValidator, DiagCheckRelativeAngle) +{ + const auto diag_name = "planning_validator: trajectory_validation_relative_angle"; + + // TODO(Horibe): interval must be larger than min_interval used in planning_validator.cpp + constexpr auto interval = 1.1; + + const auto odom = generateDefaultOdometry(); + + // Larger Relative Angle than threshold -> must be NG + { + auto bad_trajectory = generateTrajectory(interval, 1.0, 0.0, 10); + auto bad_tp = bad_trajectory.points.back(); + bad_tp.pose.position.x += interval * std::cos(THRESHOLD_RELATIVE_ANGLE + 0.01); + bad_tp.pose.position.y += interval * std::sin(THRESHOLD_RELATIVE_ANGLE + 0.01); + bad_trajectory.points.push_back(bad_tp); + runWithBadTrajectory(bad_trajectory, generateDefaultOdometry(), diag_name); + } + + // Smaller Relative Angle than threshold -> must be OK + { + auto ok_trajectory = generateTrajectory(interval, 1.0, 0.0, 10); + auto ok_tp = ok_trajectory.points.back(); + ok_tp.pose.position.x += interval * std::cos(THRESHOLD_RELATIVE_ANGLE - 0.01); + ok_tp.pose.position.y += interval * std::sin(THRESHOLD_RELATIVE_ANGLE - 0.01); + ok_trajectory.points.push_back(ok_tp); + runWithOKTrajectory(ok_trajectory, generateDefaultOdometry(), diag_name); + } +} + +TEST(PlanningValidator, DiagCheckCurvature) +{ + const auto diag_name = "planning_validator: trajectory_validation_curvature"; + + // TODO(Horibe): interval must be larger than min_interval used in planning_validator.cpp + constexpr auto interval = 1.1; + + const auto odom = generateDefaultOdometry(); + + // Large y at one point -> must be NG + { + auto bad_trajectory = generateTrajectory(interval, 1.0, 0.0, 10); + bad_trajectory.points.at(5).pose.position.y += 5.0; + runWithBadTrajectory(bad_trajectory, odom, diag_name); + } + + // Higher curvature than threshold -> must be NG + { + constexpr double curvature = THRESHOLD_CURVATURE * scale_margin; + auto bad_trajectory = + generateTrajectoryWithConstantCurvature(interval, 1.0, curvature, 10, WHEELBASE); + runWithBadTrajectory(bad_trajectory, odom, diag_name); + } + + // Lower curvature than threshold -> must be OK + { + constexpr double curvature = THRESHOLD_CURVATURE / scale_margin; + auto ok_trajectory = + generateTrajectoryWithConstantCurvature(interval, 1.0, curvature, 10, WHEELBASE); + runWithOKTrajectory(ok_trajectory, odom, diag_name); + } +} + +TEST(PlanningValidator, DiagCheckLateralAcceleration) +{ + const auto diag_name = "planning_validator: trajectory_validation_lateral_acceleration"; + constexpr double speed = 10.0; + + // Note: lateral_acceleration is speed^2 * curvature; + + // Higher lateral acc than threshold -> must be NG + { + constexpr double curvature = THRESHOLD_LATERAL_ACC / (speed * speed) * scale_margin; + const auto bad_trajectory = + generateTrajectoryWithConstantCurvature(1.0, speed, curvature, 10, WHEELBASE); + runWithBadTrajectory(bad_trajectory, generateDefaultOdometry(), diag_name); + } + + // Smaller lateral acc than threshold -> must be OK + { + constexpr double curvature = THRESHOLD_LATERAL_ACC / (speed * speed) / scale_margin; + const auto ok_trajectory = + generateTrajectoryWithConstantCurvature(1.0, speed, curvature, 10, WHEELBASE); + runWithOKTrajectory(ok_trajectory, generateDefaultOdometry(), diag_name); + } +} + +TEST(PlanningValidator, DiagCheckLongitudinalMaxAcc) +{ + const auto diag_name = "planning_validator: trajectory_validation_acceleration"; + constexpr double speed = 1.0; + + // Larger acceleration than threshold -> must be NG + { + constexpr double acceleration = THRESHOLD_LONGITUDINAL_MAX_ACC + epsilon; + auto bad_trajectory = + generateTrajectoryWithConstantAcceleration(1.0, speed, 0.0, 20, acceleration); + runWithBadTrajectory(bad_trajectory, generateDefaultOdometry(), diag_name); + } + + // Smaller acceleration than threshold -> must be OK + { + constexpr double acceleration = THRESHOLD_LONGITUDINAL_MAX_ACC - epsilon; + auto bad_trajectory = + generateTrajectoryWithConstantAcceleration(1.0, speed, 0.0, 20, acceleration); + runWithOKTrajectory(bad_trajectory, generateDefaultOdometry(), diag_name); + } +} + +TEST(PlanningValidator, DiagCheckLongitudinalMinAcc) +{ + const auto diag_name = "planning_validator: trajectory_validation_deceleration"; + constexpr double speed = 20.0; + + const auto test = [&](const auto acceleration, const bool expect_ok) { + auto trajectory = generateTrajectoryWithConstantAcceleration(1.0, speed, 0.0, 10, acceleration); + if (expect_ok) { + runWithOKTrajectory(trajectory, generateDefaultOdometry(), diag_name); + } else { + runWithBadTrajectory(trajectory, generateDefaultOdometry(), diag_name); + } + }; + + // Larger deceleration than threshold -> must be NG + test(THRESHOLD_LONGITUDINAL_MIN_ACC - epsilon, false); + + // Larger deceleration than threshold -> must be OK + test(THRESHOLD_LONGITUDINAL_MIN_ACC + epsilon, true); +} + +TEST(PlanningValidator, DiagCheckSteering) +{ + const auto diag_name = "planning_validator: trajectory_validation_steering"; + + // TODO(Horibe): interval must be larger than min_interval used in planning_validator.cpp + constexpr auto interval = 1.1; + + const auto test = [&](const auto steering, const bool expect_ok) { + auto trajectory = + generateTrajectoryWithConstantSteering(interval, 1.0, steering, 10, WHEELBASE); + const auto odom = generateDefaultOdometry(); + expect_ok ? runWithOKTrajectory(trajectory, odom, diag_name) + : runWithBadTrajectory(trajectory, odom, diag_name); + }; + + // Larger steering than threshold -> must be NG + test(THRESHOLD_STEERING * scale_margin, false); + test(-THRESHOLD_STEERING * scale_margin, false); + + // Smaller steering than threshold -> must be OK + test(THRESHOLD_STEERING / scale_margin, true); + test(-THRESHOLD_STEERING / scale_margin, true); +} + +TEST(PlanningValidator, DiagCheckSteeringRate) +{ + const auto diag_name = "planning_validator: trajectory_validation_steering_rate"; + + // TODO(Horibe): interval must be larger than min_interval used in planning_validator.cpp + constexpr auto interval = 1.1; + + const auto test = [&](const auto steering_rate, const bool expect_ok) { + auto trajectory = + generateTrajectoryWithConstantSteeringRate(interval, 1.0, steering_rate, 10, WHEELBASE); + const auto odom = generateDefaultOdometry(); + expect_ok ? runWithOKTrajectory(trajectory, odom, diag_name) + : runWithBadTrajectory(trajectory, odom, diag_name); + }; + + // Larger steering rate than threshold -> must be NG + test(THRESHOLD_STEERING_RATE * scale_margin, false); + test(-THRESHOLD_STEERING_RATE * scale_margin, false); + + // Smaller steering rate than threshold -> must be OK + test(THRESHOLD_STEERING_RATE / scale_margin, true); + test(-THRESHOLD_STEERING_RATE / scale_margin, true); +} + +TEST(PlanningValidator, DiagCheckVelocityDeviation) +{ + const auto diag_name = "planning_validator: trajectory_validation_velocity_deviation"; + const auto test = [&](const auto trajectory_speed, const auto ego_speed, const bool expect_ok) { + const auto trajectory = generateTrajectory(1.0, trajectory_speed, 0.0, 10); + const auto ego_odom = generateDefaultOdometry(0.0, 0.0, ego_speed); + expect_ok ? runWithOKTrajectory(trajectory, ego_odom, diag_name) + : runWithBadTrajectory(trajectory, ego_odom, diag_name); + }; + + // Larger velocity deviation than threshold -> must be NG + test(1.0 + THRESHOLD_VELOCITY_DEVIATION * scale_margin, 1.0, false); + test(1.0, 1.0 + THRESHOLD_VELOCITY_DEVIATION * scale_margin, false); + + // Larger velocity deviation than threshold -> must be OK + test(1.0, 1.0 + THRESHOLD_VELOCITY_DEVIATION / scale_margin, true); +} + +TEST(PlanningValidator, DiagCheckDistanceDeviation) +{ + const auto diag_name = "planning_validator: trajectory_validation_distance_deviation"; + const auto test = [&](const auto ego_x, const auto ego_y, const bool expect_ok) { + const auto trajectory = generateTrajectory(1.0, 3.0, 0.0, 10); + const auto last_p = trajectory.points.back().pose.position; + const auto ego_odom = generateDefaultOdometry(last_p.x + ego_x, last_p.y + ego_y, 0.0); + expect_ok ? runWithOKTrajectory(trajectory, ego_odom, diag_name) + : runWithBadTrajectory(trajectory, ego_odom, diag_name); + }; + + // Larger distance deviation than threshold -> must be NG + const auto error_distance = THRESHOLD_DISTANCE_DEVIATION * scale_margin; + test(error_distance, 0.0, false); + test(0.0, error_distance, false); + test(0.0, -error_distance, false); + test(error_distance, error_distance, false); + test(error_distance, -error_distance, false); + + // Smaller distance deviation than threshold -> must be OK + const auto ok_distance = THRESHOLD_DISTANCE_DEVIATION / scale_margin; + test(ok_distance, 0.0, true); + test(0.0, ok_distance, true); + test(0.0, -ok_distance, true); +} + +TEST(PlanningValidator, DiagCheckLongitudinalDistanceDeviation) +{ + const auto diag_name = + "planning_validator: trajectory_validation_longitudinal_distance_deviation"; + const auto trajectory = generateTrajectory(1.0, 3.0, 0.0, 10); + const auto test = [&](const auto ego_x, const auto ego_y, const bool expect_ok) { + const auto ego_odom = generateDefaultOdometry(ego_x, ego_y, 0.0); + expect_ok ? runWithOKTrajectory(trajectory, ego_odom, diag_name) + : runWithBadTrajectory(trajectory, ego_odom, diag_name); + }; + + const auto invalid_distance = THRESHOLD_LONGITUDINAL_DISTANCE_DEVIATION * scale_margin; + const auto valid_distance = THRESHOLD_LONGITUDINAL_DISTANCE_DEVIATION / scale_margin; + + // behind from idx=0 -> must be NG + test(-invalid_distance, 0.0, false); + + // ahead from idx=last -> must be NG + test(trajectory.points.back().pose.position.x + invalid_distance, 0.0, false); + + // In between trajectory points -> must be OK + const auto mid_point = trajectory.points.at(5).pose.position; + test(mid_point.x, mid_point.y, true); + + // behind from idx=0, but close to 0 -> must be OK + test(-valid_distance, 0.0, true); + + // ahead from idx=last, but close to last -> must be OK + test(trajectory.points.back().pose.position.x + valid_distance, 0.0, true); +} + +TEST(PlanningValidator, DiagCheckForwardTrajectoryLength) +{ + const auto diag_name = "planning_validator: trajectory_validation_forward_trajectory_length"; + constexpr auto trajectory_v = 10.0; + constexpr size_t trajectory_size = 10; + constexpr auto ego_v = 10.0; + constexpr auto ego_a = std::abs(PARAMETER_FORWARD_TRAJECTORY_LENGTH_ACCELERATION); + constexpr auto margin = PARAMETER_FORWARD_TRAJECTORY_LENGTH_MARGIN; + + // Longer trajectory than threshold -> must be OK + { + constexpr auto ok_trajectory_length = ego_v * ego_v / (2.0 * ego_a); // v1^2 - v0^2 = 2as + std::cerr << "aaa: " << ok_trajectory_length << std::endl; + constexpr auto trajectory_interval = ok_trajectory_length / trajectory_size; + const auto ok_trajectory = + generateTrajectory(trajectory_interval, trajectory_v, 0.0, trajectory_size); + const auto ego_odom = generateDefaultOdometry(0.0, 0.0, ego_v); + runWithOKTrajectory(ok_trajectory, ego_odom, diag_name); + } + + // Smaller trajectory than threshold -> must be NG + { + // shorter with 1.0m + constexpr auto bad_trajectory_length = ego_v * ego_v / (2.0 * ego_a) - margin - 1.0; + std::cerr << "bbb: " << bad_trajectory_length << std::endl; + constexpr auto trajectory_interval = bad_trajectory_length / trajectory_size; + const auto bad_trajectory = + generateTrajectory(trajectory_interval, trajectory_v, 0.0, trajectory_size); + const auto ego_odom = generateDefaultOdometry(0.0, 0.0, ego_v); + runWithBadTrajectory(bad_trajectory, ego_odom, diag_name); + } + + // Longer trajectory than threshold, but smaller length from ego -> must be NG + { + constexpr auto bad_trajectory_length = ego_v * ego_v / (2.0 * ego_a); // v1^2 - v0^2 = 2as + std::cerr << "ccc: " << bad_trajectory_length << std::endl; + constexpr auto trajectory_interval = bad_trajectory_length / trajectory_size; + const auto bad_trajectory = + generateTrajectory(trajectory_interval, trajectory_v, 0.0, trajectory_size); + const auto & p = bad_trajectory.points.at(trajectory_size - 1).pose.position; + const auto ego_odom = generateDefaultOdometry(p.x, p.y, ego_v); + runWithBadTrajectory(bad_trajectory, ego_odom, diag_name); + } }