feat(tier4_system_rviz_plugin): improve visualization results and log…

…ics (#5222) * Add Init Localization and Init Planning Check; Add error list check Signed-off-by: Owen-Liuyuxuan <[email protected]> * style(pre-commit): autofix * int casting problem updated Signed-off-by: Owen-Liuyuxuan <[email protected]> * style(pre-commit): autofix * improve if statement writing styles Signed-off-by: Owen-Liuyuxuan <[email protected]> * style(pre-commit): autofix * FIX ci Signed-off-by: Owen-Liuyuxuan <[email protected]> * subscribe to ADAPI for more stable status checking Signed-off-by: Owen-Liuyuxuan <[email protected]> * style(pre-commit): autofix * try using level attribute only Signed-off-by: Owen-Liuyuxuan <[email protected]> * fix hpp so that it just look like the original hpp Signed-off-by: Owen-Liuyuxuan <[email protected]> * style(pre-commit): autofix * set default value to NO_FAULT Signed-off-by: Owen-Liuyuxuan <[email protected]> --------- Signed-off-by: Owen-Liuyuxuan <[email protected]> Co-authored-by: Owen-Liuyuxuan <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Oct 12, 2023 · 4ba8923 · 4ba8923
1 parent d5709ab
commit 4ba8923
Show file tree

Hide file tree

Showing 2 changed files with 40 additions and 10 deletions.
diff --git a/common/tier4_system_rviz_plugin/README.md b/common/tier4_system_rviz_plugin/README.md
@@ -1 +1,11 @@
 # tier4_system_rviz_plugin
+
+## Purpose
+
+This plugin display the Hazard information from Autoware; and output notices when emergencies are from initial localization and route setting.
+
+## Input
+
+| Name                              | Type                                                  | Description                                                  |
+| --------------------------------- | ----------------------------------------------------- | ------------------------------------------------------------ |
+| `/system/emergency/hazard_status` | `autoware_auto_system_msgs::msg::HazardStatusStamped` | The topic represents the emergency information from Autoware |
diff --git a/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp
@@ -50,6 +50,7 @@
 #include <QPainter>
 #include <rviz_common/uniform_string_stream.hpp>
 
+#include <autoware_auto_system_msgs/msg/hazard_status_stamped.hpp>
 #include <diagnostic_msgs/msg/diagnostic_status.hpp>
 
 #include <X11/Xlib.h>
@@ -126,6 +127,7 @@ MrmSummaryOverlayDisplay::~MrmSummaryOverlayDisplay()
 void MrmSummaryOverlayDisplay::onInitialize()
 {
   RTDClass::onInitialize();
+
   static int count = 0;
   rviz_common::UniformStringStream ss;
   ss << "MrmSummaryOverlayDisplayObject" << count++;
@@ -160,9 +162,11 @@ void MrmSummaryOverlayDisplay::update(float wall_dt, float ros_dt)
   // MRM summary
   std::vector<std::string> mrm_comfortable_stop_summary_list = {};
   std::vector<std::string> mrm_emergency_stop_summary_list = {};
+  int hazard_level = autoware_auto_system_msgs::msg::HazardStatus::NO_FAULT;
   {
     std::lock_guard<std::mutex> message_lock(mutex_);
     if (last_msg_ptr_) {
+      hazard_level = last_msg_ptr_->status.level;
       for (const auto & diag_status : last_msg_ptr_->status.diag_latent_fault) {
         const std::optional<std::string> msg = generateMrmMessage(diag_status);
         if (msg != std::nullopt) {
@@ -201,16 +205,32 @@ void MrmSummaryOverlayDisplay::update(float wall_dt, float ros_dt)
   painter.setFont(font);
 
   std::ostringstream output_text;
-  output_text << std::fixed
-              << "Comfortable Stop MRM Summary: " << int(mrm_comfortable_stop_summary_list.size())
-              << std::endl;
-  for (const auto & mrm_element : mrm_comfortable_stop_summary_list) {
-    output_text << mrm_element << std::endl;
-  }
-  output_text << "Emergency Stop MRM Summary: " << int(mrm_emergency_stop_summary_list.size())
-              << std::endl;
-  for (const auto & mrm_element : mrm_emergency_stop_summary_list) {
-    output_text << mrm_element << std::endl;
+
+  // Display the MRM Summary only when there is a fault
+  if (hazard_level != autoware_auto_system_msgs::msg::HazardStatus::NO_FAULT) {
+    // Broadcasting the Basic Error Infos
+    int number_of_comfortable_stop_messages =
+      static_cast<int>(mrm_comfortable_stop_summary_list.size());
+    if (number_of_comfortable_stop_messages > 0)  // Only Display when there are errors
+    {
+      output_text << std::fixed
+                  << "Comfortable Stop MRM Summary: " << number_of_comfortable_stop_messages
+                  << std::endl;
+      for (const auto & mrm_element : mrm_comfortable_stop_summary_list) {
+        output_text << mrm_element << std::endl;
+      }
+    }
+
+    int number_of_emergency_stop_messages =
+      static_cast<int>(mrm_emergency_stop_summary_list.size());
+    if (number_of_emergency_stop_messages > 0)  // Only Display when there are some errors
+    {
+      output_text << "Emergency Stop MRM Summary: " << int(mrm_emergency_stop_summary_list.size())
+                  << std::endl;
+      for (const auto & mrm_element : mrm_emergency_stop_summary_list) {
+        output_text << mrm_element << std::endl;
+      }
+    }
   }
 
   // same as above, but align on right side