From 547a0251975ad3077d414adbf8463082dc55357e Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Sun, 21 Jan 2024 22:37:39 +0900 Subject: [PATCH] feat(planning_validator): add invalid size error (#6126) * feat(planning_validator): add invalid size error Signed-off-by: Takamasa Horibe * Update planning/planning_validator/src/planning_validator.cpp Co-authored-by: Kosuke Takeuchi * Update planning/planning_validator/src/planning_validator.cpp Co-authored-by: Kosuke Takeuchi * Update planning/planning_validator/include/planning_validator/planning_validator.hpp Co-authored-by: Kosuke Takeuchi * add const for some functions Signed-off-by: Takamasa Horibe --------- Signed-off-by: Takamasa Horibe Co-authored-by: Kosuke Takeuchi --- .../planning_validator/planning_validator.hpp | 3 +- .../msg/PlanningValidatorStatus.msg | 1 + .../src/planning_validator.cpp | 42 ++++++++++++++----- 3 files changed, 35 insertions(+), 11 deletions(-) diff --git a/planning/planning_validator/include/planning_validator/planning_validator.hpp b/planning/planning_validator/include/planning_validator/planning_validator.hpp index 037c7fb0f4e95..a12eb7dedcf88 100644 --- a/planning/planning_validator/include/planning_validator/planning_validator.hpp +++ b/planning/planning_validator/include/planning_validator/planning_validator.hpp @@ -64,6 +64,7 @@ class PlanningValidator : public rclcpp::Node void onTrajectory(const Trajectory::ConstSharedPtr msg); + bool checkValidSize(const Trajectory & trajectory) const; bool checkValidFiniteValue(const Trajectory & trajectory); bool checkValidInterval(const Trajectory & trajectory); bool checkValidRelativeAngle(const Trajectory & trajectory); @@ -116,7 +117,7 @@ class PlanningValidator : public rclcpp::Node vehicle_info_util::VehicleInfo vehicle_info_; - bool isAllValid(const PlanningValidatorStatus & status); + bool isAllValid(const PlanningValidatorStatus & status) const; Trajectory::ConstSharedPtr current_trajectory_; Trajectory::ConstSharedPtr previous_published_trajectory_; diff --git a/planning/planning_validator/msg/PlanningValidatorStatus.msg b/planning/planning_validator/msg/PlanningValidatorStatus.msg index cfc991b35ffc7..b218e7996888a 100644 --- a/planning/planning_validator/msg/PlanningValidatorStatus.msg +++ b/planning/planning_validator/msg/PlanningValidatorStatus.msg @@ -1,6 +1,7 @@ builtin_interfaces/Time stamp # states +bool is_valid_size bool is_valid_finite_value bool is_valid_interval bool is_valid_relative_angle diff --git a/planning/planning_validator/src/planning_validator.cpp b/planning/planning_validator/src/planning_validator.cpp index 58af2c08ccb22..6185163811614 100644 --- a/planning/planning_validator/src/planning_validator.cpp +++ b/planning/planning_validator/src/planning_validator.cpp @@ -113,6 +113,9 @@ void PlanningValidator::setupDiag() d->setHardwareID("planning_validator"); std::string ns = "trajectory_validation_"; + d->add(ns + "size", [&](auto & stat) { + setStatus(stat, validation_status_.is_valid_size, "invalid trajectory size is found"); + }); d->add(ns + "finite", [&](auto & stat) { setStatus(stat, validation_status_.is_valid_finite_value, "infinite value is found"); }); @@ -251,14 +254,26 @@ void PlanningValidator::publishDebugInfo() void PlanningValidator::validate(const Trajectory & trajectory) { - if (trajectory.points.size() < 2) { - RCLCPP_ERROR(get_logger(), "trajectory size is less than 2. Cannot validate."); - return; - } - auto & s = validation_status_; + const auto terminateValidation = [&](const auto & ss) { + RCLCPP_ERROR_STREAM(get_logger(), ss); + s.invalid_count += 1; + }; + + s.is_valid_size = checkValidSize(trajectory); + if (!s.is_valid_size) { + return terminateValidation( + "trajectory has invalid point size (" + std::to_string(trajectory.points.size()) + + "). Stop validation process, raise an error."); + } + s.is_valid_finite_value = checkValidFiniteValue(trajectory); + if (!s.is_valid_finite_value) { + return terminateValidation( + "trajectory has invalid value (NaN, Inf, etc). Stop validation process, raise an error."); + } + s.is_valid_interval = checkValidInterval(trajectory); s.is_valid_lateral_acc = checkValidLateralAcceleration(trajectory); s.is_valid_longitudinal_max_acc = checkValidMaxLongitudinalAcceleration(trajectory); @@ -279,6 +294,11 @@ void PlanningValidator::validate(const Trajectory & trajectory) s.invalid_count = isAllValid(s) ? 0 : s.invalid_count + 1; } +bool PlanningValidator::checkValidSize(const Trajectory & trajectory) const +{ + return trajectory.points.size() >= 2; +} + bool PlanningValidator::checkValidFiniteValue(const Trajectory & trajectory) { for (const auto & p : trajectory.points) { @@ -427,12 +447,13 @@ bool PlanningValidator::checkValidDistanceDeviation(const Trajectory & trajector return true; } -bool PlanningValidator::isAllValid(const PlanningValidatorStatus & s) +bool PlanningValidator::isAllValid(const PlanningValidatorStatus & s) const { - return 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; + 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; } void PlanningValidator::displayStatus() @@ -447,6 +468,7 @@ void PlanningValidator::displayStatus() const auto & s = validation_status_; + 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!!");