diff --git a/system/default_ad_api_helpers/ad_api_distributor/CMakeLists.txt b/system/default_ad_api_helpers/ad_api_distributor/CMakeLists.txt new file mode 100644 index 0000000000000..269885d73ad76 --- /dev/null +++ b/system/default_ad_api_helpers/ad_api_distributor/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.14) +project(ad_api_distributor) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_executable(distributor + src/distributor.cpp +) + +ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/system/default_ad_api_helpers/ad_api_distributor/README.md b/system/default_ad_api_helpers/ad_api_distributor/README.md new file mode 100644 index 0000000000000..8a5adaf31cad2 --- /dev/null +++ b/system/default_ad_api_helpers/ad_api_distributor/README.md @@ -0,0 +1 @@ +# ad_api_distributor diff --git a/system/default_ad_api_helpers/ad_api_distributor/launch/distributor.launch.xml b/system/default_ad_api_helpers/ad_api_distributor/launch/distributor.launch.xml new file mode 100644 index 0000000000000..5fea4d3257ad2 --- /dev/null +++ b/system/default_ad_api_helpers/ad_api_distributor/launch/distributor.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/system/default_ad_api_helpers/ad_api_distributor/launch/remapped_api.launch.py b/system/default_ad_api_helpers/ad_api_distributor/launch/remapped_api.launch.py new file mode 100644 index 0000000000000..ba6bc74b464e4 --- /dev/null +++ b/system/default_ad_api_helpers/ad_api_distributor/launch/remapped_api.launch.py @@ -0,0 +1,81 @@ +# Copyright 2022 TIER IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# This file is copied from system/default_ad_api/launch/default_ad_api.launch.py + +import launch +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch_ros.parameter_descriptions import ParameterFile +from launch_ros.substitutions import FindPackageShare + + +def create_remapping(name): + return (name, ["/", LaunchConfiguration("prefix"), name]) + + +def create_api_node(node_name, class_name, **kwargs): + remappings = [ + create_remapping("/api/localization/initialize"), + create_remapping("/api/routing/set_route"), + create_remapping("/api/routing/clear_route"), + create_remapping("/api/operation_mode/change_to_stop"), + create_remapping("/api/operation_mode/change_to_autonomous"), + ] + return ComposableNode( + namespace="default_ad_api/node", + name=node_name, + package="default_ad_api", + plugin="default_ad_api::" + class_name, + parameters=[ParameterFile(LaunchConfiguration("config"))], + remappings=remappings, + ) + + +def get_default_config(): + path = FindPackageShare("default_ad_api") + path = PathJoinSubstitution([path, "config/default_ad_api.param.yaml"]) + return path + + +def generate_launch_description(): + components = [ + create_api_node("autoware_state", "AutowareStateNode"), + create_api_node("fail_safe", "FailSafeNode"), + create_api_node("interface", "InterfaceNode"), + create_api_node("localization", "LocalizationNode"), + create_api_node("motion", "MotionNode"), + create_api_node("operation_mode", "OperationModeNode"), + create_api_node("perception", "PerceptionNode"), + create_api_node("planning", "PlanningNode"), + create_api_node("routing", "RoutingNode"), + create_api_node("vehicle", "VehicleNode"), + create_api_node("vehicle_info", "VehicleInfoNode"), + ] + container = ComposableNodeContainer( + namespace="default_ad_api", + name="container", + package="rclcpp_components", + executable="component_container_mt", + ros_arguments=["--log-level", "default_ad_api.container:=WARN"], + composable_node_descriptions=components, + ) + arguments = [ + DeclareLaunchArgument("config", default_value=get_default_config()), + DeclareLaunchArgument("prefix"), + ] + return launch.LaunchDescription([*arguments, container]) diff --git a/system/default_ad_api_helpers/ad_api_distributor/package.xml b/system/default_ad_api_helpers/ad_api_distributor/package.xml new file mode 100644 index 0000000000000..ef50cd6ac9f06 --- /dev/null +++ b/system/default_ad_api_helpers/ad_api_distributor/package.xml @@ -0,0 +1,23 @@ + + + + ad_api_distributor + 0.1.0 + The ad_api_distributor package + Makoto Kurihara + Takagi, Isamu + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_adapi_v1_msgs + rclcpp + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + 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 new file mode 100644 index 0000000000000..76e07e9ffb93e --- /dev/null +++ b/system/default_ad_api_helpers/ad_api_distributor/src/distributor.cpp @@ -0,0 +1,141 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "distributor.hpp" + +#include + +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 res) +{ + if (!cli_main_initialize_->service_is_ready() || !cli_sub_initialize_->service_is_ready()) { + res->status.success = false; + return; + } + cli_main_initialize_->async_send_request(req); + cli_sub_initialize_->async_send_request(req); + res->status.success = true; +} + +void Distributor::on_set_route( + const autoware_adapi_v1_msgs::srv::SetRoute::Request::SharedPtr req, + const autoware_adapi_v1_msgs::srv::SetRoute::Response::SharedPtr res) +{ + if (!cli_main_set_route_->service_is_ready() || !cli_sub_set_route_->service_is_ready()) { + res->status.success = false; + return; + } + cli_main_set_route_->async_send_request(req); + cli_sub_set_route_->async_send_request(req); + res->status.success = true; +} + +void Distributor::on_clear_route( + const autoware_adapi_v1_msgs::srv::ClearRoute::Request::SharedPtr req, + const autoware_adapi_v1_msgs::srv::ClearRoute::Response::SharedPtr res) +{ + if (!cli_main_clear_route_->service_is_ready() || !cli_sub_clear_route_->service_is_ready()) { + res->status.success = false; + return; + } + cli_main_clear_route_->async_send_request(req); + cli_sub_clear_route_->async_send_request(req); + res->status.success = true; +} + +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 res) +{ + if (!cli_main_stop_mode_->service_is_ready() || !cli_sub_stop_mode_->service_is_ready()) { + res->status.success = false; + return; + } + cli_main_stop_mode_->async_send_request(req); + cli_sub_stop_mode_->async_send_request(req); + res->status.success = true; +} + +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 res) +{ + if ( + !cli_main_autonomous_mode_->service_is_ready() || + !cli_sub_autonomous_mode_->service_is_ready()) { + res->status.success = false; + return; + } + cli_main_autonomous_mode_->async_send_request(req); + cli_sub_autonomous_mode_->async_send_request(req); + res->status.success = true; +} + +} // namespace ad_api_distributor + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::executors::MultiThreadedExecutor executor; + auto node = std::make_shared(); + executor.add_node(node); + executor.spin(); + executor.remove_node(node); + rclcpp::shutdown(); +} 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 new file mode 100644 index 0000000000000..f01ad2ba84fae --- /dev/null +++ b/system/default_ad_api_helpers/ad_api_distributor/src/distributor.hpp @@ -0,0 +1,81 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef DISTRIBUTOR_HPP_ +#define DISTRIBUTOR_HPP_ + +#include + +#include +#include +#include +#include + +namespace ad_api_distributor +{ + +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 + +#endif // DISTRIBUTOR_HPP_