feat: add selector mode and disengage function (autowarefoundation#781)…

… (autowarefoundation#194)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>
Co-authored-by: Tomoya Kimura <tomoya.kimura@tier4.jp>
Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

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<tier4_control_msgs::msg::GateMode>(
     "/control/current_gate_mode", 10, std::bind(&AutowareStatePanel::onGateMode, this, _1));
 
+  sub_selector_mode_ =
+    raw_node_->create_subscription<tier4_control_msgs::msg::ExternalCommandSelectorMode>(
+      "/control/external_cmd_selector/current_selector_mode", 10,
+      std::bind(&AutowareStatePanel::onSelectorMode, this, _1));
+
   sub_autoware_state_ =
     raw_node_->create_subscription<autoware_auto_system_msgs::msg::AutowareState>(
       "/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<tier4_external_api_msgs::srv::Engage::Request>();
-  req->engage = true;
+  req->engage = current_engage_ ? false : true;
 
   RCLCPP_INFO(raw_node_->get_logger(), "client request");
 

common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp

@@ -24,6 +24,7 @@
 
 #include <autoware_auto_system_msgs/msg/autoware_state.hpp>
 #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/engage_status.hpp>
 #include <tier4_external_api_msgs/srv/engage.hpp>
@@ -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<tier4_control_msgs::msg::GateMode>::SharedPtr sub_gate_mode_;
+  rclcpp::Subscription<tier4_control_msgs::msg::ExternalCommandSelectorMode>::SharedPtr
+    sub_selector_mode_;
   rclcpp::Subscription<autoware_auto_system_msgs::msg::AutowareState>::SharedPtr
     sub_autoware_state_;
   rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::GearReport>::SharedPtr sub_gear_;
@@ -57,10 +62,13 @@ public Q_SLOTS:
   rclcpp::Client<tier4_external_api_msgs::srv::Engage>::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