Skip to content

Commit 720abf3

Browse files
committed
fix tests
Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
1 parent 7a10058 commit 720abf3

File tree

5 files changed

+23
-17
lines changed

5 files changed

+23
-17
lines changed

planning/planning_validator/include/planning_validator/planning_validator.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -83,7 +83,7 @@ class PlanningValidator : public rclcpp::Node
8383
bool checkValidVelocityDeviation(const Trajectory & trajectory);
8484
bool checkValidDistanceDeviation(const Trajectory & trajectory);
8585
bool checkValidLongitudinalDistanceDeviation(const Trajectory & trajectory);
86-
bool checkValidForwardTrajectoryTimeLength(const Trajectory & trajectory);
86+
bool checkValidForwardTrajectoryLength(const Trajectory & trajectory);
8787

8888
private:
8989
void setupDiag();

planning/planning_validator/src/planning_validator.cpp

+5-8
Original file line numberDiff line numberDiff line change
@@ -303,7 +303,7 @@ void PlanningValidator::validate(const Trajectory & trajectory)
303303
s.is_valid_velocity_deviation = checkValidVelocityDeviation(trajectory);
304304
s.is_valid_distance_deviation = checkValidDistanceDeviation(trajectory);
305305
s.is_valid_longitudinal_distance_deviation = checkValidLongitudinalDistanceDeviation(trajectory);
306-
s.is_valid_forward_trajectory_length = checkValidForwardTrajectoryTimeLength(trajectory);
306+
s.is_valid_forward_trajectory_length = checkValidForwardTrajectoryLength(trajectory);
307307

308308
// use resampled trajectory because the following metrics can not be evaluated for closed points.
309309
// Note: do not interpolate to keep original trajectory shape.
@@ -519,7 +519,7 @@ bool PlanningValidator::checkValidLongitudinalDistanceDeviation(const Trajectory
519519
return true;
520520
}
521521

522-
bool PlanningValidator::checkValidForwardTrajectoryTimeLength(const Trajectory & trajectory)
522+
bool PlanningValidator::checkValidForwardTrajectoryLength(const Trajectory & trajectory)
523523
{
524524
const auto ego_speed = std::abs(current_kinematics_->twist.twist.linear.x);
525525
if (ego_speed < 1.0 / 3.6) {
@@ -529,16 +529,13 @@ bool PlanningValidator::checkValidForwardTrajectoryTimeLength(const Trajectory &
529529
const auto forward_length = motion_utils::calcSignedArcLength(
530530
trajectory.points, current_kinematics_->pose.pose.position, trajectory.points.size() - 1);
531531

532-
const auto tmp_all_length =
533-
motion_utils::calcSignedArcLength(trajectory.points, 0, trajectory.points.size() - 1);
534-
535-
std::cerr << "forward_length = " << forward_length << ", tmp_all_length = " << tmp_all_length
536-
<< std::endl;
537-
538532
const auto acc = validation_params_.forward_trajectory_length_acceleration;
539533
const auto forward_length_required = ego_speed * ego_speed / (2.0 * std::abs(acc)) -
540534
validation_params_.forward_trajectory_length_margin;
541535

536+
std::cerr << "forward_length_required = " << forward_length_required
537+
<< ", forward_length = " << forward_length << std::endl;
538+
542539
validation_status_.forward_trajectory_length_required = forward_length_required;
543540
validation_status_.forward_trajectory_length_measured = forward_length;
544541

planning/planning_validator/test/src/test_parameter.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,7 @@ constexpr double THRESHOLD_STEERING_RATE = 7.0 * deg2rad;
3232
constexpr double THRESHOLD_VELOCITY_DEVIATION = 15.0 * kmph2mps;
3333
constexpr double THRESHOLD_DISTANCE_DEVIATION = 3.0;
3434
constexpr double THRESHOLD_LONGITUDINAL_DISTANCE_DEVIATION = 2.0;
35-
constexpr double THRESHOLD_FORWARD_TRAJECTORY_TIME_LENGTH = 3.0;
35+
constexpr double PARAMETER_FORWARD_TRAJECTORY_LENGTH_ACCELERATION = -5.0;
36+
constexpr double PARAMETER_FORWARD_TRAJECTORY_LENGTH_MARGIN = 2.0;
3637

3738
#endif // TEST_PARAMETER_HPP_

planning/planning_validator/test/src/test_planning_validator_helper.cpp

+4-1
Original file line numberDiff line numberDiff line change
@@ -207,7 +207,10 @@ rclcpp::NodeOptions getNodeOptionsWithDefaultParams()
207207
node_options.append_parameter_override(
208208
"thresholds.longitudinal_distance_deviation", THRESHOLD_LONGITUDINAL_DISTANCE_DEVIATION);
209209
node_options.append_parameter_override(
210-
"thresholds.forward_trajectory_time_length", THRESHOLD_FORWARD_TRAJECTORY_TIME_LENGTH);
210+
"parameters.forward_trajectory_length_acceleration",
211+
PARAMETER_FORWARD_TRAJECTORY_LENGTH_ACCELERATION);
212+
node_options.append_parameter_override(
213+
"parameters.forward_trajectory_length_margin", PARAMETER_FORWARD_TRAJECTORY_LENGTH_MARGIN);
211214

212215
// for vehicle info
213216
node_options.append_parameter_override("wheel_radius", 0.5);

planning/planning_validator/test/src/test_planning_validator_pubsub.cpp

+11-6
Original file line numberDiff line numberDiff line change
@@ -466,16 +466,19 @@ TEST(PlanningValidator, DiagCheckLongitudinalDistanceDeviation)
466466
test(trajectory.points.back().pose.position.x + valid_distance, 0.0, true);
467467
}
468468

469-
TEST(PlanningValidator, DiagCheckForwardTrajectoryTimeLength)
469+
TEST(PlanningValidator, DiagCheckForwardTrajectoryLength)
470470
{
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";
472472
constexpr auto trajectory_v = 10.0;
473473
constexpr size_t trajectory_size = 10;
474474
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;
475477

476478
// Longer trajectory than threshold -> must be OK
477479
{
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;
479482
constexpr auto trajectory_interval = ok_trajectory_length / trajectory_size;
480483
const auto ok_trajectory =
481484
generateTrajectory(trajectory_interval, trajectory_v, 0.0, trajectory_size);
@@ -485,7 +488,9 @@ TEST(PlanningValidator, DiagCheckForwardTrajectoryTimeLength)
485488

486489
// Smaller trajectory than threshold -> must be NG
487490
{
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;
489494
constexpr auto trajectory_interval = bad_trajectory_length / trajectory_size;
490495
const auto bad_trajectory =
491496
generateTrajectory(trajectory_interval, trajectory_v, 0.0, trajectory_size);
@@ -495,8 +500,8 @@ TEST(PlanningValidator, DiagCheckForwardTrajectoryTimeLength)
495500

496501
// Longer trajectory than threshold, but smaller length from ego -> must be NG
497502
{
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;
500505
constexpr auto trajectory_interval = bad_trajectory_length / trajectory_size;
501506
const auto bad_trajectory =
502507
generateTrajectory(trajectory_interval, trajectory_v, 0.0, trajectory_size);

0 commit comments

Comments
 (0)