feat(pose_initializer)!: support ad 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/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

@@ -12,7 +12,11 @@
 
   <!-- pose_initializer -->
   <group>
-    <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml"/>
+    <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
+      <arg name="gnss_enabled" value="true"/>
+      <arg name="ndt_enabled" value="true"/>
+      <arg name="stop_check_enabled" value="true"/>
+    </include>
   </group>
   <!-- util -->
   <group>

launch/tier4_simulator_launch/launch/simulator.launch.xml

@@ -1,6 +1,7 @@
 <launch>
   <arg name="launch_dummy_perception"/>
   <arg name="launch_dummy_vehicle"/>
+  <arg name="launch_dummy_localization"/>
   <arg name="vehicle_info_param_file"/>
 
   <arg name="perception/enable_detection_failure" default="true" description="enable to simulate detection failure when using dummy perception"/>
@@ -71,6 +72,15 @@
     </group>
   </group>
 
+  <!-- Dummy localization -->
+  <group if="$(var launch_dummy_localization)">
+    <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
+      <arg name="gnss_enabled" value="false"/>
+      <arg name="ndt_enabled" value="false"/>
+      <arg name="stop_check_enabled" value="false"/>
+    </include>
+  </group>
+
   <!-- Simulator model -->
   <group if="$(var launch_dummy_vehicle)">
     <arg name="simulator_model" default="$(var vehicle_model_pkg)/config/simulator_model.param.yaml" description="path to the file of simulator model"/>

launch/tier4_system_launch/config/ad_service_state_monitor/ad_service_state_monitor.param.yaml

@@ -25,9 +25,9 @@
         "/map/vector_map",
         "/map/pointcloud_map",
         "/perception/obstacle_segmentation/pointcloud",
+        "/perception/object_recognition/objects",
         "/initialpose3d",
         "/localization/pose_twist_fusion_filter/pose",
-        "/perception/object_recognition/objects",
         "/planning/mission_planning/route",
         "/planning/scenario_planning/trajectory",
         "/control/trajectory_follower/control_cmd",

launch/tier4_system_launch/config/ad_service_state_monitor/ad_service_state_monitor.planning_simulation.param.yaml

