use acceleration to calculate required trajectory length

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

Author: TakaHoribe

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

@@ -27,4 +27,13 @@
       velocity_deviation: 100.0
       distance_deviation: 100.0
       longitudinal_distance_deviation: 1.0
-      forward_trajectory_time_length: 3.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;
@@ -56,7 +57,10 @@ struct ValidationParams
   double velocity_deviation_threshold;
   double distance_deviation_threshold;
   double longitudinal_distance_deviation_threshold;
-  double forward_trajectory_time_length_threshold;
+
+  // parameters
+  double forward_trajectory_length_acceleration;
+  double forward_trajectory_length_margin;
 };
 
 class PlanningValidator : public rclcpp::Node

planning/planning_validator/msg/PlanningValidatorStatus.msg

@@ -14,7 +14,7 @@ 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_time_length
+bool is_valid_forward_trajectory_length
 
 # values
 int64 trajectory_size
@@ -29,6 +29,7 @@ float64 max_steering_rate
 float64 velocity_deviation
 float64 distance_deviation
 float64 longitudinal_distance_deviation
-float64 forward_trajectory_time_length
+float64 forward_trajectory_length_required
+float64 forward_trajectory_length_measured
 
 int64 invalid_count

planning/planning_validator/src/planning_validator.cpp

@@ -82,8 +82,12 @@ void PlanningValidator::setupParameters()
     p.distance_deviation_threshold = declare_parameter<double>(t + "distance_deviation");
     p.longitudinal_distance_deviation_threshold =
       declare_parameter<double>(t + "longitudinal_distance_deviation");
-    p.forward_trajectory_time_length_threshold =
-      declare_parameter<double>(t + "forward_trajectory_time_length");
+
+    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 {
@@ -161,10 +165,10 @@ void PlanningValidator::setupDiag()
       stat, validation_status_.is_valid_longitudinal_distance_deviation,
       "longitudinal distance deviation is too large");
   });
-  d->add(ns + "forward_trajectory_time_length", [&](auto & stat) {
+  d->add(ns + "forward_trajectory_length", [&](auto & stat) {
     setStatus(
-      stat, validation_status_.is_valid_forward_trajectory_time_length,
-      "trajectory time length is too short");
+      stat, validation_status_.is_valid_forward_trajectory_length,
+      "trajectory length is too short");
   });
 }
 
@@ -299,7 +303,7 @@ void PlanningValidator::validate(const Trajectory & 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_time_length = checkValidForwardTrajectoryTimeLength(trajectory);
+  s.is_valid_forward_trajectory_length = checkValidForwardTrajectoryTimeLength(trajectory);
 
   // use resampled trajectory because the following metrics can not be evaluated for closed points.
   // Note: do not interpolate to keep original trajectory shape.
@@ -517,17 +521,28 @@ bool PlanningValidator::checkValidLongitudinalDistanceDeviation(const Trajectory
 
 bool PlanningValidator::checkValidForwardTrajectoryTimeLength(const Trajectory & trajectory)
 {
-  const auto forward_length = motion_utils::calcSignedArcLength(
-    trajectory.points, current_kinematics_->pose.pose.position, trajectory.points.size() - 1);
   const auto ego_speed = std::abs(current_kinematics_->twist.twist.linear.x);
-  if (ego_speed < 0.1) {
-    return true;  // Ego is stopped.
+  if (ego_speed < 1.0 / 3.6) {
+    return true;  // Ego is almost stopped.
   }
-  const auto forward_time_length = forward_length / std::max(ego_speed, 0.1);
-  validation_status_.forward_trajectory_time_length = forward_time_length;
 
-  return validation_status_.forward_trajectory_time_length >
-         validation_params_.forward_trajectory_time_length_threshold;
+  const auto forward_length = motion_utils::calcSignedArcLength(
+    trajectory.points, current_kinematics_->pose.pose.position, trajectory.points.size() - 1);
+
+  const auto tmp_all_length =
+    motion_utils::calcSignedArcLength(trajectory.points, 0, trajectory.points.size() - 1);
+
+  std::cerr << "forward_length = " << forward_length << ", tmp_all_length = " << tmp_all_length
+            << std::endl;
+
+  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
@@ -537,7 +552,7 @@ bool PlanningValidator::isAllValid(const PlanningValidatorStatus & s) const
          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_longitudinal_distance_deviation &&
-         s.is_valid_forward_trajectory_time_length;
+         s.is_valid_forward_trajectory_length;
 }
 
 void PlanningValidator::displayStatus()
@@ -567,9 +582,7 @@ void PlanningValidator::displayStatus()
   warn(
     s.is_valid_longitudinal_distance_deviation,
     "planning trajectory is too far from ego in longitudinal direction!!");
-  warn(
-    s.is_valid_forward_trajectory_time_length,
-    "planning trajectory forward time length is not enough!!");
+  warn(s.is_valid_forward_trajectory_length, "planning trajectory forward length is not enough!!");
 }
 
 }  // namespace planning_validator