Move api pkgs

autoware_external_api_msgs/CMakeLists.txt

@@ -0,0 +1,28 @@
+cmake_minimum_required(VERSION 3.5)
+project(autoware_external_api_msgs)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  srv/GetVersion.srv
+  DEPENDENCIES
+    tier4_external_api_msgs
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package()

autoware_external_api_msgs/package.xml

@@ -0,0 +1,30 @@
+<?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>autoware_external_api_msgs</name>
+  <version>0.0.0</version>
+  <description>The autoware_external_api_msgs package</description>
+  <maintainer email="isamu.takagi@tier4.jp">Takagi, Isamu</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>rosidl_default_generators</build_depend>
+  <build_depend>builtin_interfaces</build_depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <exec_depend>builtin_interfaces</exec_depend>
+
+  <depend>tier4_external_api_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+
+</package>

autoware_external_api_msgs/srv/GetVersion.srv

@@ -0,0 +1,3 @@
+---
+tier4_external_api_msgs/ResponseStatus status
+string version

autoware_iv_external_api_adaptor/package.xml

@@ -10,12 +10,14 @@
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
 
-  <depend>tier4_api_utils</depend>
+
   <depend>autoware_auto_control_msgs</depend>
   <depend>autoware_auto_system_msgs</depend>
   <depend>autoware_auto_vehicle_msgs</depend>
-  <depend>tier4_external_api_msgs</depend>
+  <depend>autoware_external_api_msgs</depend>
+  <depend>tier4_api_utils</depend>
   <depend>tier4_auto_msgs_converter</depend>
+  <depend>tier4_external_api_msgs</depend>
   <depend>tier4_vehicle_msgs</depend>
   <depend>nlohmann-json-dev</depend>
   <depend>rclcpp</depend>

autoware_iv_external_api_adaptor/src/version.cpp

@@ -24,15 +24,15 @@ Version::Version(const rclcpp::NodeOptions & options)
   tier4_api_utils::ServiceProxyNodeInterface proxy(this);
 
   group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-  srv_ = proxy.create_service<tier4_external_api_msgs::srv::GetVersion>(
+  srv_ = proxy.create_service<autoware_external_api_msgs::srv::GetVersion>(
     "/api/external/get/version",
     std::bind(&Version::getVersion, this, _1, _2),
     rmw_qos_profile_services_default, group_);
 }
 
 void Version::getVersion(
-  const tier4_external_api_msgs::srv::GetVersion::Request::SharedPtr,
-  const tier4_external_api_msgs::srv::GetVersion::Response::SharedPtr response)
+  const autoware_external_api_msgs::srv::GetVersion::Request::SharedPtr,
+  const autoware_external_api_msgs::srv::GetVersion::Response::SharedPtr response)
 {
   response->version = "0.2.0";
   response->status = tier4_api_utils::response_success();

autoware_iv_external_api_adaptor/src/version.hpp

@@ -17,7 +17,7 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "tier4_api_utils/tier4_api_utils.hpp"
-#include "tier4_external_api_msgs/srv/get_version.hpp"
+#include "autoware_external_api_msgs/srv/get_version.hpp"
 
 namespace external_api
 {
@@ -30,12 +30,12 @@ class Version : public rclcpp::Node
 private:
   // ros interface
   rclcpp::CallbackGroup::SharedPtr group_;
-  tier4_api_utils::Service<tier4_external_api_msgs::srv::GetVersion>::SharedPtr srv_;
+  tier4_api_utils::Service<autoware_external_api_msgs::srv::GetVersion>::SharedPtr srv_;
 
   // ros callback
   void getVersion(
-    const tier4_external_api_msgs::srv::GetVersion::Request::SharedPtr request,
-    const tier4_external_api_msgs::srv::GetVersion::Response::SharedPtr response);
+    const autoware_external_api_msgs::srv::GetVersion::Request::SharedPtr request,
+    const autoware_external_api_msgs::srv::GetVersion::Response::SharedPtr response);
 };
 
 }  // namespace external_api

autoware_iv_internal_api_adaptor/CMakeLists.txt

