feat(tier4_state_rviz_plugin): add emergency button (#1048)

* feat(tier4_state_rviz_plugin):add emergency button

Signed-off-by: Shumpei Wakabayashi <shumpei.wakabayashi@tier4.jp>

* ci(pre-commit): autofix

* chore: add default button name

Signed-off-by: Shumpei Wakabayashi <shumpei.wakabayashi@tier4.jp>

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

@@ -97,6 +97,10 @@ AutowareStatePanel::AutowareStatePanel(QWidget * parent) : rviz_common::Panel(pa
   pub_velocity_limit_input_->setSingleStep(5.0);
   connect(velocity_limit_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickVelocityLimit()));
 
+  // Emergency Button
+  emergency_button_ptr_ = new QPushButton("Set Emergency");
+  connect(emergency_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickEmergencyButton()));
+
   // Layout
   auto * v_layout = new QVBoxLayout;
   auto * gate_mode_path_change_approval_layout = new QHBoxLayout;
@@ -114,6 +118,7 @@ AutowareStatePanel::AutowareStatePanel(QWidget * parent) : rviz_common::Panel(pa
   velocity_limit_layout->addWidget(velocity_limit_button_ptr_);
   velocity_limit_layout->addWidget(pub_velocity_limit_input_);
   velocity_limit_layout->addWidget(new QLabel("  [km/h]"));
+  velocity_limit_layout->addWidget(emergency_button_ptr_);
   v_layout->addLayout(velocity_limit_layout);
   setLayout(v_layout);
 }
@@ -140,9 +145,15 @@ void AutowareStatePanel::onInitialize()
   sub_engage_ = raw_node_->create_subscription<tier4_external_api_msgs::msg::EngageStatus>(
     "/api/external/get/engage", 10, std::bind(&AutowareStatePanel::onEngageStatus, this, _1));
 
+  sub_emergency_ = raw_node_->create_subscription<tier4_external_api_msgs::msg::Emergency>(
+    "/api/autoware/get/emergency", 10, std::bind(&AutowareStatePanel::onEmergencyStatus, this, _1));
+
   client_engage_ = raw_node_->create_client<tier4_external_api_msgs::srv::Engage>(
     "/api/external/set/engage", rmw_qos_profile_services_default);
 
+  client_emergency_stop_ = raw_node_->create_client<tier4_external_api_msgs::srv::SetEmergency>(
+    "/api/autoware/set/emergency", rmw_qos_profile_services_default);
+
   pub_velocity_limit_ = raw_node_->create_publisher<tier4_planning_msgs::msg::VelocityLimit>(
     "/planning/scenario_planning/max_velocity_default", rclcpp::QoS(1));
 
@@ -254,6 +265,19 @@ void AutowareStatePanel::onEngageStatus(
   engage_status_label_ptr_->setText(QString::fromStdString(Bool2String(current_engage_)));
 }
 
+void AutowareStatePanel::onEmergencyStatus(
+  const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr msg)
+{
+  current_emergency_ = msg->emergency;
+  if (msg->emergency) {
+    emergency_button_ptr_->setText(QString::fromStdString("Clear Emergency"));
+    emergency_button_ptr_->setStyleSheet("background-color: #FF0000;");
+  } else {
+    emergency_button_ptr_->setText(QString::fromStdString("Set Emergency"));
+    emergency_button_ptr_->setStyleSheet("background-color: #00FF00;");
+  }
+}
+
 void AutowareStatePanel::onClickVelocityLimit()
 {
   auto velocity_limit = std::make_shared<tier4_planning_msgs::msg::VelocityLimit>();
@@ -281,6 +305,29 @@ void AutowareStatePanel::onClickAutowareEngage()
     });
 }
 
+void AutowareStatePanel::onClickEmergencyButton()
+{
+  auto request = std::make_shared<tier4_external_api_msgs::srv::SetEmergency::Request>();
+  if (current_emergency_) {
+    request->emergency = false;
+  } else {
+    request->emergency = true;
+  }
+  RCLCPP_INFO(raw_node_->get_logger(), request->emergency ? "Set Emergency" : "Clear Emergency");
+
+  client_emergency_stop_->async_send_request(
+    request,
+    [this]([[maybe_unused]] rclcpp::Client<tier4_external_api_msgs::srv::SetEmergency>::SharedFuture
+             result) {
+      auto response = result.get();
+      if (response->status.code == tier4_external_api_msgs::msg::ResponseStatus::SUCCESS) {
+        RCLCPP_INFO(raw_node_->get_logger(), "service succeeded");
+      } else {
+        RCLCPP_WARN(
+          raw_node_->get_logger(), "service failed: %s", response->status.message.c_str());
+      }
+    });
+}
 void AutowareStatePanel::onClickGateMode()
 {
   const auto data = gate_mode_label_ptr_->text().toStdString() == "AUTO"

common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp

@@ -27,8 +27,10 @@
 #include <autoware_auto_vehicle_msgs/msg/gear_report.hpp>
 #include <tier4_control_msgs/msg/external_command_selector_mode.hpp>
 #include <tier4_control_msgs/msg/gate_mode.hpp>
+#include <tier4_external_api_msgs/msg/emergency.hpp>
 #include <tier4_external_api_msgs/msg/engage_status.hpp>
 #include <tier4_external_api_msgs/srv/engage.hpp>
+#include <tier4_external_api_msgs/srv/set_emergency.hpp>
 #include <tier4_planning_msgs/msg/approval.hpp>
 #include <tier4_planning_msgs/msg/velocity_limit.hpp>
 
@@ -47,13 +49,15 @@ public Q_SLOTS:
   void onClickVelocityLimit();
   void onClickGateMode();
   void onClickPathChangeApproval();
+  void onClickEmergencyButton();
 
 protected:
   void onGateMode(const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg);
   void onSelectorMode(
     const tier4_control_msgs::msg::ExternalCommandSelectorMode::ConstSharedPtr msg);
   void onAutowareState(const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr msg);
   void onShift(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg);
+  void onEmergencyStatus(const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr msg);
   void onEngageStatus(const tier4_external_api_msgs::msg::EngageStatus::ConstSharedPtr msg);
 
   rclcpp::Node::SharedPtr raw_node_;
@@ -66,6 +70,8 @@ public Q_SLOTS:
   rclcpp::Subscription<tier4_external_api_msgs::msg::EngageStatus>::SharedPtr sub_engage_;
 
   rclcpp::Client<tier4_external_api_msgs::srv::Engage>::SharedPtr client_engage_;
+  rclcpp::Client<tier4_external_api_msgs::srv::SetEmergency>::SharedPtr client_emergency_stop_;
+  rclcpp::Subscription<tier4_external_api_msgs::msg::Emergency>::SharedPtr sub_emergency_;
 
   rclcpp::Publisher<tier4_planning_msgs::msg::VelocityLimit>::SharedPtr pub_velocity_limit_;
   rclcpp::Publisher<tier4_control_msgs::msg::GateMode>::SharedPtr pub_gate_mode_;
@@ -81,8 +87,10 @@ public Q_SLOTS:
   QPushButton * gate_mode_button_ptr_;
   QPushButton * path_change_approval_button_ptr_;
   QSpinBox * pub_velocity_limit_input_;
+  QPushButton * emergency_button_ptr_;
 
   bool current_engage_;
+  bool current_emergency_;
 };
 
 }  // namespace rviz_plugins