feat(mpc_lateral_controller): signal a MRM when MPC fails. (#7016)

* mpc fail checker diagnostic added

Signed-off-by: Zhe Shen <lucidshenzhe@gmail.com>

* fix some scope issues

Signed-off-by: Zhe Shen <lucidshenzhe@gmail.com>

* member attribute added.

Signed-off-by: Zhe Shen <lucidshenzhe@gmail.com>

* shared pointer added.

Signed-off-by: Zhe Shen <lucidshenzhe@gmail.com>

* member attribute (diag_updater_) added

Signed-off-by: Zhe Shen <lucidshenzhe@gmail.com>

* dependency added.

Signed-off-by: Zhe Shen <lucidshenzhe@gmail.com>

* implementation of the MpcLateralController corrected!

Signed-off-by: Zhe Shen <lucidshenzhe@gmail.com>

* typo in comment corrected!

Signed-off-by: Zhe Shen <lucidshenzhe@gmail.com>

* member method argument corrected

Signed-off-by: Zhe Shen <lucidshenzhe@gmail.com>

* delete unnecessary reference mark

Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* rebase

Signed-off-by: Zhe Shen <lucidshenzhe@gmail.com>

* correct the include

Signed-off-by: Zhe Shen <lucidshenzhe@gmail.com>

* pre-commit

Signed-off-by: Zhe Shen <lucidshenzhe@gmail.com>

---------

Signed-off-by: Zhe Shen <lucidshenzhe@gmail.com>
Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>

Author: HansOersted

control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp

@@ -22,6 +22,8 @@
 #include "autoware/trajectory_follower_base/lateral_controller_base.hpp"
 #include "rclcpp/rclcpp.hpp"
 
+#include <diagnostic_updater/diagnostic_updater.hpp>
+
 #include "autoware_control_msgs/msg/lateral.hpp"
 #include "autoware_planning_msgs/msg/trajectory.hpp"
 #include "autoware_vehicle_msgs/msg/steering_report.hpp"
@@ -52,7 +54,8 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase
 {
 public:
   /// \param node Reference to the node used only for the component and parameter initialization.
-  explicit MpcLateralController(rclcpp::Node & node);
+  explicit MpcLateralController(
+    rclcpp::Node & node, std::shared_ptr<diagnostic_updater::Updater> diag_updater);
   virtual ~MpcLateralController();
 
 private:
@@ -63,6 +66,9 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase
   rclcpp::Publisher<Float32MultiArrayStamped>::SharedPtr m_pub_debug_values;
   rclcpp::Publisher<Float32Stamped>::SharedPtr m_pub_steer_offset;
 
+  std::shared_ptr<diagnostic_updater::Updater>
+    diag_updater_{};  // Diagnostic updater for publishing diagnostic data.
+
   //!< @brief parameters for path smoothing
   TrajectoryFilteringParam m_trajectory_filtering_param;
 
@@ -87,9 +93,16 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase
   // Flag indicating whether to keep the steering control until it converges.
   bool m_keep_steer_control_until_converged;
 
+  // MPC solver checker.
+  bool m_is_mpc_solved{true};
+
   // trajectory buffer for detecting new trajectory
   std::deque<Trajectory> m_trajectory_buffer;
 
+  void setStatus(diagnostic_updater::DiagnosticStatusWrapper & stat);
+
+  void setupDiag();
+
   std::unique_ptr<MPC> m_mpc;  // MPC object for trajectory following.
 
   // Check is mpc output converged

control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp

@@ -35,13 +35,16 @@
 namespace autoware::motion::control::mpc_lateral_controller
 {
 
-MpcLateralController::MpcLateralController(rclcpp::Node & node)
+MpcLateralController::MpcLateralController(
+  rclcpp::Node & node, std::shared_ptr<diagnostic_updater::Updater> diag_updater)
 : clock_(node.get_clock()), logger_(node.get_logger().get_child("lateral_controller"))
 {
   const auto dp_int = [&](const std::string & s) { return node.declare_parameter<int>(s); };
   const auto dp_bool = [&](const std::string & s) { return node.declare_parameter<bool>(s); };
   const auto dp_double = [&](const std::string & s) { return node.declare_parameter<double>(s); };
 
+  diag_updater_ = diag_updater;
+
   m_mpc = std::make_unique<MPC>(node);
 
   m_mpc->m_ctrl_period = node.get_parameter("ctrl_period").as_double();
@@ -152,6 +155,8 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node)
 
   m_mpc->setLogger(logger_);
   m_mpc->setClock(clock_);
+
+  setupDiag();
 }
 
 MpcLateralController::~MpcLateralController()
@@ -227,6 +232,24 @@ std::shared_ptr<SteeringOffsetEstimator> MpcLateralController::createSteerOffset
   return steering_offset_;
 }
 
+void MpcLateralController::setStatus(diagnostic_updater::DiagnosticStatusWrapper & stat)
+{
+  if (m_is_mpc_solved) {
+    stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "MPC succeeded.");
+  } else {
+    const std::string error_msg = "The MPC solver failed. Call MRM to stop the car.";
+    stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, error_msg);
+  }
+}
+
+void MpcLateralController::setupDiag()
+{
+  auto & d = diag_updater_;
+  d->setHardwareID("mpc_lateral_controller");
+
+  d->add("MPC_solve_checker", [&](auto & stat) { setStatus(stat); });
+}
+
 trajectory_follower::LateralOutput MpcLateralController::run(
   trajectory_follower::InputData const & input_data)
 {
@@ -255,6 +278,10 @@ trajectory_follower::LateralOutput MpcLateralController::run(
   const bool is_mpc_solved = m_mpc->calculateMPC(
     m_current_steering, m_current_kinematic_state, ctrl_cmd, predicted_traj, debug_values);
 
+  m_is_mpc_solved = is_mpc_solved;  // for diagnostic updater
+
+  diag_updater_->force_update();
+
   // reset previous MPC result
   // Note: When a large deviation from the trajectory occurs, the optimization stops and
   // the vehicle will return to the path by re-planning the trajectory or external operation.

control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/controller_node.hpp

@@ -30,6 +30,7 @@
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 #include <autoware/universe_utils/ros/published_time_publisher.hpp>
+#include <diagnostic_updater/diagnostic_updater.hpp>
 
 #include "autoware_control_msgs/msg/control.hpp"
 #include "autoware_control_msgs/msg/longitudinal.hpp"
@@ -73,6 +74,10 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node
   double timeout_thr_sec_;
   boost::optional<LongitudinalOutput> longitudinal_output_{boost::none};
 
+  std::shared_ptr<diagnostic_updater::Updater> diag_updater_ =
+    std::make_shared<diagnostic_updater::Updater>(
+      this);  // Diagnostic updater for publishing diagnostic data.
+
   std::shared_ptr<trajectory_follower::LongitudinalControllerBase> longitudinal_controller_;
   std::shared_ptr<trajectory_follower::LateralControllerBase> lateral_controller_;
 

control/autoware_trajectory_follower_node/src/controller_node.cpp

@@ -39,7 +39,8 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control
     getLateralControllerMode(declare_parameter<std::string>("lateral_controller_mode"));
   switch (lateral_controller_mode) {
     case LateralControllerMode::MPC: {
-      lateral_controller_ = std::make_shared<mpc_lateral_controller::MpcLateralController>(*this);
+      lateral_controller_ =
+        std::make_shared<mpc_lateral_controller::MpcLateralController>(*this, diag_updater_);
       break;
     }
     case LateralControllerMode::PURE_PURSUIT: {