@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.5)
+project(autoware_iv_internal_api_adaptor)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/initial_pose.cpp
+  src/iv_msgs.cpp
+  src/operator.cpp
+  src/route.cpp
+  src/velocity.cpp
+)
+rclcpp_components_register_nodes(${PROJECT_NAME} "internal_api::InitialPose")
+rclcpp_components_register_nodes(${PROJECT_NAME} "internal_api::IVMsgs")
+rclcpp_components_register_nodes(${PROJECT_NAME} "internal_api::Operator")
+rclcpp_components_register_nodes(${PROJECT_NAME} "internal_api::Route")
+rclcpp_components_register_nodes(${PROJECT_NAME} "internal_api::Velocity")
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package()

autoware_iv_internal_api_adaptor/README.md

@@ -0,0 +1 @@
+[See Github Pages](https://tier4.github.io/autoware.proj/tree/main/design/apis/ja/concept/)

autoware_iv_internal_api_adaptor/package.xml

@@ -0,0 +1,40 @@
+<?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>autoware_iv_internal_api_adaptor</name>
+  <version>0.0.0</version>
+  <description>The autoware_iv_internal_api_adaptor package</description>
+  <maintainer email="isamu.takagi@tier4.jp">Takagi, Isamu</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>autoware_auto_planning_msgs</depend>
+  <depend>autoware_auto_vehicle_msgs</depend>
+  <depend>tier4_api_utils</depend>
+  <depend>tier4_control_msgs</depend>
+  <depend>tier4_external_api_msgs</depend>
+  <depend>tier4_localization_msgs</depend>
+  <depend>tier4_planning_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>std_srvs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <!-- for backward compatibility -->
+  <!-- nolint --> <depend>autoware_auto_perception_msgs</depend>
+  <!-- nolint --> <depend>autoware_auto_system_msgs</depend>
+  <!-- nolint --> <depend>tier4_perception_msgs</depend>
+  <!-- nolint --> <depend>tier4_system_msgs</depend>
+  <!-- nolint --> <depend>tier4_vehicle_msgs</depend>
+  <!-- nolint --> <depend>tier4_auto_msgs_converter</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+
+</package>

autoware_iv_internal_api_adaptor/src/initial_pose.cpp

@@ -0,0 +1,107 @@
+// Copyright 2021 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 "initial_pose.hpp"
+
+#include <memory>
+
+namespace internal_api
+{
+InitialPose::InitialPose(const rclcpp::NodeOptions & options)
+: Node("internal_api_initial_pose", options)
+{
+  using std::placeholders::_1;
+  using std::placeholders::_2;
+  tier4_api_utils::ServiceProxyNodeInterface proxy(this);
+
+  init_simulator_pose_ = declare_parameter("init_simulator_pose", false);
+  init_localization_pose_ = declare_parameter("init_localization_pose", false);
+
+  group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  srv_set_initialize_pose_ = proxy.create_service<InitializePose>(
+    "/api/autoware/set/initialize_pose", std::bind(&InitialPose::setInitializePose, this, _1, _2),
+    rmw_qos_profile_services_default, group_);
+  srv_set_initialize_pose_auto_ = proxy.create_service<InitializePoseAuto>(
+    "/api/autoware/set/initialize_pose_auto",
+    std::bind(&InitialPose::setInitializePoseAuto, this, _1, _2), rmw_qos_profile_services_default,
+    group_);
+
+  if (init_localization_pose_) {
+    cli_set_initialize_pose_ = proxy.create_client<PoseWithCovarianceStampedSrv>(
+      "/localization/util/initialize_pose", rmw_qos_profile_services_default);
+    cli_set_initialize_pose_auto_ = proxy.create_client<InitializePoseAuto>(
+      "/localization/util/initialize_pose_auto", rmw_qos_profile_services_default);
+  }
+
+  if (init_simulator_pose_) {
+    cli_set_simulator_pose_ = proxy.create_client<InitializePose>(
+      "/api/simulator/set/pose", rmw_qos_profile_services_default);
+    pub_initialpose2d_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
+      "/initialpose2d", rclcpp::QoS(1));
+  }
+}
+
+void InitialPose::setInitializePose(
+  const tier4_external_api_msgs::srv::InitializePose::Request::SharedPtr request,
+  const tier4_external_api_msgs::srv::InitializePose::Response::SharedPtr response)
+{
+  response->status = tier4_api_utils::response_ignored("No processing.");
+
+  if (init_simulator_pose_) {
+    const auto [status, resp] = cli_set_simulator_pose_->call(request);
+    RCLCPP_INFO(get_logger(), "%d %s", status.code, status.message.c_str());
+    if (!tier4_api_utils::is_success(status)) {
+      response->status = status;
+      return;
+    }
+    pub_initialpose2d_->publish(request->pose);
+    response->status = resp->status;
+  }
+
+  if (init_localization_pose_) {
+    const auto req = std::make_shared<PoseWithCovarianceStampedSrv::Request>();
+    req->pose_with_covariance = request->pose;
+    const auto [status, resp] = cli_set_initialize_pose_->call(req);
+    if (!tier4_api_utils::is_success(status)) {
+      response->status = status;
+      return;
+    }
+    if (resp->success) {
+      response->status = tier4_api_utils::response_success();
+    } else {
+      response->status = tier4_api_utils::response_error("Internal service failed.");
+    }
+  }
+}
+
+void InitialPose::setInitializePoseAuto(
+  const tier4_external_api_msgs::srv::InitializePoseAuto::Request::SharedPtr request,
+  const tier4_external_api_msgs::srv::InitializePoseAuto::Response::SharedPtr response)
+{
+  response->status = tier4_api_utils::response_ignored("No processing.");
+
+  if (init_localization_pose_) {
+    const auto [status, resp] = cli_set_initialize_pose_auto_->call(request);
+    if (!tier4_api_utils::is_success(status)) {
+      response->status = status;
+      return;
+    }
+    response->status = resp->status;
+  }
+}
+
+}  // namespace internal_api
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(internal_api::InitialPose)

