feat(default_ad_api): add localization api

common/autoware_ad_api_msgs/CMakeLists.txt

@@ -7,6 +7,8 @@ autoware_package()
 rosidl_generate_interfaces(${PROJECT_NAME}
   common/msg/ResponseStatus.msg
   interface/srv/InterfaceVersion.srv
+  localization/msg/LocalizationInitializationState.msg
+  localization/srv/InitializeLocalization.srv
   DEPENDENCIES
     builtin_interfaces
     std_msgs

common/autoware_ad_api_msgs/localization/msg/LocalizationInitializationState.msg

@@ -0,0 +1,7 @@
+uint16 UNKNOWN = 0
+uint16 UNINITIALIZED = 1
+uint16 INITIALIZING = 2
+uint16 INITIALIZED = 3
+
+builtin_interfaces/Time stamp
+uint16 state

common/autoware_ad_api_msgs/localization/srv/InitializeLocalization.srv

@@ -0,0 +1,7 @@
+geometry_msgs/PoseWithCovarianceStamped[<=1] pose
+---
+uint16 ERROR_UNSAFE = 1
+uint16 ERROR_GNSS_SUPPORT = 2
+uint16 ERROR_GNSS = 3
+uint16 ERROR_ESTIMATION = 4
+autoware_ad_api_msgs/ResponseStatus status

common/autoware_ad_api_specs/CMakeLists.txt

@@ -0,0 +1,7 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_ad_api_specs)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_package()

common/autoware_ad_api_specs/include/autoware_ad_api_specs/localization.hpp

