feat(tier4_state_rviz_plugin): add Fail Safe Visualization (autowaref…

…oundation#2626)

* feat(tier4_state_rviz_plugin): add information for Fail Safe

* fix color

* ci(pre-commit): autofix

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

common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp

@@ -64,6 +64,7 @@ AutowareStatePanel::AutowareStatePanel(QWidget * parent) : rviz_common::Panel(pa
     h_layout->addWidget(makeRoutingGroup());
     h_layout->addWidget(makeLocalizationGroup());
     h_layout->addWidget(makeMotionGroup());
+    h_layout->addWidget(makeFailSafeGroup());
     v_layout->addLayout(h_layout);
   }
 
@@ -186,6 +187,25 @@ QGroupBox * AutowareStatePanel::makeMotionGroup()
   return group;
 }
 
+QGroupBox * AutowareStatePanel::makeFailSafeGroup()
+{
+  auto * group = new QGroupBox("FalSafe");
+  auto * grid = new QGridLayout;
+
+  mrm_state_label_ptr_ = new QLabel("INIT");
+  mrm_state_label_ptr_->setAlignment(Qt::AlignCenter);
+  mrm_state_label_ptr_->setStyleSheet("border:1px solid black;");
+  grid->addWidget(mrm_state_label_ptr_, 0, 0);
+
+  mrm_behavior_label_ptr_ = new QLabel("INIT");
+  mrm_behavior_label_ptr_->setAlignment(Qt::AlignCenter);
+  mrm_behavior_label_ptr_->setStyleSheet("border:1px solid black;");
+  grid->addWidget(mrm_behavior_label_ptr_, 1, 0);
+
+  group->setLayout(grid);
+  return group;
+}
+
 void AutowareStatePanel::onInitialize()
 {
   raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
@@ -234,6 +254,12 @@ void AutowareStatePanel::onInitialize()
   client_accept_start_ = raw_node_->create_client<AcceptStart>(
     "/api/motion/accept_start", rmw_qos_profile_services_default);
 
+  // FailSafe
+  sub_mrm_ = raw_node_->create_subscription<MRMState>(
+    "/api/fail_safe/mrm_state", rclcpp::QoS{1}.transient_local(),
+    std::bind(&AutowareStatePanel::onMRMState, this, _1));
+
+  // Others
   sub_gear_ = raw_node_->create_subscription<autoware_auto_vehicle_msgs::msg::GearReport>(
     "/vehicle/status/gear_status", 10, std::bind(&AutowareStatePanel::onShift, this, _1));
 
@@ -421,6 +447,72 @@ void AutowareStatePanel::onMotion(const MotionState::ConstSharedPtr msg)
   }
 }
 
+void AutowareStatePanel::onMRMState(const MRMState::ConstSharedPtr msg)
+{
+  // state
+  {
+    QString text = "";
+    QString style_sheet = "";
+    switch (msg->state) {
+      case MRMState::NONE:
+        text = "NONE";
+        style_sheet = "background-color: #00FF00;";  // green
+        break;
+
+      case MRMState::MRM_OPERATING:
+        text = "MRM_OPERATING";
+        style_sheet = "background-color: #FFA500;";  // orange
+        break;
+
+      case MRMState::MRM_SUCCEEDED:
+        text = "MRM_SUCCEEDED";
+        style_sheet = "background-color: #FFFF00;";  // yellow
+        break;
+
+      case MRMState::MRM_FAILED:
+        text = "MRM_FAILED";
+        style_sheet = "background-color: #FF0000;";  // red
+        break;
+
+      default:
+        text = "UNKNOWN";
+        style_sheet = "background-color: #FF0000;";  // red
+        break;
+    }
+
+    updateLabel(mrm_state_label_ptr_, text, style_sheet);
+  }
+
+  // behavior
+  {
+    QString text = "";
+    QString style_sheet = "";
+    switch (msg->state) {
+      case MRMState::NONE:
+        text = "NONE";
+        style_sheet = "background-color: #00FF00;";  // green
+        break;
+
+      case MRMState::COMFORTABLE_STOP:
+        text = "COMFORTABLE_STOP";
+        style_sheet = "background-color: #FFFF00;";  // yellow
+        break;
+
+      case MRMState::EMERGENCY_STOP:
+        text = "EMERGENCY_STOP";
+        style_sheet = "background-color: #FFA500;";  // orange
+        break;
+
+      default:
+        text = "UNKNOWN";
+        style_sheet = "background-color: #FF0000;";  // red
+        break;
+    }
+
+    updateLabel(mrm_behavior_label_ptr_, text, style_sheet);
+  }
+}
+
 void AutowareStatePanel::onShift(
   const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg)
 {

common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp

@@ -27,6 +27,7 @@
 
 #include <autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>
 #include <autoware_adapi_v1_msgs/msg/motion_state.hpp>
+#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
 #include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
 #include <autoware_adapi_v1_msgs/msg/route_state.hpp>
 #include <autoware_adapi_v1_msgs/srv/accept_start.hpp>
@@ -51,6 +52,7 @@ class AutowareStatePanel : public rviz_common::Panel
     autoware_adapi_v1_msgs::msg::LocalizationInitializationState;
   using MotionState = autoware_adapi_v1_msgs::msg::MotionState;
   using AcceptStart = autoware_adapi_v1_msgs::srv::AcceptStart;
+  using MRMState = autoware_adapi_v1_msgs::msg::MrmState;
 
   Q_OBJECT
 
@@ -77,6 +79,7 @@ public Q_SLOTS:  // NOLINT for Qt
   QGroupBox * makeRoutingGroup();
   QGroupBox * makeLocalizationGroup();
   QGroupBox * makeMotionGroup();
+  QGroupBox * makeFailSafeGroup();
 
   void onShift(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg);
   void onEmergencyStatus(const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr msg);
@@ -139,6 +142,15 @@ public Q_SLOTS:  // NOLINT for Qt
 
   void onMotion(const MotionState::ConstSharedPtr msg);
 
+  // FailSafe
+  QLabel * mrm_state_label_ptr_{nullptr};
+  QLabel * mrm_behavior_label_ptr_{nullptr};
+
+  rclcpp::Subscription<MRMState>::SharedPtr sub_mrm_;
+
+  void onMRMState(const MRMState::ConstSharedPtr msg);
+
+  // Others
   QPushButton * velocity_limit_button_ptr_;
   QLabel * gear_label_ptr_;