Merge pull request #1069 from tier4/feature/add-adapi-distributor

feat(ad_api_distributor): add adapi distributor

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)

system/default_ad_api_helpers/ad_api_distributor/README.md

@@ -0,0 +1 @@
+# ad_api_distributor

system/default_ad_api_helpers/ad_api_distributor/launch/distributor.launch.xml

@@ -0,0 +1,3 @@
+<launch>
+  <node pkg="ad_api_distributor" exec="distributor" name="distributor"/>
+</launch>

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])

system/default_ad_api_helpers/ad_api_distributor/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ad_api_distributor</name>
+  <version>0.1.0</version>
+  <description>The ad_api_distributor package</description>
+  <maintainer email="[email protected]">Makoto Kurihara</maintainer>
+  <maintainer email="[email protected]">Takagi, Isamu</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_adapi_v1_msgs</depend>
+  <depend>rclcpp</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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 <memory>
+
+namespace ad_api_distributor
+{
+
+Distributor::Distributor() : Node("distributor")
+{
+  using std::placeholders::_1;
+  using std::placeholders::_2;
+
+  // Service
+  srv_initialize_ = create_service<autoware_adapi_v1_msgs::srv::InitializeLocalization>(
+    "/api/localization/initialize", std::bind(&Distributor::on_initialize, this, _1, _2));
+  srv_set_route_ = create_service<autoware_adapi_v1_msgs::srv::SetRoute>(
+    "/api/routing/set_route", std::bind(&Distributor::on_set_route, this, _1, _2));
+  srv_clear_route_ = create_service<autoware_adapi_v1_msgs::srv::ClearRoute>(
+    "/api/routing/clear_route", std::bind(&Distributor::on_clear_route, this, _1, _2));
+  srv_stop_mode_ = create_service<autoware_adapi_v1_msgs::srv::ChangeOperationMode>(
+    "/api/operation_mode/change_to_stop", std::bind(&Distributor::on_change_to_stop, this, _1, _2));
+  srv_autonomous_mode_ = create_service<autoware_adapi_v1_msgs::srv::ChangeOperationMode>(
+    "/api/operation_mode/change_to_autonomous",
+    std::bind(&Distributor::on_change_to_autonomous, this, _1, _2));
+
+  // Client
+  cli_main_initialize_ = create_client<autoware_adapi_v1_msgs::srv::InitializeLocalization>(
+    "/main/api/localization/initialize");
+  cli_sub_initialize_ = create_client<autoware_adapi_v1_msgs::srv::InitializeLocalization>(
+    "/sub/api/localization/initialize");
+  cli_main_set_route_ =
+    create_client<autoware_adapi_v1_msgs::srv::SetRoute>("/main/api/localization/initialize");
+  cli_sub_set_route_ =
+    create_client<autoware_adapi_v1_msgs::srv::SetRoute>("/sub/api/localization/initialize");
+  cli_main_clear_route_ =
+    create_client<autoware_adapi_v1_msgs::srv::ClearRoute>("/main/api/localization/initialize");
+  cli_sub_clear_route_ =
+    create_client<autoware_adapi_v1_msgs::srv::ClearRoute>("/sub/api/localization/initialize");
+  cli_main_stop_mode_ = create_client<autoware_adapi_v1_msgs::srv::ChangeOperationMode>(
+    "/main/api/localization/initialize");
+  cli_sub_stop_mode_ = create_client<autoware_adapi_v1_msgs::srv::ChangeOperationMode>(
+    "/sub/api/localization/initialize");
+  cli_main_autonomous_mode_ = create_client<autoware_adapi_v1_msgs::srv::ChangeOperationMode>(
+    "/main/api/localization/initialize");
+  cli_sub_autonomous_mode_ = create_client<autoware_adapi_v1_msgs::srv::ChangeOperationMode>(
+    "/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<ad_api_distributor::Distributor>();
+  executor.add_node(node);
+  executor.spin();
+  executor.remove_node(node);
+  rclcpp::shutdown();
+}

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 <rclcpp/rclcpp.hpp>
+
+#include <autoware_adapi_v1_msgs/srv/change_operation_mode.hpp>
+#include <autoware_adapi_v1_msgs/srv/clear_route.hpp>
+#include <autoware_adapi_v1_msgs/srv/initialize_localization.hpp>
+#include <autoware_adapi_v1_msgs/srv/set_route.hpp>
+
+namespace ad_api_distributor
+{
+
+class Distributor : public rclcpp::Node
+{
+public:
+  Distributor();
+
+private:
+  // Service
+  rclcpp::Service<autoware_adapi_v1_msgs::srv::InitializeLocalization>::SharedPtr srv_initialize_;
+  rclcpp::Service<autoware_adapi_v1_msgs::srv::SetRoute>::SharedPtr srv_set_route_;
+  rclcpp::Service<autoware_adapi_v1_msgs::srv::ClearRoute>::SharedPtr srv_clear_route_;
+  rclcpp::Service<autoware_adapi_v1_msgs::srv::ChangeOperationMode>::SharedPtr srv_stop_mode_;
+  rclcpp::Service<autoware_adapi_v1_msgs::srv::ChangeOperationMode>::SharedPtr srv_autonomous_mode_;
+
+  // Client
+  rclcpp::Client<autoware_adapi_v1_msgs::srv::InitializeLocalization>::SharedPtr
+    cli_main_initialize_;
+  rclcpp::Client<autoware_adapi_v1_msgs::srv::InitializeLocalization>::SharedPtr
+    cli_sub_initialize_;
+  rclcpp::Client<autoware_adapi_v1_msgs::srv::SetRoute>::SharedPtr cli_main_set_route_;
+  rclcpp::Client<autoware_adapi_v1_msgs::srv::SetRoute>::SharedPtr cli_sub_set_route_;
+  rclcpp::Client<autoware_adapi_v1_msgs::srv::ClearRoute>::SharedPtr cli_main_clear_route_;
+  rclcpp::Client<autoware_adapi_v1_msgs::srv::ClearRoute>::SharedPtr cli_sub_clear_route_;
+  rclcpp::Client<autoware_adapi_v1_msgs::srv::ChangeOperationMode>::SharedPtr cli_main_stop_mode_;
+  rclcpp::Client<autoware_adapi_v1_msgs::srv::ChangeOperationMode>::SharedPtr cli_sub_stop_mode_;
+  rclcpp::Client<autoware_adapi_v1_msgs::srv::ChangeOperationMode>::SharedPtr
+    cli_main_autonomous_mode_;
+  rclcpp::Client<autoware_adapi_v1_msgs::srv::ChangeOperationMode>::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_