From 2f341cc1f785b14043f16ecbe1dd83a6bf5dd443 Mon Sep 17 00:00:00 2001 From: Kah Hooi Tan <41041286+tkhmy@users.noreply.github.com> Date: Wed, 20 Dec 2023 17:27:00 +0900 Subject: [PATCH] feat: add support auto mode status (#111) * feat: add support auto mode status Signed-off-by: tkhmy * style(pre-commit): autofix --------- Signed-off-by: tkhmy Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/rtc_controller.cpp | 48 +++++++++++++++++++ .../src/rtc_controller.hpp | 12 +++++ 2 files changed, 60 insertions(+) diff --git a/autoware_iv_external_api_adaptor/src/rtc_controller.cpp b/autoware_iv_external_api_adaptor/src/rtc_controller.cpp index 7af3204..7934aa3 100644 --- a/autoware_iv_external_api_adaptor/src/rtc_controller.cpp +++ b/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( + auto_mode_status_namespace_ + "/" + name, rclcpp::QoS(1), + std::bind(&RTCModule::autoModeCallback, this, _1)); + cli_set_module_ = proxy.create_client( 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 & cooperate_statuses) { cooperate_statuses.insert( cooperate_statuses.end(), module_statuses_.begin(), module_statuses_.end()); } +void RTCModule::insertAutoModeMessage(std::vector & 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("/api/external/get/rtc_status", rclcpp::QoS(1)); + auto_mode_pub_ = + create_publisher("/api/external/get/rtc_auto_mode", rclcpp::QoS(1)); group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); srv_set_rtc_ = proxy.create_service( @@ -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 & statuses_vector) @@ -259,6 +278,35 @@ void RTCController::setRTC( } } +void RTCController::onAutoModeTimer() +{ + std::vector 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) diff --git a/autoware_iv_external_api_adaptor/src/rtc_controller.hpp b/autoware_iv_external_api_adaptor/src/rtc_controller.hpp index 0838570..fd44fbc 100644 --- a/autoware_iv_external_api_adaptor/src/rtc_controller.hpp +++ b/autoware_iv_external_api_adaptor/src/rtc_controller.hpp @@ -18,6 +18,8 @@ #include #include +#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 module_statuses_; + AutoModeStatus auto_mode_status_; rclcpp::Subscription::SharedPtr module_sub_; + rclcpp::Subscription::SharedPtr auto_mode_sub_; tier4_api_utils::Client::SharedPtr cli_set_module_; tier4_api_utils::Client::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 & cooperate_statuses); + void insertAutoModeMessage(std::vector & auto_mode_status); void callService( CooperateCommands::Request::SharedPtr request, const CooperateCommands::Response::SharedPtr & responses); @@ -88,6 +97,7 @@ class RTCController : public rclcpp::Node /* publishers */ rclcpp::Publisher::SharedPtr rtc_status_pub_; + rclcpp::Publisher::SharedPtr auto_mode_pub_; /* service from external */ rclcpp::CallbackGroup::SharedPtr group_; tier4_api_utils::Service::SharedPtr srv_set_rtc_; @@ -95,6 +105,7 @@ class RTCController : public rclcpp::Node /* Timer */ rclcpp::TimerBase::SharedPtr timer_; + rclcpp::TimerBase::SharedPtr auto_mode_timer_; void insertionSortAndValidation(std::vector & statuses_vector); void checkInfDistance(CooperateStatus & status); @@ -108,6 +119,7 @@ class RTCController : public rclcpp::Node // ros callback void onTimer(); + void onAutoModeTimer(); }; } // namespace external_api