feat(default_ad_api): add localization api (autowarefoundation#1431)

* feat(default_ad_api): add localization api

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* docs: add readme

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: add auto initial pose

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat(autoware_ad_api_msgs): define localization interface

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix(default_ad_api): fix interface definition

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat(default_ad_api): modify interface version api to use spec package

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat(default_ad_api): modify interface version api to use spec package

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix: pre-commit

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix: pre-commit

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix: pre-commit

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix: copyright

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: split helper package

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix: change topic name to local

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix: style

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix: style

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix: style

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix: remove needless keyword

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: change api helper node namespace

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix: fix launch file path

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

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/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 automatic_pose_initializer)/launch/automatic_pose_initializer.launch.xml"/>
   </group>
   <!-- util -->
   <group>

system/default_ad_api/CMakeLists.txt

@@ -6,11 +6,13 @@ autoware_package()
 
 ament_auto_add_library(${PROJECT_NAME} SHARED
   src/interface.cpp
+  src/localization.cpp
   src/routing.cpp
 )
 
 rclcpp_components_register_nodes(${PROJECT_NAME}
   "default_ad_api::InterfaceNode"
+  "default_ad_api::LocalizationNode"
   "default_ad_api::RoutingNode"
 )
 

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"),
         _create_api_node("routing", "RoutingNode"),
     ]
     container = ComposableNodeContainer(

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 adaptor = component_interface_utils::NodeAdaptor(this);
+  group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  adaptor.relay_message(pub_state_, sub_state_);
+  adaptor.relay_service(cli_initialize_, srv_initialize_, group_cli_);
+}
+
+}  // 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,44 @@
+// 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>
+
+// This file should be included after messages.
+#include "utils/types.hpp"
+
+namespace default_ad_api
+{
+
+class LocalizationNode : public rclcpp::Node
+{
+public:
+  explicit LocalizationNode(const rclcpp::NodeOptions & options);
+
+private:
+  rclcpp::CallbackGroup::SharedPtr group_cli_;
+  Srv<autoware_ad_api::localization::Initialize> srv_initialize_;
+  Pub<autoware_ad_api::localization::InitializationState> pub_state_;
+  Cli<localization_interface::Initialize> cli_initialize_;
+  Sub<localization_interface::InitializationState> sub_state_;
+};
+
+}  // namespace default_ad_api
+
+#endif  // LOCALIZATION_HPP_

system/default_ad_api_helpers/ad_api_adaptors/CMakeLists.txt

@@ -4,8 +4,12 @@ project(ad_api_adaptors)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
+ament_auto_add_executable(initial_pose_adaptor
+  src/initial_pose_adaptor.cpp
+)
+
 ament_auto_add_executable(routing_adaptor
   src/routing_adaptor.cpp
 )
 
-ament_auto_package(INSTALL_TO_SHARE launch)
+ament_auto_package(INSTALL_TO_SHARE config launch)

system/default_ad_api_helpers/ad_api_adaptors/README.md

@@ -1,5 +1,17 @@
 # ad_api_adaptors
 
+## 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.          |
+
 ## routing_adaptor
 
 This node makes it easy to use the routing AD API from RViz.

system/default_ad_api_helpers/ad_api_adaptors/config/initial_pose.param.yaml

