fix tests

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

Author: TakaHoribe

planning/planning_validator/include/planning_validator/planning_validator.hpp

@@ -83,7 +83,7 @@ class PlanningValidator : public rclcpp::Node
   bool checkValidVelocityDeviation(const Trajectory & trajectory);
   bool checkValidDistanceDeviation(const Trajectory & trajectory);
   bool checkValidLongitudinalDistanceDeviation(const Trajectory & trajectory);
-  bool checkValidForwardTrajectoryTimeLength(const Trajectory & trajectory);
+  bool checkValidForwardTrajectoryLength(const Trajectory & trajectory);
 
 private:
   void setupDiag();

planning/planning_validator/src/planning_validator.cpp

@@ -303,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_length = checkValidForwardTrajectoryTimeLength(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.
@@ -519,7 +519,7 @@ bool PlanningValidator::checkValidLongitudinalDistanceDeviation(const Trajectory
   return true;
 }
 
-bool PlanningValidator::checkValidForwardTrajectoryTimeLength(const Trajectory & trajectory)
+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) {
@@ -529,16 +529,13 @@ bool PlanningValidator::checkValidForwardTrajectoryTimeLength(const Trajectory &
   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;
 
+  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;
 

planning/planning_validator/test/src/test_parameter.hpp

@@ -32,6 +32,7 @@ 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 THRESHOLD_FORWARD_TRAJECTORY_TIME_LENGTH = 3.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_helper.cpp

@@ -207,7 +207,10 @@ rclcpp::NodeOptions getNodeOptionsWithDefaultParams()
   node_options.append_parameter_override(
     "thresholds.longitudinal_distance_deviation", THRESHOLD_LONGITUDINAL_DISTANCE_DEVIATION);
   node_options.append_parameter_override(
-    "thresholds.forward_trajectory_time_length", THRESHOLD_FORWARD_TRAJECTORY_TIME_LENGTH);
+    "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);

planning/planning_validator/test/src/test_planning_validator_pubsub.cpp

@@ -466,16 +466,19 @@ TEST(PlanningValidator, DiagCheckLongitudinalDistanceDeviation)
   test(trajectory.points.back().pose.position.x + valid_distance, 0.0, true);
 }
 
-TEST(PlanningValidator, DiagCheckForwardTrajectoryTimeLength)
+TEST(PlanningValidator, DiagCheckForwardTrajectoryLength)
 {
-  const auto diag_name = "planning_validator: trajectory_validation_forward_trajectory_time_length";
+  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 * 10.0;  // trajectory exists for 10 seconds
+    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);
@@ -485,7 +488,9 @@ TEST(PlanningValidator, DiagCheckForwardTrajectoryTimeLength)
 
   // Smaller trajectory than threshold -> must be NG
   {
-    constexpr auto bad_trajectory_length = ego_v * 3.0;  // trajectory exists for 3 seconds
+    // 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);
@@ -495,8 +500,8 @@ TEST(PlanningValidator, DiagCheckForwardTrajectoryTimeLength)
 
   // Longer trajectory than threshold, but smaller length from ego -> must be NG
   {
-    constexpr auto bad_trajectory_length =
-      ego_v * 10.0;  // trajectory exists for 10 seconds, but 1 sec for forward.
+    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);