feat(pid_longitudinal_controller): add diag for control state (#2092)

* feat(pid_longitudinal_controller): add diag for control state

* feat: add translation and rotation deviation in diag

* feat: add cause in message

* feat: add control_state to diagnostic_aggregator

* feat: merge cpp

* feat: create DiagnosticData struct

* ci(pre-commit): autofix

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp

@@ -15,6 +15,7 @@
 #ifndef TRAJECTORY_FOLLOWER__PID_LONGITUDINAL_CONTROLLER_HPP_
 #define TRAJECTORY_FOLLOWER__PID_LONGITUDINAL_CONTROLLER_HPP_
 
+#include "diagnostic_updater/diagnostic_updater.hpp"
 #include "eigen3/Eigen/Core"
 #include "eigen3/Eigen/Geometry"
 #include "motion_common/motion_common.hpp"
@@ -204,6 +205,18 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal
 
   std::shared_ptr<rclcpp::Time> m_last_running_time{std::make_shared<rclcpp::Time>(node_->now())};
 
+  // Diagnostic
+
+  diagnostic_updater::Updater diagnostic_updater_;
+  struct DiagnosticData
+  {
+    float64_t trans_deviation{0.0};  // translation deviation between nearest point and current_pose
+    float64_t rot_deviation{0.0};    // rotation deviation between nearest point and current_pose
+  };
+  DiagnosticData m_diagnostic_data;
+  void setupDiagnosticUpdater();
+  void checkControlState(diagnostic_updater::DiagnosticStatusWrapper & stat);
+
   /**
    * @brief set current and previous velocity with received message
    * @param [in] msg current state message

control/trajectory_follower/src/pid_longitudinal_controller.cpp

@@ -34,7 +34,8 @@ namespace control
 {
 namespace trajectory_follower
 {
-PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) : node_{&node}
+PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node)
+: node_{&node}, diagnostic_updater_(&node)
 {
   using std::placeholders::_1;
 
@@ -199,6 +200,9 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) : node
   // set parameter callback
   m_set_param_res = node_->add_on_set_parameters_callback(
     std::bind(&PidLongitudinalController::paramCallback, this, _1));
+
+  // diagnostic
+  setupDiagnosticUpdater();
 }
 void PidLongitudinalController::setInputData(InputData const & input_data)
 {
@@ -404,6 +408,9 @@ boost::optional<LongitudinalOutput> PidLongitudinalController::run()
   // publish debug data
   publishDebugData(ctrl_cmd, control_data);
 
+  // diagnostic
+  diagnostic_updater_.force_update();
+
   return output;
 }
 
@@ -426,13 +433,15 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData
   const auto & nearest_point = m_trajectory_ptr->points.at(nearest_idx).pose;
 
   // check if the deviation is worth emergency
-  const bool is_dist_deviation_large =
-    m_state_transition_params.emergency_state_traj_trans_dev <
+  m_diagnostic_data.trans_deviation =
     tier4_autoware_utils::calcDistance2d(nearest_point, current_pose);
+  const bool is_dist_deviation_large =
+    m_state_transition_params.emergency_state_traj_trans_dev < m_diagnostic_data.trans_deviation;
+  m_diagnostic_data.rot_deviation = std::abs(tier4_autoware_utils::normalizeRadian(
+    tf2::getYaw(nearest_point.orientation) - tf2::getYaw(current_pose.orientation)));
   const bool is_yaw_deviation_large =
-    m_state_transition_params.emergency_state_traj_rot_dev <
-    std::abs(tier4_autoware_utils::normalizeRadian(
-      tf2::getYaw(nearest_point.orientation) - tf2::getYaw(current_pose.orientation)));
+    m_state_transition_params.emergency_state_traj_rot_dev < m_diagnostic_data.rot_deviation;
+
   if (is_dist_deviation_large || is_yaw_deviation_large) {
     // return here if nearest index is not found
     control_data.is_far_from_trajectory = true;
@@ -935,6 +944,45 @@ void PidLongitudinalController::updateDebugVelAcc(
   m_debug_values.setValues(DebugValues::TYPE::ERROR_VEL, target_motion.vel - current_vel);
 }
 
+void PidLongitudinalController::setupDiagnosticUpdater()
+{
+  diagnostic_updater_.setHardwareID("pid_longitudinal_controller");
+  diagnostic_updater_.add("control_state", this, &PidLongitudinalController::checkControlState);
+}
+
+void PidLongitudinalController::checkControlState(
+  diagnostic_updater::DiagnosticStatusWrapper & stat)
+{
+  using diagnostic_msgs::msg::DiagnosticStatus;
+
+  auto level = DiagnosticStatus::OK;
+  std::string msg = "OK";
+
+  if (m_control_state == ControlState::EMERGENCY) {
+    level = DiagnosticStatus::ERROR;
+    msg = "emergency occurred due to ";
+  }
+
+  if (
+    m_state_transition_params.emergency_state_traj_trans_dev < m_diagnostic_data.trans_deviation) {
+    msg += "translation deviation";
+  }
+
+  if (m_state_transition_params.emergency_state_traj_rot_dev < m_diagnostic_data.rot_deviation) {
+    msg += "rotation deviation";
+  }
+
+  stat.add<int32_t>("control_state", static_cast<int32_t>(m_control_state));
+  stat.addf(
+    "translation deviation threshold", "%lf",
+    m_state_transition_params.emergency_state_traj_trans_dev);
+  stat.addf("translation deviation", "%lf", m_diagnostic_data.trans_deviation);
+  stat.addf(
+    "rotation deviation threshold", "%lf", m_state_transition_params.emergency_state_traj_rot_dev);
+  stat.addf("rotation deviation", "%lf", m_diagnostic_data.rot_deviation);
+  stat.summary(level, msg);
+}
+
 }  // namespace trajectory_follower
 }  // namespace control
 }  // namespace motion

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

@@ -48,6 +48,12 @@
                   contains: [": trajectory_deviation"]
                   timeout: 1.0
 
+                control_state:
+                  type: diagnostic_aggregator/GenericAnalyzer
+                  path: control_state
+                  contains: [": control_state"]
+                  timeout: 1.0
+
         external_control:
           type: diagnostic_aggregator/AnalyzerGroup
           path: external_control