feat(planning_validator): add invalid size error (#6126)

* feat(planning_validator): add invalid size error Signed-off-by: Takamasa Horibe <[email protected]> * Update planning/planning_validator/src/planning_validator.cpp Co-authored-by: Kosuke Takeuchi <[email protected]> * Update planning/planning_validator/src/planning_validator.cpp Co-authored-by: Kosuke Takeuchi <[email protected]> * Update planning/planning_validator/include/planning_validator/planning_validator.hpp Co-authored-by: Kosuke Takeuchi <[email protected]> * add const for some functions Signed-off-by: Takamasa Horibe <[email protected]> --------- Signed-off-by: Takamasa Horibe <[email protected]> Co-authored-by: Kosuke Takeuchi <[email protected]>
autowarefoundation · Jan 21, 2024 · 547a025 · 547a025
1 parent 1a23d01
commit 547a025
Show file tree

Hide file tree

Showing 3 changed files with 35 additions and 11 deletions.
diff --git 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
@@ -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
@@ -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!!");