feat(pose_initializer)!: support ad api

common/autoware_ad_api_msgs/README.md

@@ -25,7 +25,15 @@ The version of AD API follows [Semantic Versioning][semver] in order to provide
 
 The routing service support two formats. One uses pose and the other uses map dependent data directly.
 The body part of the route message is optional, since the route does not exist when it is cleared by the service.
+[See also routing API][api-routing].
+
+## Localization
+
+The initialization state does not reflect localization errors. Use diagnostics for that purpose.
+[See also localization API][api-localization].
 
 <!-- link -->
 
 [semver]: https://semver.org/
+[api-localization]: https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/localization/
+[api-routing]: https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/routing/

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

@@ -4,6 +4,7 @@
   <arg name="input/pointcloud" description="input topic name"/>
   <arg name="tier4_localization_launch_param_path" description="tier4_localization_launch param path"/>
   <arg name="output/pointcloud" default="downsample/pointcloud" description="final output topic name"/>
+
   <!-- container -->
   <arg name="container" default="/sensing/lidar/top/pointcloud_preprocessor/velodyne_node_container" description="container name"/>
 
@@ -12,9 +13,18 @@
 
   <!-- 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>
+
+  <!-- pose_initializer (Automatic call from AD API) -->
+  <group>
     <include file="$(find-pkg-share automatic_pose_initializer)/launch/automatic_pose_initializer.launch.xml"/>
   </group>
+
   <!-- util -->
   <group>
     <include file="$(find-pkg-share tier4_localization_launch)/launch/util/util.launch.py">

launch/tier4_simulator_launch/launch/simulator.launch.xml

@@ -4,6 +4,7 @@
 
   <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"/>
@@ -92,6 +93,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

@@ -1,24 +1,4 @@
 # pose_initializer
 
-## Purpose
-
-`pose_initializer` is the package to send an initial pose to `ekf_localizer`.
-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
-
-### Input topics
-
-| 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         |
-
-### Output topics
-
-| Name             | Type                                          | Description                 |
-| ---------------- | --------------------------------------------- | --------------------------- |
-| `/initialpose3d` | geometry_msgs::msg::PoseWithCovarianceStamped | calculated initial ego pose |
+- [pose_initializer](./docs/pose_initializer.md)
+- [map_height_fitter](./docs/map_height_fitter.md)

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:
       [

localization/pose_initializer/docs/map_height_fitter.md

@@ -0,0 +1,22 @@
+# map_height_fitter
+
+## Purpose
+
+This node provides a service to fit a pose height to a map.
+Use this service as preprocessing for `pose_initializer` when using a initial poses with inaccurate height such as RViz and GNSS.
+This service replaces the Z value of the input pose with the lowest point of the map point cloud within a cylinder of XY-radius.
+If no point is found in this range, returns the input pose without changes.
+
+## Interfaces
+
+### Services
+
+| Name                                | Type                                                    | Description          |
+| ----------------------------------- | ------------------------------------------------------- | -------------------- |
+| `/localization/util/fit_map_height` | tier4_localization_msgs::srv::PoseWithCovarianceStamped | pose fitting service |
+
+### Subscriptions
+
+| Name                  | Type                          | Description    |
+| --------------------- | ----------------------------- | -------------- |
+| `/map/pointcloud_map` | sensor_msgs::msg::PointCloud2 | pointcloud map |

localization/pose_initializer/docs/pose_initializer.md

@@ -0,0 +1,46 @@
+# pose_initializer
+
+## Purpose
+
+`pose_initializer` is the package to send an initial pose to `ekf_localizer`.
+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`.
+
+## Interfaces
+
+### Parameters
+
+| 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.                                                |
+
+### Services
+
+| 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 |
+
+### 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 |