feat: add support auto mode status (#111)

* feat: add support auto mode status

Signed-off-by: tkhmy <tkh.my.p@gmail.com>

* style(pre-commit): autofix

---------

Signed-off-by: tkhmy <tkh.my.p@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

Author: tkhmy

autoware_iv_external_api_adaptor/src/rtc_controller.cpp

@@ -27,6 +27,10 @@ RTCModule::RTCModule(rclcpp::Node * node, const std::string & name)
     cooperate_status_namespace_ + "/" + name, rclcpp::QoS(1),
     std::bind(&RTCModule::moduleCallback, this, _1));
 
+  auto_mode_sub_ = node->create_subscription<AutoModeStatus>(
+    auto_mode_status_namespace_ + "/" + name, rclcpp::QoS(1),
+    std::bind(&RTCModule::autoModeCallback, this, _1));
+
   cli_set_module_ = proxy.create_client<CooperateCommands>(
     cooperate_commands_namespace_ + "/" + name, rmw_qos_profile_services_default);
 
@@ -39,12 +43,23 @@ void RTCModule::moduleCallback(const CooperateStatusArray::ConstSharedPtr messag
   module_statuses_ = message->statuses;
 }
 
+void RTCModule::autoModeCallback(const AutoModeStatus::ConstSharedPtr message)
+{
+  auto_mode_status_.module = message->module;
+  auto_mode_status_.is_auto_mode = message->is_auto_mode;
+}
+
 void RTCModule::insertMessage(std::vector<CooperateStatus> & cooperate_statuses)
 {
   cooperate_statuses.insert(
     cooperate_statuses.end(), module_statuses_.begin(), module_statuses_.end());
 }
 
+void RTCModule::insertAutoModeMessage(std::vector<AutoModeStatus> & auto_mode_status)
+{
+  auto_mode_status.insert(auto_mode_status.begin(), auto_mode_status_);
+}
+
 void RTCModule::callService(
   CooperateCommands::Request::SharedPtr request,
   const CooperateCommands::Response::SharedPtr & responses)
@@ -101,6 +116,8 @@ RTCController::RTCController(const rclcpp::NodeOptions & options)
 
   rtc_status_pub_ =
     create_publisher<CooperateStatusArray>("/api/external/get/rtc_status", rclcpp::QoS(1));
+  auto_mode_pub_ =
+    create_publisher<AutoModeStatusArray>("/api/external/get/rtc_auto_mode", rclcpp::QoS(1));
 
   group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
   srv_set_rtc_ = proxy.create_service<CooperateCommands>(
@@ -111,6 +128,8 @@ RTCController::RTCController(const rclcpp::NodeOptions & options)
     rmw_qos_profile_services_default, group_);
 
   timer_ = rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&RTCController::onTimer, this));
+  auto_mode_timer_ = rclcpp::create_timer(
+    this, get_clock(), 100ms, std::bind(&RTCController::onAutoModeTimer, this));
 }
 
 void RTCController::insertionSortAndValidation(std::vector<CooperateStatus> & statuses_vector)
@@ -259,6 +278,35 @@ void RTCController::setRTC(
   }
 }
 
+void RTCController::onAutoModeTimer()
+{
+  std::vector<AutoModeStatus> auto_mode_statuses;
+  blind_spot_->insertAutoModeMessage(auto_mode_statuses);
+  crosswalk_->insertAutoModeMessage(auto_mode_statuses);
+  detection_area_->insertAutoModeMessage(auto_mode_statuses);
+  intersection_->insertAutoModeMessage(auto_mode_statuses);
+  intersection_occlusion_->insertAutoModeMessage(auto_mode_statuses);
+  no_stopping_area_->insertAutoModeMessage(auto_mode_statuses);
+  occlusion_spot_->insertAutoModeMessage(auto_mode_statuses);
+  traffic_light_->insertAutoModeMessage(auto_mode_statuses);
+  virtual_traffic_light_->insertAutoModeMessage(auto_mode_statuses);
+  lane_change_left_->insertAutoModeMessage(auto_mode_statuses);
+  lane_change_right_->insertAutoModeMessage(auto_mode_statuses);
+  ext_request_lane_change_left_->insertAutoModeMessage(auto_mode_statuses);
+  ext_request_lane_change_right_->insertAutoModeMessage(auto_mode_statuses);
+  avoidance_left_->insertAutoModeMessage(auto_mode_statuses);
+  avoidance_right_->insertAutoModeMessage(auto_mode_statuses);
+  avoidance_by_lc_left_->insertAutoModeMessage(auto_mode_statuses);
+  avoidance_by_lc_right_->insertAutoModeMessage(auto_mode_statuses);
+  goal_planner_->insertAutoModeMessage(auto_mode_statuses);
+  start_planner_->insertAutoModeMessage(auto_mode_statuses);
+
+  AutoModeStatusArray msg;
+  msg.stamp = now();
+  msg.statuses = auto_mode_statuses;
+  auto_mode_pub_->publish(msg);
+}
+
 void RTCController::setRTCAutoMode(
   const AutoModeWithModule::Request::SharedPtr request,
   const AutoModeWithModule::Response::SharedPtr response)

