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 b86227de49272..d93c88e381b14 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp @@ -40,6 +40,15 @@ AutowareStatePanel::AutowareStatePanel(QWidget * parent) : rviz_common::Panel(pa gate_layout->addWidget(gate_prefix_label_ptr); gate_layout->addWidget(gate_mode_label_ptr_); + // Selector Mode + auto * selector_prefix_label_ptr = new QLabel("SELECT: "); + selector_prefix_label_ptr->setAlignment(Qt::AlignRight); + selector_mode_label_ptr_ = new QLabel("INIT"); + selector_mode_label_ptr_->setAlignment(Qt::AlignCenter); + auto * selector_layout = new QHBoxLayout; + selector_layout->addWidget(selector_prefix_label_ptr); + selector_layout->addWidget(selector_mode_label_ptr_); + // State auto * state_prefix_label_ptr = new QLabel("STATE: "); state_prefix_label_ptr->setAlignment(Qt::AlignRight); @@ -73,6 +82,7 @@ AutowareStatePanel::AutowareStatePanel(QWidget * parent) : rviz_common::Panel(pa auto * v_layout = new QVBoxLayout; v_layout->addLayout(gate_layout); + v_layout->addLayout(selector_layout); v_layout->addLayout(state_layout); v_layout->addLayout(gear_layout); v_layout->addLayout(engage_status_layout); @@ -87,6 +97,11 @@ void AutowareStatePanel::onInitialize() sub_gate_mode_ = raw_node_->create_subscription( "/control/current_gate_mode", 10, std::bind(&AutowareStatePanel::onGateMode, this, _1)); + sub_selector_mode_ = + raw_node_->create_subscription( + "/control/external_cmd_selector/current_selector_mode", 10, + std::bind(&AutowareStatePanel::onSelectorMode, this, _1)); + sub_autoware_state_ = raw_node_->create_subscription( "/autoware/state", 10, std::bind(&AutowareStatePanel::onAutowareState, this, _1)); @@ -121,6 +136,32 @@ void AutowareStatePanel::onGateMode(const tier4_control_msgs::msg::GateMode::Con } } +void AutowareStatePanel::onSelectorMode( + const tier4_control_msgs::msg::ExternalCommandSelectorMode::ConstSharedPtr msg) +{ + switch (msg->data) { + case tier4_control_msgs::msg::ExternalCommandSelectorMode::REMOTE: + selector_mode_label_ptr_->setText("REMOTE"); + selector_mode_label_ptr_->setStyleSheet("background-color: #00FF00;"); + break; + + case tier4_control_msgs::msg::ExternalCommandSelectorMode::LOCAL: + selector_mode_label_ptr_->setText("LOCAL"); + selector_mode_label_ptr_->setStyleSheet("background-color: #FFFF00;"); + break; + + case tier4_control_msgs::msg::ExternalCommandSelectorMode::NONE: + selector_mode_label_ptr_->setText("NONE"); + selector_mode_label_ptr_->setStyleSheet("background-color: #FF0000;"); + break; + + default: + selector_mode_label_ptr_->setText("UNKNOWN"); + selector_mode_label_ptr_->setStyleSheet("background-color: #FF0000;"); + break; + } +} + void AutowareStatePanel::onAutowareState( const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr msg) { @@ -170,13 +211,14 @@ void AutowareStatePanel::onShift( void AutowareStatePanel::onEngageStatus( const tier4_external_api_msgs::msg::EngageStatus::ConstSharedPtr msg) { - engage_status_label_ptr_->setText(QString::fromStdString(Bool2String(msg->engage))); + current_engage_ = msg->engage; + engage_status_label_ptr_->setText(QString::fromStdString(Bool2String(current_engage_))); } void AutowareStatePanel::onClickAutowareEngage() { auto req = std::make_shared(); - req->engage = true; + req->engage = current_engage_ ? false : true; RCLCPP_INFO(raw_node_->get_logger(), "client request"); 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 58ef0305f00dd..af562dd055f7c 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp @@ -24,6 +24,7 @@ #include #include +#include #include #include #include @@ -43,12 +44,16 @@ public Q_SLOTS: 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 onEngageStatus(const tier4_external_api_msgs::msg::EngageStatus::ConstSharedPtr msg); rclcpp::Node::SharedPtr raw_node_; rclcpp::Subscription::SharedPtr sub_gate_mode_; + rclcpp::Subscription::SharedPtr + sub_selector_mode_; rclcpp::Subscription::SharedPtr sub_autoware_state_; rclcpp::Subscription::SharedPtr sub_gear_; @@ -57,10 +62,13 @@ public Q_SLOTS: rclcpp::Client::SharedPtr client_engage_; QLabel * gate_mode_label_ptr_; + QLabel * selector_mode_label_ptr_; QLabel * autoware_state_label_ptr_; QLabel * gear_label_ptr_; QLabel * engage_status_label_ptr_; QPushButton * engage_button_ptr_; + + bool current_engage_; }; } // namespace rviz_plugins