@@ -0,0 +1,13 @@
+/**:
+  ros__parameters:
+
+    # from initialpose (Rviz's 2DPoseEstimate)
+    initial_pose_particle_covariance:
+      [
+        4.0, 0.0, 0.0,  0.0,  0.0,  0.0,
+        0.0, 4.0, 0.0,  0.0,  0.0,  0.0,
+        0.0, 0.0, 0.01, 0.0,  0.0,  0.0,
+        0.0, 0.0, 0.0,  0.01, 0.0,  0.0,
+        0.0, 0.0, 0.0,  0.0,  0.01, 0.0,
+        0.0, 0.0, 0.0,  0.0,  0.0,  1.0,
+      ]

system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml

@@ -1,6 +1,14 @@
 <launch>
-  <node pkg="ad_api_adaptors" exec="initial_pose_adaptor" name="initial_pose_adaptor">
-    <remap from="~/input/goal" to="/planning/mission_planning/goal"/>
-    <remap from="~/input/waypoint" to="/planning/mission_planning/checkpoint"/>
-  </node>
+  <group>
+    <push-ros-namespace namespace="default_ad_api/helpers"/>
+    <node pkg="ad_api_adaptors" exec="initial_pose_adaptor" name="initial_pose_adaptor">
+      <param from="$(find-pkg-share ad_api_adaptors)/config/initial_pose.param.yaml"/>
+      <remap from="~/initialpose" to="/initialpose"/>
+      <remap from="~/fit_map_height" to="/localization/util/fit_map_height"/>
+    </node>
+    <node pkg="ad_api_adaptors" exec="routing_adaptor" name="routing_adaptor">
+      <remap from="~/input/goal" to="/planning/mission_planning/goal"/>
+      <remap from="~/input/waypoint" to="/planning/mission_planning/checkpoint"/>
+    </node>
+  </group>
 </launch>

system/default_ad_api_helpers/ad_api_adaptors/package.xml

@@ -15,6 +15,7 @@
   <depend>autoware_ad_api_specs</depend>
   <depend>component_interface_utils</depend>
   <depend>rclcpp</depend>
+  <depend>tier4_localization_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>

system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.cpp

@@ -0,0 +1,72 @@
+// 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 "initial_pose_adaptor.hpp"
+
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace ad_api_adaptors
+{
+template <class ServiceT>
+using Future = typename rclcpp::Client<ServiceT>::SharedFuture;
+
+std::array<double, 36> get_covariance_parameter(rclcpp::Node * node, const std::string & name)
+{
+  const auto vector = node->declare_parameter<std::vector<double>>(name);
+  if (vector.size() != 36) {
+    throw std::invalid_argument("The covariance parameter size is not 36.");
+  }
+  std::array<double, 36> array;
+  std::copy_n(vector.begin(), array.size(), array.begin());
+  return array;
+}
+
+InitialPoseAdaptor::InitialPoseAdaptor() : Node("initial_pose_adaptor")
+{
+  rviz_particle_covariance_ = get_covariance_parameter(this, "initial_pose_particle_covariance");
+  cli_map_fit_ = create_client<RequestHeightFitting>("~/fit_map_height");
+  sub_initial_pose_ = create_subscription<PoseWithCovarianceStamped>(
+    "~/initialpose", rclcpp::QoS(1),
+    std::bind(&InitialPoseAdaptor::on_initial_pose, this, std::placeholders::_1));
+
+  const auto adaptor = component_interface_utils::NodeAdaptor(this);
+  adaptor.init_cli(cli_initialize_);
+}
+
+void InitialPoseAdaptor::on_initial_pose(const PoseWithCovarianceStamped::ConstSharedPtr msg)
+{
+  const auto req = std::make_shared<RequestHeightFitting::Request>();
+  req->pose_with_covariance = *msg;
+  cli_map_fit_->async_send_request(req, [this](Future<RequestHeightFitting> future) {
+    const auto req = std::make_shared<Initialize::Service::Request>();
+    req->pose.push_back(future.get()->pose_with_covariance);
+    req->pose.back().pose.covariance = rviz_particle_covariance_;
+    cli_initialize_->async_send_request(req);
+  });
+}
+
+}  // namespace ad_api_adaptors
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::executors::SingleThreadedExecutor executor;
+  auto node = std::make_shared<ad_api_adaptors::InitialPoseAdaptor>();
+  executor.add_node(node);
+  executor.spin();
+  executor.remove_node(node);
+  rclcpp::shutdown();
+}

system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.hpp

@@ -0,0 +1,47 @@
+// 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 INITIAL_POSE_ADAPTOR_HPP_
+#define INITIAL_POSE_ADAPTOR_HPP_
+
+#include <autoware_ad_api_specs/localization.hpp>
+#include <component_interface_utils/rclcpp.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <tier4_localization_msgs/srv/pose_with_covariance_stamped.hpp>
+
+namespace ad_api_adaptors
+{
+
+class InitialPoseAdaptor : public rclcpp::Node
+{
+public:
+  InitialPoseAdaptor();
+
+private:
+  using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
+  using Initialize = autoware_ad_api::localization::Initialize;
+  using RequestHeightFitting = tier4_localization_msgs::srv::PoseWithCovarianceStamped;
+  rclcpp::Subscription<PoseWithCovarianceStamped>::SharedPtr sub_initial_pose_;
+  rclcpp::Client<RequestHeightFitting>::SharedPtr cli_map_fit_;
+  component_interface_utils::Client<Initialize>::SharedPtr cli_initialize_;
+  std::array<double, 36> rviz_particle_covariance_;
+
+  void on_initial_pose(const PoseWithCovarianceStamped::ConstSharedPtr msg);
+};
+
+}  // namespace ad_api_adaptors
+
+#endif  // INITIAL_POSE_ADAPTOR_HPP_

system/default_ad_api_helpers/automatic_pose_initializer/CMakeLists.txt

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

system/default_ad_api_helpers/automatic_pose_initializer/README.md

@@ -0,0 +1,11 @@
+# automatic_pose_initializer
+
+## 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.           |

system/default_ad_api_helpers/automatic_pose_initializer/launch/automatic_pose_initializer.launch.xml

@@ -0,0 +1,6 @@
+<launch>
+  <group>
+    <push-ros-namespace namespace="default_ad_api/helpers"/>
+    <node pkg="automatic_pose_initializer" exec="automatic_pose_initializer" name="automatic_pose_initializer"/>
+  </group>
+</launch>