feat(motion_velocity_smoother): add a diagnostic for smoothing failure

planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml

@@ -50,3 +50,6 @@
 
     # system
     over_stop_velocity_warn_thr: 1.389  # used to check if the optimization exceeds the input velocity on the stop point
+
+    # diagnostics
+    enable_diagnostics_when_smoothing_failure: 0.0

planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp

@@ -15,6 +15,7 @@
 #ifndef MOTION_VELOCITY_SMOOTHER__MOTION_VELOCITY_SMOOTHER_NODE_HPP_
 #define MOTION_VELOCITY_SMOOTHER__MOTION_VELOCITY_SMOOTHER_NODE_HPP_
 
+#include <diagnostic_updater/diagnostic_updater.hpp>
 #include "motion_utils/trajectory/tmp_conversion.hpp"
 #include "motion_utils/trajectory/trajectory.hpp"
 #include "motion_velocity_smoother/resample.hpp"
@@ -129,6 +130,8 @@ class MotionVelocitySmootherNode : public rclcpp::Node
     double ego_nearest_dist_threshold;    // for ego's closest index calculation
     double ego_nearest_yaw_threshold;     // for ego's closest index calculation
 
+    double enable_diagnostics_when_smoothing_failure;  // 0: DISABLE, 1: ENABLE
+
     resampling::ResampleParam post_resample_param;
     AlgorithmType algorithm_type;  // Option : JerkFiltered, Linf, L2
   } node_param_{};
@@ -180,11 +183,11 @@ class MotionVelocitySmootherNode : public rclcpp::Node
 
   AlgorithmType getAlgorithmType(const std::string & algorithm_name) const;
 
-  TrajectoryPoints calcTrajectoryVelocity(const TrajectoryPoints & traj_input) const;
+  TrajectoryPoints calcTrajectoryVelocity(const TrajectoryPoints & traj_input);
 
   bool smoothVelocity(
     const TrajectoryPoints & input, const size_t input_closest,
-    TrajectoryPoints & traj_smoothed) const;
+    TrajectoryPoints & traj_smoothed);
 
   std::pair<Motion, InitializeType> calcInitialMotion(
     const TrajectoryPoints & input_traj, const size_t input_closest) const;
@@ -242,6 +245,13 @@ class MotionVelocitySmootherNode : public rclcpp::Node
   bool isReverse(const TrajectoryPoints & points) const;
   void flipVelocity(TrajectoryPoints & points) const;
   void publishStopWatchTime();
+
+  // diagnostics
+  diagnostic_updater::Updater diagnostic_updater_{this};
+  bool failure_to_apply_smoother_{false};
+
+  void setupDiagnosticUpdater();
+  void checkSmootherErrors(diagnostic_updater::DiagnosticStatusWrapper & stat);
 };
 }  // namespace motion_velocity_smoother
 

planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp

@@ -129,6 +129,44 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions
   pub_velocity_limit_->publish(max_vel_msg);
 
   clock_ = get_clock();