@@ -0,0 +1,43 @@
+// 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.
+
+#ifndef AUTOWARE_AD_API_SPECS__LOCALIZATION_HPP_
+#define AUTOWARE_AD_API_SPECS__LOCALIZATION_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <autoware_ad_api_msgs/msg/localization_initialization_state.hpp>
+#include <autoware_ad_api_msgs/srv/initialize_localization.hpp>
+
+namespace autoware_ad_api::localization
+{
+
+struct Initialize
+{
+  using Service = autoware_ad_api_msgs::srv::InitializeLocalization;
+  static constexpr char name[] = "/api/localization/initialize";
+};
+
+struct InitializationState
+{
+  using Message = autoware_ad_api_msgs::msg::LocalizationInitializationState;
+  static constexpr char name[] = "/api/localization/initialization_state";
+  static constexpr size_t depth = 3;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
+}  // namespace autoware_ad_api::localization
+
+#endif  // AUTOWARE_AD_API_SPECS__LOCALIZATION_HPP_

common/autoware_ad_api_specs/package.xml

@@ -0,0 +1,20 @@
+<?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_ad_api_specs</name>
+  <version>0.0.0</version>
+  <description>The autoware_ad_api_specs 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>autoware_cmake</build_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

common/component_interface_specs/CMakeLists.txt

@@ -0,0 +1,7 @@
+cmake_minimum_required(VERSION 3.14)
+project(component_interface_specs)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_package()

common/component_interface_specs/include/component_interface_specs/localization.hpp

@@ -0,0 +1,43 @@
+// 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.
+
+#ifndef COMPONENT_INTERFACE_SPECS__LOCALIZATION_HPP_
+#define COMPONENT_INTERFACE_SPECS__LOCALIZATION_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <autoware_ad_api_msgs/msg/localization_initialization_state.hpp>
+#include <autoware_ad_api_msgs/srv/initialize_localization.hpp>
+
+namespace localization_interface
+{
+
+struct Initialize
+{
+  using Service = autoware_ad_api_msgs::srv::InitializeLocalization;
+  static constexpr char name[] = "/localization/initialize";
+};
+
+struct InitializationState
+{
+  using Message = autoware_ad_api_msgs::msg::LocalizationInitializationState;
+  static constexpr char name[] = "/localization/initialization_state";
+  static constexpr size_t depth = 3;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
+}  // namespace localization_interface
+
+#endif  // COMPONENT_INTERFACE_SPECS__LOCALIZATION_HPP_

common/component_interface_specs/package.xml

@@ -0,0 +1,20 @@
+<?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>component_interface_specs</name>
+  <version>0.0.0</version>
+  <description>The component_interface_specs 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>autoware_cmake</build_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

launch/tier4_autoware_api_launch/README.md

@@ -10,13 +10,10 @@ Please see `<exec_depend>` in `package.xml`.
 
 ## Usage
 
-You can include as follows in `*.launch.xml` to use `control.launch.py`.
+You can include as follows in `*.launch.xml` to use `autoware_api.launch.xml`.
 
 ```xml
-  <include file="$(find-pkg-share tier4_autoware_api_launch)/launch/autoware_api.launch.xml">
-    <arg name="init_simulator_pose" value="true"/>
-    <arg name="init_localization_pose" value="false"/>
-  </include>
+  <include file="$(find-pkg-share tier4_autoware_api_launch)/launch/autoware_api.launch.xml"/>
 ```
 
 ## Notes

launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml

@@ -6,6 +6,11 @@
   <arg name="launch_calibration_status_api" default="false"/>
   <arg name="launch_start_api" default="true"/>
 
+  <!-- AD API -->
+  <group>
+    <include file="$(find-pkg-share default_ad_api)/launch/default_ad_api.launch.py"/>
+  </group>
+
   <!-- awapi (deprecated) -->
   <group>
     <include file="$(find-pkg-share awapi_awiv_adapter)/launch/awapi_awiv_adapter.launch.xml"/>

launch/tier4_autoware_api_launch/launch/include/internal_api_adaptor.launch.py

@@ -13,7 +13,6 @@
 # limitations under the License.
 
 import launch
-from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import ComposableNodeContainer
 from launch_ros.descriptions import ComposableNode
 
@@ -29,12 +28,7 @@ def _create_api_node(node_name, class_name, **kwargs):
 
 
 def generate_launch_description():
-    param_initial_pose = {
-        "init_simulator_pose": LaunchConfiguration("init_simulator_pose"),
-        "init_localization_pose": LaunchConfiguration("init_localization_pose"),
-    }
     components = [
-        _create_api_node("initial_pose", "InitialPose", parameters=[param_initial_pose]),
         _create_api_node("iv_msgs", "IVMsgs"),
         _create_api_node("operator", "Operator"),
         _create_api_node("route", "Route"),

launch/tier4_autoware_api_launch/launch/include/internal_api_relay.launch.xml

@@ -16,8 +16,5 @@
       name="crosswalk_states"
       args="/api/autoware/set/crosswalk_states /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/input/external_crosswalk_states"
     />
-
-    <!-- TODO(Takagi, Isamu): workaround for topic check -->
-    <node pkg="topic_tools" exec="relay" name="initial_pose_2d" args="/initialpose /initialpose2d"/>
   </group>
 </launch>

launch/tier4_localization_launch/launch/util/util.launch.xml

@@ -13,6 +13,7 @@
   <!-- pose_initializer -->
   <group>
     <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml"/>
+    <include file="$(find-pkg-share default_ad_api_helpers)/launch/auto_pose_init.launch.xml"/>
   </group>
   <!-- util -->
   <group>

system/default_ad_api/CMakeLists.txt

@@ -6,9 +6,13 @@ autoware_package()
 
 ament_auto_add_library(${PROJECT_NAME} SHARED
   src/interface.cpp
+  src/localization.cpp
 )
 
-rclcpp_components_register_nodes(${PROJECT_NAME} "default_ad_api::InterfaceNode")
+rclcpp_components_register_nodes(${PROJECT_NAME}
+  "default_ad_api::InterfaceNode"
+  "default_ad_api::LocalizationNode"
+)
 
 if(BUILD_TESTING)
   add_launch_test(test/main.test.py)

system/default_ad_api/launch/default_ad_api.launch.py

@@ -31,6 +31,7 @@ def _create_api_node(node_name, class_name, **kwargs):
 def generate_launch_description():
     components = [
         _create_api_node("interface", "InterfaceNode"),
+        _create_api_node("localization", "LocalizationNode"),
     ]
     container = ComposableNodeContainer(
         namespace="default_ad_api",

system/default_ad_api/package.xml

@@ -12,6 +12,8 @@
   <build_depend>autoware_cmake</build_depend>
 
   <depend>autoware_ad_api_msgs</depend>
+  <depend>autoware_ad_api_specs</depend>
+  <depend>component_interface_specs</depend>
   <depend>component_interface_utils</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>

system/default_ad_api/src/localization.cpp

@@ -0,0 +1,32 @@
+// 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.
+
+#include "localization.hpp"
+
+namespace default_ad_api
+{
+
+LocalizationNode::LocalizationNode(const rclcpp::NodeOptions & options)
+: Node("localization", options)
+{
+  const auto node = component_interface_utils::NodeAdaptor(this);
+  group_srv_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  node.relay_message(pub_state_, sub_state_);
+  node.relay_service(cli_initialize_, srv_initialize_, group_srv_);
+}
+
+}  // namespace default_ad_api
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::LocalizationNode)

system/default_ad_api/src/localization.hpp

@@ -0,0 +1,46 @@
+// 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.
+
+#ifndef LOCALIZATION_HPP_
+#define LOCALIZATION_HPP_
+
+#include <autoware_ad_api_specs/localization.hpp>
+#include <component_interface_specs/localization.hpp>
+#include <component_interface_utils/rclcpp.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+using Initialize1 = autoware_ad_api::localization::Initialize;
+using Initialize2 = localization_interface::Initialize;
+using State1 = autoware_ad_api::localization::InitializationState;
+using State2 = localization_interface::InitializationState;
+
+namespace default_ad_api
+{
+
+class LocalizationNode : public rclcpp::Node
+{
+public:
+  explicit LocalizationNode(const rclcpp::NodeOptions & options);
+
+private:
+  rclcpp::CallbackGroup::SharedPtr group_srv_;
+  component_interface_utils::Service<Initialize1>::SharedPtr srv_initialize_;
+  component_interface_utils::Client<Initialize2>::SharedPtr cli_initialize_;
+  component_interface_utils::Publisher<State1>::SharedPtr pub_state_;
+  component_interface_utils::Subscription<State2>::SharedPtr sub_state_;
+};
+
+}  // namespace default_ad_api
+
+#endif  // LOCALIZATION_HPP_

system/default_ad_api_helpers/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.14)
+project(default_ad_api_helpers)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_executable(automatic_pose_initializer
+  src/automatic_pose_initializer.cpp
+)
+
+ament_auto_add_executable(initial_pose_adaptor
+  src/initial_pose_adaptor.cpp
+)
+
+ament_auto_package(INSTALL_TO_SHARE config launch)

system/default_ad_api_helpers/README.md

@@ -0,0 +1,23 @@
+# default_ad_api_helpers
+
+## initial_pose_adaptor
+
+This node makes it easy to use the localization AD API from RViz.
+When a initial pose topic is received, call the localization initialize API.
+This node depends on fitting to map height service.
+
+| Interface    | Local Name     | Global Name                       | Description                               |
+| ------------ | -------------- | --------------------------------- | ----------------------------------------- |
+| Subscription | initialpose    | /initialpose                      | The pose for localization initialization. |
+| Client       | fit_map_height | /localization/util/fit_map_height | To fix initial pose to map height         |
+| Client       | -              | /api/localization/initialize      | The localization initialize API.          |
+
+## automatic_pose_initializer
+
+This node calls localization initialize API when the localization initialization state is uninitialized.
+Since the API uses GNSS pose when no pose is specified, initialization using GNSS can be performed automatically.
+
+| Interface    | Local Name | Global Name                            | Description                                |
+| ------------ | ---------- | -------------------------------------- | ------------------------------------------ |
+| Subscription | -          | /api/localization/initialization_state | The localization initialization state API. |
+| Client       | -          | /api/localization/initialize           | The localization initialize API.           |