diff --git a/system/default_ad_api_helpers/ad_api_distributor/src/distributor.cpp b/system/default_ad_api_helpers/ad_api_distributor/src/distributor.cpp index 8df72cc39d263..cf60752797d84 100644 --- a/system/default_ad_api_helpers/ad_api_distributor/src/distributor.cpp +++ b/system/default_ad_api_helpers/ad_api_distributor/src/distributor.cpp @@ -21,6 +21,82 @@ namespace ad_api_distributor Distributor::Distributor() : Node("distributor") { + using std::placeholders::_1; + using std::placeholders::_2; + + // Service + srv_initialize_ = create_service( + "/api/localization/initialize", std::bind(&Distributor::on_initialize, this, _1, _2)); + srv_set_route_ = create_service( + "/api/routing/set_route", std::bind(&Distributor::on_set_route, this, _1, _2)); + srv_clear_route_ = create_service( + "/api/routing/clear_route", std::bind(&Distributor::on_clear_route, this, _1, _2)); + srv_stop_mode_ = create_service( + "/api/operation_mode/change_to_stop", std::bind(&Distributor::on_change_to_stop, this, _1, _2)); + srv_autonomous_mode_ = create_service( + "/api/operation_mode/change_to_autonomous", std::bind(&Distributor::on_change_to_autonomous, this, _1, _2)); + + // Client + cli_main_initialize_ = create_client( + "/main/api/localization/initialize"); + cli_sub_initialize_ = create_client( + "/sub/api/localization/initialize"); + cli_main_set_route_ = create_client( + "/main/api/localization/initialize"); + cli_sub_set_route_ = create_client( + "/sub/api/localization/initialize"); + cli_main_clear_route_ = create_client( + "/main/api/localization/initialize"); + cli_sub_clear_route_ = create_client( + "/sub/api/localization/initialize"); + cli_main_stop_mode_ = create_client( + "/main/api/localization/initialize"); + cli_sub_stop_mode_ = create_client( + "/sub/api/localization/initialize"); + cli_main_autonomous_mode_ = create_client( + "/main/api/localization/initialize"); + cli_sub_autonomous_mode_ = create_client( + "/sub/api/localization/initialize"); +} + +void Distributor::on_initialize( + const autoware_adapi_v1_msgs::srv::InitializeLocalization::Request::SharedPtr req, + const autoware_adapi_v1_msgs::srv::InitializeLocalization::Response::SharedPtr) +{ + cli_main_initialize_->async_send_request(req); + cli_sub_initialize_->async_send_request(req); +} + +void Distributor::on_set_route( + const autoware_adapi_v1_msgs::srv::SetRoute::Request::SharedPtr req, + const autoware_adapi_v1_msgs::srv::SetRoute::Response::SharedPtr) +{ + cli_main_set_route_->async_send_request(req); + cli_sub_set_route_->async_send_request(req); +} + +void Distributor::on_clear_route( + const autoware_adapi_v1_msgs::srv::ClearRoute::Request::SharedPtr req, + const autoware_adapi_v1_msgs::srv::ClearRoute::Response::SharedPtr) +{ + cli_main_clear_route_->async_send_request(req); + cli_sub_clear_route_->async_send_request(req); +} + +void Distributor::on_change_to_stop( + const autoware_adapi_v1_msgs::srv::ChangeOperationMode::Request::SharedPtr req, + const autoware_adapi_v1_msgs::srv::ChangeOperationMode::Response::SharedPtr) +{ + cli_main_stop_mode_->async_send_request(req); + cli_sub_stop_mode_->async_send_request(req); +} + +void Distributor::on_change_to_autonomous( + const autoware_adapi_v1_msgs::srv::ChangeOperationMode::Request::SharedPtr req, + const autoware_adapi_v1_msgs::srv::ChangeOperationMode::Response::SharedPtr) +{ + cli_main_autonomous_mode_->async_send_request(req); + cli_sub_autonomous_mode_->async_send_request(req); } } // namespace ad_api_distributor diff --git a/system/default_ad_api_helpers/ad_api_distributor/src/distributor.hpp b/system/default_ad_api_helpers/ad_api_distributor/src/distributor.hpp index 768a2b7f9d43d..328c224c57470 100644 --- a/system/default_ad_api_helpers/ad_api_distributor/src/distributor.hpp +++ b/system/default_ad_api_helpers/ad_api_distributor/src/distributor.hpp @@ -15,6 +15,11 @@ #ifndef DISTRIBUTOR_HPP_ #define DISTRIBUTOR_HPP_ +#include +#include +#include +#include + #include namespace ad_api_distributor @@ -24,6 +29,47 @@ class Distributor : public rclcpp::Node { public: Distributor(); + +private: + // Service + rclcpp::Service::SharedPtr srv_initialize_; + rclcpp::Service::SharedPtr srv_set_route_; + rclcpp::Service::SharedPtr srv_clear_route_; + rclcpp::Service::SharedPtr srv_stop_mode_; + rclcpp::Service::SharedPtr srv_autonomous_mode_; + + // Client + rclcpp::Client::SharedPtr cli_main_initialize_; + rclcpp::Client::SharedPtr cli_sub_initialize_; + rclcpp::Client::SharedPtr cli_main_set_route_; + rclcpp::Client::SharedPtr cli_sub_set_route_; + rclcpp::Client::SharedPtr cli_main_clear_route_; + rclcpp::Client::SharedPtr cli_sub_clear_route_; + rclcpp::Client::SharedPtr cli_main_stop_mode_; + rclcpp::Client::SharedPtr cli_sub_stop_mode_; + rclcpp::Client::SharedPtr cli_main_autonomous_mode_; + rclcpp::Client::SharedPtr cli_sub_autonomous_mode_; + + // Function + void on_initialize( + const autoware_adapi_v1_msgs::srv::InitializeLocalization::Request::SharedPtr req, + const autoware_adapi_v1_msgs::srv::InitializeLocalization::Response::SharedPtr res); + + void on_set_route( + const autoware_adapi_v1_msgs::srv::SetRoute::Request::SharedPtr req, + const autoware_adapi_v1_msgs::srv::SetRoute::Response::SharedPtr res); + + void on_clear_route( + const autoware_adapi_v1_msgs::srv::ClearRoute::Request::SharedPtr req, + const autoware_adapi_v1_msgs::srv::ClearRoute::Response::SharedPtr res); + + void on_change_to_stop( + const autoware_adapi_v1_msgs::srv::ChangeOperationMode::Request::SharedPtr req, + const autoware_adapi_v1_msgs::srv::ChangeOperationMode::Response::SharedPtr res); + + void on_change_to_autonomous( + const autoware_adapi_v1_msgs::srv::ChangeOperationMode::Request::SharedPtr req, + const autoware_adapi_v1_msgs::srv::ChangeOperationMode::Response::SharedPtr res); }; } // namespace ad_api_distributor