From 980a84ec3b5d40b9620093830707f32d997982af Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Wed, 28 Dec 2022 11:34:51 +0900 Subject: [PATCH] feat(rtc_auto_mode_manager)!: add auto mode status array (#2517) * feat(rtc_aut_mode_manager)!: add auto mode status array Signed-off-by: taikitanaka3 * chore: planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp Signed-off-by: taikitanaka3 --- .../include/rtc_auto_mode_manager/node.hpp | 6 +++ .../rtc_auto_mode_manager_interface.hpp | 9 +++- planning/rtc_auto_mode_manager/src/node.cpp | 11 +++++ .../src/rtc_auto_mode_manager_interface.cpp | 42 ++++++++++++++++++- 4 files changed, 65 insertions(+), 3 deletions(-) diff --git a/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/node.hpp b/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/node.hpp index adb6e85e463a..d36974508d2d 100644 --- a/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/node.hpp +++ b/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 #include #include 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::SharedPtr statuses_pub_; std::vector> managers_; }; diff --git a/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp b/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp index 0a9b8add0e0e..098852c39716 100644 --- a/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp +++ b/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 @@ -25,6 +27,8 @@ 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 @@ -32,12 +36,13 @@ 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::SharedPtr enable_cli_; rclcpp::Service::SharedPtr enable_srv_; diff --git a/planning/rtc_auto_mode_manager/src/node.cpp b/planning/rtc_auto_mode_manager/src/node.cpp index 98a44f97b9ee..a37987c2efe3 100644 --- a/planning/rtc_auto_mode_manager/src/node.cpp +++ b/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(this, module_name, enabled)); } + statuses_pub_ = create_publisher("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 diff --git a/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp b/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp index 5cdc0f7dce38..9ac306534e62 100644 --- a/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp +++ b/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,8 +78,10 @@ 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(); request->enable = true; enable_cli_->async_send_request(request); @@ -50,8 +89,9 @@ RTCAutoModeManagerInterface::RTCAutoModeManagerInterface( } 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; }