+
+  // Diagnostics
+  setupDiagnosticUpdater();
+}
+
+void MotionVelocitySmootherNode::setupDiagnosticUpdater()
+{
+  diagnostic_updater_.setHardwareID("smoother");
+  if (node_param_.enable_diagnostics_when_smoothing_failure) {
+    diagnostic_updater_.add("smoother_errors", this, &MotionVelocitySmootherNode::checkSmootherErrors);
+  }
+  diagnostic_updater_.setPeriod(0.1);
+}
+
+void MotionVelocitySmootherNode::checkSmootherErrors(diagnostic_updater::DiagnosticStatusWrapper & stat)
+{
+  diagnostic_msgs::msg::DiagnosticStatus status;
+  status.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  status.message = "OK";
+
+  // guard
+  if (current_odometry_ptr_ == nullptr) {
+    return;
+  }
+
+  if (current_odometry_ptr_->twist.twist.linear.x < 1e-3) {
+    failure_to_apply_smoother_ = false;
+  }
+
+  if (failure_to_apply_smoother_) {
+    RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 3000 /*ms*/, "Failure to apply smoother");
+    status.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+    status.message = "Failure to apply smoother";
+  }
+
+  RCLCPP_INFO(get_logger(), "failure_to_apply_smoother_: %d", failure_to_apply_smoother_);
+
+  stat.summary(status);
 }
 
 rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter(
@@ -160,6 +198,7 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter
     update_param("stop_dist_to_prohibit_engage", p.stop_dist_to_prohibit_engage);
     update_param("ego_nearest_dist_threshold", p.ego_nearest_dist_threshold);
     update_param("ego_nearest_yaw_threshold", p.ego_nearest_yaw_threshold);
+    update_param("enable_diagnostics_when_smoothing_failure", p.enable_diagnostics_when_smoothing_failure);
   }
 
   {
@@ -274,6 +313,8 @@ void MotionVelocitySmootherNode::initCommonParam()
   p.post_resample_param.sparse_min_interval_distance =
     declare_parameter("post_sparse_min_interval_distance", 1.0);
   p.algorithm_type = getAlgorithmType(declare_parameter("algorithm_type", "JerkFiltered"));
+  p.enable_diagnostics_when_smoothing_failure =
+    declare_parameter("enable_diagnostics_when_smoothing_failure", 0.0);
 }
 
 void MotionVelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const
@@ -484,7 +525,7 @@ void MotionVelocitySmootherNode::updateDataForExternalVelocityLimit()
 }
 
 TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity(
-  const TrajectoryPoints & traj_input) const
+  const TrajectoryPoints & traj_input)
 {
   TrajectoryPoints output{};  // velocity is optimized by qp solver
 
@@ -531,7 +572,7 @@ TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity(
 
 bool MotionVelocitySmootherNode::smoothVelocity(
   const TrajectoryPoints & input, const size_t input_closest,
-  TrajectoryPoints & traj_smoothed) const
+  TrajectoryPoints & traj_smoothed)
 {
   // Calculate initial motion for smoothing
   const auto [initial_motion, type] = calcInitialMotion(input, input_closest);
@@ -579,6 +620,10 @@ bool MotionVelocitySmootherNode::smoothVelocity(
   if (!smoother_->apply(
         initial_motion.vel, initial_motion.acc, clipped, traj_smoothed, debug_trajectories)) {
     RCLCPP_WARN(get_logger(), "Fail to solve optimization.");
+    if (node_param_.enable_diagnostics_when_smoothing_failure) {
+      failure_to_apply_smoother_ = 1.0;
+      diagnostic_updater_.force_update();
+    }
   }
 
   // Set 0 velocity after input-stop-point

planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp

@@ -199,7 +199,7 @@ bool AnalyticalJerkConstrainedSmoother::apply(
         applyMaxVelocity(0.0, bwd_start_index, filtered_trajectory.size() - 1, filtered_trajectory);
         output = filtered_trajectory;
         RCLCPP_DEBUG(logger_, "-------------------- Finish --------------------");
-        return true;
+        return false;
       }
       applyMaxVelocity(decel_target_vel, bwd_start_index, decel_target_index, reference_trajectory);
       RCLCPP_DEBUG(logger_, "Apply forward jerk filter from: %ld", bwd_start_index);

system/system_error_monitor/config/diagnostic_aggregator/planning.param.yaml

@@ -55,3 +55,13 @@
               path: collision_check
               contains: [": collision_check"]
               timeout: 1.0
+
+        smoother_errors:
+          type: diagnostic_aggregator/AnalyzerGroup
+          path: smoother_errors
+          analyzers:
+            smoother_errors:
+              type: diagnostic_aggregator/GenericAnalyzer
+              path: smoother_errors
+              contains: [": smoother_errors"]
+              timeout: 1.0