feat(rtc_auto_mode_manager)!: add auto mode status array (autowarefou…

…ndation#2517)

* feat(rtc_aut_mode_manager)!: add auto mode status array

Signed-off-by: taikitanaka3 <ttatcoder@outlook.jp>

* chore:  planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp

Signed-off-by: taikitanaka3 <ttatcoder@outlook.jp>

planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/node.hpp

@@ -18,18 +18,24 @@
 #include "rclcpp/rclcpp.hpp"
 #include "rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp"
 
+#include "tier4_rtc_msgs/msg/auto_mode_status_array.hpp"
+
 #include <memory>
 #include <string>
 #include <vector>
 
 namespace rtc_auto_mode_manager
 {
+using tier4_rtc_msgs::msg::AutoModeStatusArray;
 class RTCAutoModeManagerNode : public rclcpp::Node
 {
 public:
   explicit RTCAutoModeManagerNode(const rclcpp::NodeOptions & node_options);
 
 private:
+  rclcpp::TimerBase::SharedPtr timer_;
+  AutoModeStatusArray auto_mode_statuses_;
+  rclcpp::Publisher<AutoModeStatusArray>::SharedPtr statuses_pub_;
   std::vector<std::shared_ptr<RTCAutoModeManagerInterface>> managers_;
 };
 

planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp

@@ -17,6 +17,8 @@
 
 #include "rclcpp/rclcpp.hpp"
 
+#include "tier4_rtc_msgs/msg/auto_mode_status.hpp"
+#include "tier4_rtc_msgs/msg/module.hpp"
 #include "tier4_rtc_msgs/srv/auto_mode.hpp"
 
 #include <memory>
@@ -25,19 +27,22 @@
 
 namespace rtc_auto_mode_manager
 {
+using tier4_rtc_msgs::msg::AutoModeStatus;
+using tier4_rtc_msgs::msg::Module;
 using tier4_rtc_msgs::srv::AutoMode;
 
 class RTCAutoModeManagerInterface
 {
 public:
   RTCAutoModeManagerInterface(
     rclcpp::Node * node, const std::string & module_name, const bool default_enable);
+  AutoModeStatus getAutoModeStatus() const { return auto_mode_status_; }
 
 private:
   void onEnableService(
-    const AutoMode::Request::SharedPtr request, const AutoMode::Response::SharedPtr response) const;
+    const AutoMode::Request::SharedPtr request, const AutoMode::Response::SharedPtr response);
   AutoMode::Request createRequest(const AutoMode::Request::SharedPtr request) const;
-
+  AutoModeStatus auto_mode_status_;
   rclcpp::Client<AutoMode>::SharedPtr enable_cli_;
   rclcpp::Service<AutoMode>::SharedPtr enable_srv_;
 

planning/rtc_auto_mode_manager/src/node.cpp

@@ -32,6 +32,17 @@ RTCAutoModeManagerNode::RTCAutoModeManagerNode(const rclcpp::NodeOptions & node_
       std::count(default_enable_list.begin(), default_enable_list.end(), module_name) != 0;
     managers_.push_back(std::make_shared<RTCAutoModeManagerInterface>(this, module_name, enabled));
   }
+  statuses_pub_ = create_publisher<AutoModeStatusArray>("output/auto_mode_statuses", 1);
+  using std::chrono_literals::operator""ms;
+  timer_ = rclcpp::create_timer(this, get_clock(), 100ms, [this] {
+    AutoModeStatusArray auto_mode_statuses;
+    for (const auto & m : managers_) {
+      auto_mode_statuses.stamp = get_clock()->now();
+      auto_mode_statuses.statuses.emplace_back(m->getAutoModeStatus());
+    }
+    statuses_pub_->publish(auto_mode_statuses);
+    auto_mode_statuses.statuses.clear();
+  });
 }
 
 }  // namespace rtc_auto_mode_manager

planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp

@@ -16,6 +16,43 @@
 
 namespace rtc_auto_mode_manager
 {
+Module getModuleType(const std::string & module_name)
+{
+  Module module;
+  if (module_name == "blind_spot") {
+    module.type = Module::BLIND_SPOT;
+  } else if (module_name == "crosswalk") {
+    module.type = Module::CROSSWALK;
+  } else if (module_name == "detection_area") {
+    module.type = Module::DETECTION_AREA;
+  } else if (module_name == "intersection") {
+    module.type = Module::INTERSECTION;
+  } else if (module_name == "no_stopping_area") {
+    module.type = Module::NO_STOPPING_AREA;
+  } else if (module_name == "occlusion_spot") {
+    module.type = Module::OCCLUSION_SPOT;
+  } else if (module_name == "traffic_light") {
+    module.type = Module::TRAFFIC_LIGHT;
+  } else if (module_name == "virtual_traffic_light") {
+    module.type = Module::TRAFFIC_LIGHT;
+  } else if (module_name == "lane_change_left") {
+    module.type = Module::LANE_CHANGE_LEFT;
+  } else if (module_name == "lane_change_right") {
+    module.type = Module::LANE_CHANGE_RIGHT;
+  } else if (module_name == "avoidance_left") {
+    module.type = Module::AVOIDANCE_LEFT;
+  } else if (module_name == "avoidance_right") {
+    module.type = Module::AVOIDANCE_RIGHT;
+  } else if (module_name == "pull_over") {
+    module.type = Module::PULL_OVER;
+  } else if (module_name == "pull_out") {
+    module.type = Module::PULL_OUT;
+  } else {
+    module.type = Module::NONE;
+  }
+  return module;
+}
+
 RTCAutoModeManagerInterface::RTCAutoModeManagerInterface(
   rclcpp::Node * node, const std::string & module_name, const bool default_enable)
 {
@@ -41,17 +78,20 @@ RTCAutoModeManagerInterface::RTCAutoModeManagerInterface(
     enable_auto_mode_namespace_ + "/" + module_name,
     std::bind(&RTCAutoModeManagerInterface::onEnableService, this, _1, _2));
 
+  auto_mode_status_.module = getModuleType(module_name);
   // Send enable auto mode request
   if (default_enable) {
+    auto_mode_status_.is_auto_mode = true;
     AutoMode::Request::SharedPtr request = std::make_shared<AutoMode::Request>();
     request->enable = true;
     enable_cli_->async_send_request(request);
   }
 }
 
 void RTCAutoModeManagerInterface::onEnableService(
-  const AutoMode::Request::SharedPtr request, const AutoMode::Response::SharedPtr response) const
+  const AutoMode::Request::SharedPtr request, const AutoMode::Response::SharedPtr response)
 {
+  auto_mode_status_.is_auto_mode = request->enable;
   enable_cli_->async_send_request(request);
   response->success = true;
 }