@@ -466,16 +466,19 @@ TEST(PlanningValidator, DiagCheckLongitudinalDistanceDeviation)
466
466
test (trajectory.points .back ().pose .position .x + valid_distance, 0.0 , true );
467
467
}
468
468
469
- TEST (PlanningValidator, DiagCheckForwardTrajectoryTimeLength )
469
+ TEST (PlanningValidator, DiagCheckForwardTrajectoryLength )
470
470
{
471
- const auto diag_name = " planning_validator: trajectory_validation_forward_trajectory_time_length " ;
471
+ const auto diag_name = " planning_validator: trajectory_validation_forward_trajectory_length " ;
472
472
constexpr auto trajectory_v = 10.0 ;
473
473
constexpr size_t trajectory_size = 10 ;
474
474
constexpr auto ego_v = 10.0 ;
475
+ constexpr auto ego_a = std::abs (PARAMETER_FORWARD_TRAJECTORY_LENGTH_ACCELERATION);
476
+ constexpr auto margin = PARAMETER_FORWARD_TRAJECTORY_LENGTH_MARGIN;
475
477
476
478
// Longer trajectory than threshold -> must be OK
477
479
{
478
- constexpr auto ok_trajectory_length = ego_v * 10.0 ; // trajectory exists for 10 seconds
480
+ constexpr auto ok_trajectory_length = ego_v * ego_v / (2.0 * ego_a); // v1^2 - v0^2 = 2as
481
+ std::cerr << " aaa: " << ok_trajectory_length << std::endl;
479
482
constexpr auto trajectory_interval = ok_trajectory_length / trajectory_size;
480
483
const auto ok_trajectory =
481
484
generateTrajectory (trajectory_interval, trajectory_v, 0.0 , trajectory_size);
@@ -485,7 +488,9 @@ TEST(PlanningValidator, DiagCheckForwardTrajectoryTimeLength)
485
488
486
489
// Smaller trajectory than threshold -> must be NG
487
490
{
488
- constexpr auto bad_trajectory_length = ego_v * 3.0 ; // trajectory exists for 3 seconds
491
+ // shorter with 1.0m
492
+ constexpr auto bad_trajectory_length = ego_v * ego_v / (2.0 * ego_a) - margin - 1.0 ;
493
+ std::cerr << " bbb: " << bad_trajectory_length << std::endl;
489
494
constexpr auto trajectory_interval = bad_trajectory_length / trajectory_size;
490
495
const auto bad_trajectory =
491
496
generateTrajectory (trajectory_interval, trajectory_v, 0.0 , trajectory_size);
@@ -495,8 +500,8 @@ TEST(PlanningValidator, DiagCheckForwardTrajectoryTimeLength)
495
500
496
501
// Longer trajectory than threshold, but smaller length from ego -> must be NG
497
502
{
498
- constexpr auto bad_trajectory_length =
499
- ego_v * 10.0 ; // trajectory exists for 10 seconds, but 1 sec for forward.
503
+ constexpr auto bad_trajectory_length = ego_v * ego_v / ( 2.0 * ego_a); // v1^2 - v0^2 = 2as
504
+ std::cerr << " ccc: " << bad_trajectory_length << std::endl;
500
505
constexpr auto trajectory_interval = bad_trajectory_length / trajectory_size;
501
506
const auto bad_trajectory =
502
507
generateTrajectory (trajectory_interval, trajectory_v, 0.0 , trajectory_size);
0 commit comments