diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp index 3a588d857ff15..8b0903e55f343 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp +++ b/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( "/api/external/get/engage", 10, std::bind(&AutowareStatePanel::onEngageStatus, this, _1)); + sub_emergency_ = raw_node_->create_subscription( + "/api/autoware/get/emergency", 10, std::bind(&AutowareStatePanel::onEmergencyStatus, this, _1)); + client_engage_ = raw_node_->create_client( "/api/external/set/engage", rmw_qos_profile_services_default); + client_emergency_stop_ = raw_node_->create_client( + "/api/autoware/set/emergency", rmw_qos_profile_services_default); + pub_velocity_limit_ = raw_node_->create_publisher( "/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(); @@ -281,6 +305,29 @@ void AutowareStatePanel::onClickAutowareEngage() }); } +void AutowareStatePanel::onClickEmergencyButton() +{ + auto request = std::make_shared(); + 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::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" diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp index ecc3f343715d8..1833d75e422ae 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp @@ -27,8 +27,10 @@ #include #include #include +#include #include #include +#include #include #include @@ -47,6 +49,7 @@ public Q_SLOTS: void onClickVelocityLimit(); void onClickGateMode(); void onClickPathChangeApproval(); + void onClickEmergencyButton(); protected: void onGateMode(const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg); @@ -54,6 +57,7 @@ public Q_SLOTS: 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::SharedPtr sub_engage_; rclcpp::Client::SharedPtr client_engage_; + rclcpp::Client::SharedPtr client_emergency_stop_; + rclcpp::Subscription::SharedPtr sub_emergency_; rclcpp::Publisher::SharedPtr pub_velocity_limit_; rclcpp::Publisher::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