@@ -24,7 +24,7 @@
       names: [
         "/map/vector_map",
         "/perception/object_recognition/objects",
-        "/initialpose2d",
+        "/initialpose3d",
         "/planning/mission_planning/route",
         "/planning/scenario_planning/trajectory",
         "/control/trajectory_follower/control_cmd",
@@ -48,7 +48,7 @@
           warn_rate: 5.0
           type: "autoware_auto_perception_msgs/msg/PredictedObjects"
 
-        /initialpose2d:
+        /initialpose3d:
           module: "localization"
           timeout: 0.0
           warn_rate: 0.0

localization/ekf_localizer/launch/ekf_localizer.launch.xml

@@ -5,7 +5,7 @@
   <arg name="tf_rate" default="10.0"/>
   <arg name="extend_state_step" default="50"/>
 
-  <arg name="input_initial_pose_name" default="initialpose"/>
+  <arg name="input_initial_pose_name" default="initialpose3d"/>
 
   <!-- for Pose measurement -->
   <arg name="input_pose_with_cov_name" default="in_pose_with_covariance"/>

localization/initial_pose_button_panel/src/initial_pose_button_panel.cpp

@@ -69,7 +69,7 @@ void InitialPoseButtonPanel::onInitialize()
     std::bind(&InitialPoseButtonPanel::callbackPoseCov, this, std::placeholders::_1));
 
   client_ = raw_node->create_client<tier4_localization_msgs::srv::PoseWithCovarianceStamped>(
-    "/localization/util/initialize_pose");
+    "/localization/initialize");
 }
 
 void InitialPoseButtonPanel::callbackPoseCov(

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

@@ -341,14 +341,12 @@ void NDTScanMatcher::serviceNDTAlign(
 
   if (ndt_ptr_->getInputTarget() == nullptr) {
     res->success = false;
-    res->seq = req->seq;
     RCLCPP_WARN(get_logger(), "No InputTarget");
     return;
   }
 
   if (ndt_ptr_->getInputSource() == nullptr) {
     res->success = false;
-    res->seq = req->seq;
     RCLCPP_WARN(get_logger(), "No InputSource");
     return;
   }
@@ -360,7 +358,6 @@ void NDTScanMatcher::serviceNDTAlign(
   res->pose_with_covariance = alignUsingMonteCarlo(ndt_ptr_, mapTF_initial_pose_msg);
   key_value_stdmap_["state"] = "Sleeping";
   res->success = true;
-  res->seq = req->seq;
   res->pose_with_covariance.pose.covariance = req->pose_with_covariance.pose.covariance;
 }
 

localization/pose_initializer/CMakeLists.txt

@@ -4,21 +4,25 @@ project(pose_initializer)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
-find_package(PCL REQUIRED)
-
-ament_auto_add_executable(pose_initializer
-  src/pose_initializer_node.cpp
-  src/pose_initializer_core.cpp
+find_package(PCL REQUIRED COMPONENTS common)
+
+ament_auto_add_executable(pose_initializer_node
+  src/pose_initializer/pose_initializer_node.cpp
+  src/pose_initializer/pose_initializer_core.cpp
+  src/pose_initializer/gnss_module.cpp
+  src/pose_initializer/ndt_module.cpp
+  src/pose_initializer/stop_check_module.cpp
 )
-
-# fmt often fails to automatically link so manually specify here
-target_link_libraries(pose_initializer ${PCL_LIBRARIES} fmt)
+ament_auto_add_executable(map_height_fitter
+  src/map_height_fitter/map_height_fitter_node.cpp
+  src/map_height_fitter/map_height_fitter_core.cpp
+)
+target_link_libraries(map_height_fitter ${PCL_LIBRARIES})
 
 if(BUILD_TESTING)
   function(add_testcase filepath)
     get_filename_component(filename ${filepath} NAME)
     string(REGEX REPLACE ".cpp" "" test_name ${filename})
-
     ament_add_gmock(${test_name} ${filepath})
     target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
     target_link_libraries(${test_name} fmt)
@@ -27,7 +31,6 @@ if(BUILD_TESTING)
 
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
-
   add_testcase(test/test_copy_vector_to_array.cpp)
 endif()
 

localization/pose_initializer/README.md

@@ -7,18 +7,41 @@ It receives roughly estimated initial pose from GNSS/user.
 Passing the pose to `ndt_scan_matcher`, and it gets a calculated ego pose from `ndt_scan_matcher` via service.
 Finally, it publishes the initial pose to `ekf_localizer`.
 
-## Input / Output
+## Interfaces
 
-### Input topics
+### Parameters
 
-| Name                                 | Type                                          | Description            |
-| ------------------------------------ | --------------------------------------------- | ---------------------- |
-| `/initialpose`                       | geometry_msgs::msg::PoseWithCovarianceStamped | initial pose from rviz |
-| `/sensing/gnss/pose_with_covariance` | geometry_msgs::msg::PoseWithCovarianceStamped | pose from gnss         |
-| `/map/pointcloud_map`                | sensor_msgs::msg::PointCloud2                 | pointcloud map         |
+| Name                  | Type | Description                                                                              |
+| --------------------- | ---- | ---------------------------------------------------------------------------------------- |
+| `ndt_enabled`         | bool | If true, the pose will be estimated by NDT scan matcher, otherwise it is passed through. |
+| `stop_check_enabled`  | bool | If true, initialization is accepted only when the vehicle is stopped.                    |
+| `stop_check_duration` | bool | The duration used for the stop check above.                                              |
+| `gnss_enabled`        | bool | If true, use the GNSS pose when no pose is specified.                                    |
+| `gnss_pose_timeout`   | bool | The duration that the GNSS pose is valid.                                                |
 
-### Output topics
+### Services
 
-| Name             | Type                                          | Description                 |
-| ---------------- | --------------------------------------------- | --------------------------- |
-| `/initialpose3d` | geometry_msgs::msg::PoseWithCovarianceStamped | calculated initial ego pose |
+| Name                       | Type                                              | Description           |
+| -------------------------- | ------------------------------------------------- | --------------------- |
+| `/localization/initialize` | autoware_ad_api_msgs::srv::InitializeLocalization | initial pose from api |
+
+### Clients
+
+| Name                                         | Type                                                    | Description             |
+| -------------------------------------------- | ------------------------------------------------------- | ----------------------- |
+| `/localization/pose_estimator/ndt_align_srv` | tier4_localization_msgs::srv::PoseWithCovarianceStamped | pose estimation service |
+
+### Subscriptions
+
+| Name                                                        | Type                                          | Description          |
+| ----------------------------------------------------------- | --------------------------------------------- | -------------------- |
+| `/sensing/gnss/pose_with_covariance`                        | geometry_msgs::msg::PoseWithCovarianceStamped | pose from gnss       |
+| `/sensing/vehicle_velocity_converter/twist_with_covariance` | geometry_msgs::msg::TwistStamped              | twist for stop check |
+| `/map/pointcloud_map`                                       | sensor_msgs::msg::PointCloud2                 | pointcloud map       |
+
+### Publications
+
+| Name                                 | Type                                                       | Description                 |
+| ------------------------------------ | ---------------------------------------------------------- | --------------------------- |
+| `/localization/initialization_state` | autoware_ad_api_msgs::msg::LocalizationInitializationState | pose initialization state   |
+| `/initialpose3d`                     | geometry_msgs::msg::PoseWithCovarianceStamped              | calculated initial ego pose |

localization/pose_initializer/config/pose_initializer.param.yaml

@@ -1,17 +1,6 @@
 /**:
   ros__parameters:
 
-    # from initialpose (Rviz's 2DPoseEstimate)
-    initialpose_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,
-      ]
-
     # from gnss
     gnss_particle_covariance:
       [
@@ -23,17 +12,6 @@
         0.0, 0.0, 0.0,  0.0,  0.0,  10.0,
       ]
 
-    # from service
-    service_particle_covariance:
-      [
-        1.0, 0.0, 0.0,  0.0,  0.0,  0.0,
-        0.0, 1.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,  10.0,
-      ]
-
     # output
     output_pose_covariance:
       [