From 526aa9b00ce1198afcff387fee83467c53f8707a Mon Sep 17 00:00:00 2001
From: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>
Date: Tue, 12 Dec 2023 18:21:31 +0900
Subject: [PATCH] feat(rtc_interface): add publisher to publish auto mode
 status (#5845)

add publisher to publish auto mode status

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
---
 .../include/rtc_interface/rtc_interface.hpp   |  8 +++++-
 planning/rtc_interface/src/rtc_interface.cpp  | 25 ++++++++++++++++---
 2 files changed, 29 insertions(+), 4 deletions(-)

diff --git a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp
index d6e083421c0b7..912f251c70b20 100644
--- a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp
+++ b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp
@@ -17,6 +17,7 @@
 
 #include "rclcpp/rclcpp.hpp"
 
+#include "tier4_rtc_msgs/msg/auto_mode_status.hpp"
 #include "tier4_rtc_msgs/msg/command.hpp"
 #include "tier4_rtc_msgs/msg/cooperate_command.hpp"
 #include "tier4_rtc_msgs/msg/cooperate_response.hpp"
@@ -33,6 +34,7 @@
 
 namespace rtc_interface
 {
+using tier4_rtc_msgs::msg::AutoModeStatus;
 using tier4_rtc_msgs::msg::Command;
 using tier4_rtc_msgs::msg::CooperateCommand;
 using tier4_rtc_msgs::msg::CooperateResponse;
@@ -64,6 +66,7 @@ class RTCInterface
     const CooperateCommands::Response::SharedPtr responses);
   void onAutoModeService(
     const AutoMode::Request::SharedPtr request, const AutoMode::Response::SharedPtr response);
+  void onTimer();
   std::vector<CooperateResponse> validateCooperateCommands(
     const std::vector<CooperateCommand> & commands);
   void updateCooperateCommandStatus(const std::vector<CooperateCommand> & commands);
@@ -72,18 +75,21 @@ class RTCInterface
   bool isLocked() const;
 
   rclcpp::Publisher<CooperateStatusArray>::SharedPtr pub_statuses_;
+  rclcpp::Publisher<AutoModeStatus>::SharedPtr pub_auto_mode_status_;
   rclcpp::Service<CooperateCommands>::SharedPtr srv_commands_;
   rclcpp::Service<AutoMode>::SharedPtr srv_auto_mode_;
   rclcpp::CallbackGroup::SharedPtr callback_group_;
+  rclcpp::TimerBase::SharedPtr timer_;
   rclcpp::Logger logger_;
 
   Module module_;
   CooperateStatusArray registered_status_;
   std::vector<CooperateCommand> stored_commands_;
-  bool is_auto_mode_init_;
+  bool is_auto_mode_enabled_;
   bool is_locked_;
 
   std::string cooperate_status_namespace_ = "/planning/cooperate_status";
+  std::string auto_mode_status_namespace_ = "/planning/auto_mode_status";
   std::string cooperate_commands_namespace_ = "/planning/cooperate_commands";
   std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode";
 
diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp
index dbc82113d403b..bb6cbecac7971 100644
--- a/planning/rtc_interface/src/rtc_interface.cpp
+++ b/planning/rtc_interface/src/rtc_interface.cpp
@@ -14,6 +14,8 @@
 
 #include "rtc_interface/rtc_interface.hpp"
 
+#include <chrono>
+
 namespace
 {
 using tier4_rtc_msgs::msg::Module;
@@ -80,16 +82,24 @@ namespace rtc_interface
 {
 RTCInterface::RTCInterface(rclcpp::Node * node, const std::string & name, const bool enable_rtc)
 : logger_{node->get_logger().get_child("RTCInterface[" + name + "]")},
-  is_auto_mode_init_{!enable_rtc},
+  is_auto_mode_enabled_{!enable_rtc},
   is_locked_{false}
 {
   using std::placeholders::_1;
   using std::placeholders::_2;
 
+  constexpr double update_rate = 10.0;
+  const auto period_ns = rclcpp::Rate(update_rate).period();
+  timer_ = rclcpp::create_timer(
+    node, node->get_clock(), period_ns, std::bind(&RTCInterface::onTimer, this));
+
   // Publisher
   pub_statuses_ =
     node->create_publisher<CooperateStatusArray>(cooperate_status_namespace_ + "/" + name, 1);
 
+  pub_auto_mode_status_ =
+    node->create_publisher<AutoModeStatus>(auto_mode_status_namespace_ + "/" + name, 1);
+
   // Service
   callback_group_ = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
   srv_commands_ = node->create_service<CooperateCommands>(
@@ -174,13 +184,22 @@ void RTCInterface::onAutoModeService(
   const AutoMode::Request::SharedPtr request, const AutoMode::Response::SharedPtr response)
 {
   std::lock_guard<std::mutex> lock(mutex_);
-  is_auto_mode_init_ = request->enable;
+  is_auto_mode_enabled_ = request->enable;
   for (auto & status : registered_status_.statuses) {
     status.auto_mode = request->enable;
   }
   response->success = true;
 }
 
+void RTCInterface::onTimer()
+{
+  AutoModeStatus auto_mode_status;
+  auto_mode_status.module = module_;
+  auto_mode_status.is_auto_mode = is_auto_mode_enabled_;
+
+  pub_auto_mode_status_->publish(auto_mode_status);
+}
+
 void RTCInterface::updateCooperateStatus(
   const UUID & uuid, const bool safe, const double start_distance, const double finish_distance,
   const rclcpp::Time & stamp)
@@ -201,7 +220,7 @@ void RTCInterface::updateCooperateStatus(
     status.command_status.type = Command::DEACTIVATE;
     status.start_distance = start_distance;
     status.finish_distance = finish_distance;
-    status.auto_mode = is_auto_mode_init_;
+    status.auto_mode = is_auto_mode_enabled_;
     registered_status_.statuses.push_back(status);
     return;
   }