feat(planning_validator): add validation for trajectory length and related tests

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 |

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

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();

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

planning/planning_validator/src/planning_validator.cpp

@@ -80,6 +80,14 @@
     p.steering_rate_threshold = declare_parameter<double>(t + "steering_rate");
     p.velocity_deviation_threshold = declare_parameter<double>(t + "velocity_deviation");
     p.distance_deviation_threshold = declare_parameter<double>(t + "distance_deviation");
+    p.longitudinal_distance_deviation_threshold =
+      declare_parameter<double>(t + "longitudinal_distance_deviation");
+
+    const std::string ps = "parameters.";
+    p.forward_trajectory_length_acceleration =
+      declare_parameter<double>(ps + "forward_trajectory_length_acceleration");
+    p.forward_trajectory_length_margin =
+      declare_parameter<double>(ps + "forward_trajectory_length_margin");
   }
 
   try {
@@ -148,6 +156,20 @@
     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 @@
   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 @@
   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,84 @@
   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;
+
+  std::cerr << "forward_length_required = " << forward_length_required
+            << ", forward_length = " << forward_length << std::endl;
+
+  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 +566,6 @@
 
   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 +575,11 @@
   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

planning/planning_validator/src/utils.cpp

@@ -296,7 +296,7 @@ std::pair<double, size_t> 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};

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_

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 <gtest/gtest.h>
@@ -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));
   }