autoware_iv_internal_api_adaptor/src/initial_pose.hpp

@@ -0,0 +1,64 @@
+// Copyright 2021 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 INITIAL_POSE_HPP_
+#define INITIAL_POSE_HPP_
+
+#include <tier4_api_utils/tier4_api_utils.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <tier4_external_api_msgs/srv/initialize_pose.hpp>
+#include <tier4_external_api_msgs/srv/initialize_pose_auto.hpp>
+#include <tier4_localization_msgs/srv/pose_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+
+namespace internal_api
+{
+class InitialPose : public rclcpp::Node
+{
+public:
+  explicit InitialPose(const rclcpp::NodeOptions & options);
+
+private:
+  using InitializePose = tier4_external_api_msgs::srv::InitializePose;
+  using InitializePoseAuto = tier4_external_api_msgs::srv::InitializePoseAuto;
+  using PoseWithCovarianceStampedSrv = tier4_localization_msgs::srv::PoseWithCovarianceStamped;
+
+  // ros parameter
+  bool init_simulator_pose_;
+  bool init_localization_pose_;
+
+  // ros interface
+  rclcpp::CallbackGroup::SharedPtr group_;
+  tier4_api_utils::Service<InitializePose>::SharedPtr srv_set_initialize_pose_;
+  tier4_api_utils::Service<InitializePoseAuto>::SharedPtr srv_set_initialize_pose_auto_;
+  tier4_api_utils::Client<PoseWithCovarianceStampedSrv>::SharedPtr cli_set_initialize_pose_;
+  tier4_api_utils::Client<InitializePoseAuto>::SharedPtr cli_set_initialize_pose_auto_;
+  tier4_api_utils::Client<InitializePose>::SharedPtr cli_set_simulator_pose_;
+
+  // TODO(Takagi, Isamu): workaround for topic check
+  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pub_initialpose2d_;
+
+  // ros callback
+  void setInitializePose(
+    const tier4_external_api_msgs::srv::InitializePose::Request::SharedPtr request,
+    const tier4_external_api_msgs::srv::InitializePose::Response::SharedPtr response);
+  void setInitializePoseAuto(
+    const tier4_external_api_msgs::srv::InitializePoseAuto::Request::SharedPtr request,
+    const tier4_external_api_msgs::srv::InitializePoseAuto::Response::SharedPtr response);
+};
+
+}  // namespace internal_api
+
+#endif  // INITIAL_POSE_HPP_