feat(motion_velocity_smoother): add a diagnostic for smoothing failure (

#581)

* feat(motion_velocity_smoother): add a diagnostic for smoothing failure

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

* fix(motion_velocity_smoother): change enable flag to bool

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

* fix(motion_velocity_smoother): fix type of assigned value

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

* fix(motion_velocity_smoother): remove an unnecessary log

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

* refactor(motion_velocity_smoother): clang format

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

---------

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>
Co-authored-by: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com>

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: false

planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp

@@ -33,6 +33,8 @@
 #include "tier4_autoware_utils/ros/self_pose_listener.hpp"
 #include "tier4_autoware_utils/system/stop_watch.hpp"
 
+#include <diagnostic_updater/diagnostic_updater.hpp>
+
 #include "autoware_auto_planning_msgs/msg/trajectory.hpp"
 #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
 #include "nav_msgs/msg/odometry.hpp"
@@ -129,6 +131,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
 
+    bool enable_diagnostics_when_smoothing_failure;
+
     resampling::ResampleParam post_resample_param;
     AlgorithmType algorithm_type;  // Option : JerkFiltered, Linf, L2
   } node_param_{};
@@ -180,11 +184,10 @@ 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;
+    const TrajectoryPoints & input, const size_t input_closest, 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";
+  }
+
+  stat.summary(status);
 }
 
 rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter(
@@ -274,6 +312,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", false);
 }
 
 void MotionVelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const
@@ -484,7 +524,7 @@ void MotionVelocitySmootherNode::updateDataForExternalVelocityLimit()
 }
 
 TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity(
-  const TrajectoryPoints & traj_input) const
+  const TrajectoryPoints & traj_input)
 {
   TrajectoryPoints output{};  // velocity is optimized by qp solver
 
@@ -530,8 +570,7 @@ TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity(
 }
 
 bool MotionVelocitySmootherNode::smoothVelocity(
-  const TrajectoryPoints & input, const size_t input_closest,
-  TrajectoryPoints & traj_smoothed) const
+  const TrajectoryPoints & input, const size_t input_closest, TrajectoryPoints & traj_smoothed)
 {
   // Calculate initial motion for smoothing
   const auto [initial_motion, type] = calcInitialMotion(input, input_closest);
@@ -579,6 +618,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_ = true;
+      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