From 1f873b05b30114522ef0e834b242ba5e76addb72 Mon Sep 17 00:00:00 2001
From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com>
Date: Thu, 3 Aug 2023 16:41:29 +0900
Subject: [PATCH] feat(tier4_state_rviz_plugin): add init by gnss button
 (#4392)

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
---
 .../src/autoware_state_panel.cpp                     | 12 ++++++++++++
 .../src/autoware_state_panel.hpp                     |  6 ++++++
 2 files changed, 18 insertions(+)

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 8f4e56c84164e..aa1dcfcd1651d 100644
--- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp
+++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp
@@ -166,6 +166,10 @@ QGroupBox * AutowareStatePanel::makeLocalizationGroup()
   localization_label_ptr_->setStyleSheet("border:1px solid black;");
   grid->addWidget(localization_label_ptr_, 0, 0);
 
+  init_by_gnss_button_ptr_ = new QPushButton("Init by GNSS");
+  connect(init_by_gnss_button_ptr_, SIGNAL(clicked()), SLOT(onClickInitByGnss()));
+  grid->addWidget(init_by_gnss_button_ptr_, 1, 0);
+
   group->setLayout(grid);
   return group;
 }
@@ -249,6 +253,9 @@ void AutowareStatePanel::onInitialize()
     "/api/localization/initialization_state", rclcpp::QoS{1}.transient_local(),
     std::bind(&AutowareStatePanel::onLocalization, this, _1));
 
+  client_init_by_gnss_ =
+    raw_node_->create_client<InitializeLocalization>("/api/localization/initialize");
+
   // Motion
   sub_motion_ = raw_node_->create_subscription<MotionState>(
     "/api/motion/state", rclcpp::QoS{1}.transient_local(),
@@ -587,6 +594,11 @@ void AutowareStatePanel::onClickClearRoute()
   callServiceWithoutResponse<ClearRoute>(client_clear_route_);
 }
 
+void AutowareStatePanel::onClickInitByGnss()
+{
+  callServiceWithoutResponse<InitializeLocalization>(client_init_by_gnss_);
+}
+
 void AutowareStatePanel::onClickAcceptStart()
 {
   callServiceWithoutResponse<AcceptStart>(client_accept_start_);
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 3a9bd6a542401..eba3e4eacd275 100644
--- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp
+++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp
@@ -34,6 +34,7 @@
 #include <autoware_adapi_v1_msgs/srv/accept_start.hpp>
 #include <autoware_adapi_v1_msgs/srv/change_operation_mode.hpp>
 #include <autoware_adapi_v1_msgs/srv/clear_route.hpp>
+#include <autoware_adapi_v1_msgs/srv/initialize_localization.hpp>
 #include <autoware_auto_vehicle_msgs/msg/gear_report.hpp>
 #include <tier4_external_api_msgs/msg/emergency.hpp>
 #include <tier4_external_api_msgs/srv/set_emergency.hpp>
@@ -51,6 +52,7 @@ class AutowareStatePanel : public rviz_common::Panel
   using ClearRoute = autoware_adapi_v1_msgs::srv::ClearRoute;
   using LocalizationInitializationState =
     autoware_adapi_v1_msgs::msg::LocalizationInitializationState;
+  using InitializeLocalization = autoware_adapi_v1_msgs::srv::InitializeLocalization;
   using MotionState = autoware_adapi_v1_msgs::msg::MotionState;
   using AcceptStart = autoware_adapi_v1_msgs::srv::AcceptStart;
   using MRMState = autoware_adapi_v1_msgs::msg::MrmState;
@@ -69,6 +71,7 @@ public Q_SLOTS:  // NOLINT for Qt
   void onClickAutowareControl();
   void onClickDirectControl();
   void onClickClearRoute();
+  void onClickInitByGnss();
   void onClickAcceptStart();
   void onClickVelocityLimit();
   void onClickEmergencyButton();
@@ -130,7 +133,10 @@ public Q_SLOTS:  // NOLINT for Qt
 
   // Localization
   QLabel * localization_label_ptr_{nullptr};
+  QPushButton * init_by_gnss_button_ptr_{nullptr};
+
   rclcpp::Subscription<LocalizationInitializationState>::SharedPtr sub_localization_;
+  rclcpp::Client<InitializeLocalization>::SharedPtr client_init_by_gnss_;
 
   void onLocalization(const LocalizationInitializationState::ConstSharedPtr msg);