autoware_iv_external_api_adaptor/src/rtc_controller.hpp

@@ -18,6 +18,8 @@
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_api_utils/tier4_api_utils.hpp>
 
+#include "tier4_rtc_msgs/msg/auto_mode_status.hpp"
+#include "tier4_rtc_msgs/msg/auto_mode_status_array.hpp"
 #include "tier4_rtc_msgs/msg/cooperate_command.hpp"
 #include "tier4_rtc_msgs/msg/cooperate_status.hpp"
 #include "tier4_rtc_msgs/msg/cooperate_status_array.hpp"
@@ -33,6 +35,8 @@
 using CooperateCommands = tier4_rtc_msgs::srv::CooperateCommands;
 using AutoMode = tier4_rtc_msgs::srv::AutoMode;
 using AutoModeWithModule = tier4_rtc_msgs::srv::AutoModeWithModule;
+using AutoModeStatusArray = tier4_rtc_msgs::msg::AutoModeStatusArray;
+using AutoModeStatus = tier4_rtc_msgs::msg::AutoModeStatus;
 using CooperateStatusArray = tier4_rtc_msgs::msg::CooperateStatusArray;
 using CooperateStatus = tier4_rtc_msgs::msg::CooperateStatus;
 using Module = tier4_rtc_msgs::msg::Module;
@@ -42,15 +46,20 @@ class RTCModule
 public:
   std::string cooperate_status_namespace_ = "/planning/cooperate_status";
   std::string cooperate_commands_namespace_ = "/planning/cooperate_commands";
+  std::string auto_mode_status_namespace_ = "/planning/auto_mode_status";
   std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode";
   std::vector<CooperateStatus> module_statuses_;
+  AutoModeStatus auto_mode_status_;
   rclcpp::Subscription<CooperateStatusArray>::SharedPtr module_sub_;
+  rclcpp::Subscription<AutoModeStatus>::SharedPtr auto_mode_sub_;
   tier4_api_utils::Client<CooperateCommands>::SharedPtr cli_set_module_;
   tier4_api_utils::Client<AutoMode>::SharedPtr cli_set_auto_mode_;
 
   RTCModule(rclcpp::Node * node, const std::string & name);
   void moduleCallback(const CooperateStatusArray::ConstSharedPtr message);
+  void autoModeCallback(const AutoModeStatus::ConstSharedPtr message);
   void insertMessage(std::vector<CooperateStatus> & cooperate_statuses);
+  void insertAutoModeMessage(std::vector<AutoModeStatus> & auto_mode_status);
   void callService(
     CooperateCommands::Request::SharedPtr request,
     const CooperateCommands::Response::SharedPtr & responses);
@@ -88,13 +97,15 @@ class RTCController : public rclcpp::Node
 
   /* publishers */
   rclcpp::Publisher<CooperateStatusArray>::SharedPtr rtc_status_pub_;
+  rclcpp::Publisher<AutoModeStatusArray>::SharedPtr auto_mode_pub_;
   /* service from external */
   rclcpp::CallbackGroup::SharedPtr group_;
   tier4_api_utils::Service<CooperateCommands>::SharedPtr srv_set_rtc_;
   tier4_api_utils::Service<AutoModeWithModule>::SharedPtr srv_set_rtc_auto_mode_;
 
   /* Timer */
   rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::TimerBase::SharedPtr auto_mode_timer_;
 
   void insertionSortAndValidation(std::vector<CooperateStatus> & statuses_vector);
   void checkInfDistance(CooperateStatus & status);
@@ -108,6 +119,7 @@ class RTCController : public rclcpp::Node
 
   // ros callback
   void onTimer();
+  void onAutoModeTimer();
 };
 
 }  // namespace external_api