feat(autoware_pose_covariance_modifier): add new node to early fuse gnss and ndt poses

launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml

@@ -1,5 +1,6 @@
 <?xml version="1.0"?>
 <launch>
+  <arg name="output_pose_with_covariance_topic" default="/localization/pose_estimator/pose_with_covariance"/>
   <group>
     <include file="$(find-pkg-share ndt_scan_matcher)/launch/ndt_scan_matcher.launch.xml">
       <arg name="input_pointcloud" value="/localization/util/downsample/pointcloud"/>
@@ -8,7 +9,7 @@
       <arg name="input_service_trigger_node" value="/localization/pose_estimator/trigger_node"/>
 
       <arg name="output_pose_topic" value="/localization/pose_estimator/pose"/>
-      <arg name="output_pose_with_covariance_topic" value="/localization/pose_estimator/pose_with_covariance"/>
+      <arg name="output_pose_with_covariance_topic" value="$(var output_pose_with_covariance_topic)"/>
 
       <arg name="client_map_loader" value="/map/get_differential_pointcloud_map"/>
 

launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml

@@ -1,5 +1,6 @@
 <?xml version="1.0"?>
 <launch>
+  <arg name="use_autoware_pose_covariance_modifier" default="false"/>
   <!-- only when running with a real vehicle, the pose_initializer judges the stop -->
   <let name="stop_check_enabled" if="$(eval &quot;'$(var system_run_mode)'=='online'&quot;)" value="true"/>
   <let name="stop_check_enabled" if="$(eval &quot;'$(var system_run_mode)'=='logging_simulation'&quot;)" value="false"/>
@@ -29,7 +30,20 @@
   <!-- NDT Scan Matcher Launch (as pose estimator) -->
   <group if="$(var use_ndt_pose)">
     <push-ros-namespace namespace="pose_estimator"/>
-    <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml"/>
+    <let
+      name="ndt_output_pose_with_cov_topic_name"
+      if="$(eval &quot;'$(var use_autoware_pose_covariance_modifier)'=='true'&quot;)"
+      value="/localization/pose_estimator/ndt_scan_matcher/pose_with_covariance"
+    />
+    <let name="ndt_output_pose_with_cov_topic_name" unless="$(eval &quot;'$(var use_autoware_pose_covariance_modifier)'=='true'&quot;)" value="/localization/pose_estimator/pose_with_covariance"/>
+    <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml">
+      <arg name="output_pose_with_covariance_topic" value="$(var ndt_output_pose_with_cov_topic_name)"/>
+    </include>
+
+    <!-- Autoware Pose Covariance Modifier Node Launch -->
+    <group if="$(var use_autoware_pose_covariance_modifier)">
+      <include file="$(find-pkg-share autoware_pose_covariance_modifier)/launch/pose_covariance_modifier.launch.xml"/>
+    </group>
   </group>
 
   <!-- YabLoc Launch (as pose estimator) -->

localization/autoware_pose_covariance_modifier/CMakeLists.txt

@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_pose_covariance_modifier)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/pose_covariance_modifier.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "autoware::pose_covariance_modifier::PoseCovarianceModifierNode"
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+
+ament_auto_package(INSTALL_TO_SHARE
+  config
+  launch
+)

localization/autoware_pose_covariance_modifier/README.md

@@ -0,0 +1,213 @@
+# Autoware Pose Covariance Modifier Node
+
+## Purpose
+
+This package makes it possible to use GNSS and NDT poses together in real time localization.
+
+## Function
+
+This package takes in GNSS (Global Navigation Satellite System)
+and NDT (Normal Distribution Transform) poses with covariances.
+
+It outputs a single pose with covariance:
+
+- Directly the GNSS pose and its covariance.
+- Directly the NDT pose and its covariance.
+- Both GNSS and NDT poses with modified covariances.
+
+> - This package doesn't modify the pose information it receives.
+> - It only modifies NDT covariance values under certain conditions.
+
+## Assumptions
+
+- The NDT matcher provides a pose with a fixed covariance.
+- The NDT matcher is unable to provide a dynamic, reliable covariance value.
+
+## Requirements
+
+- The GNSS/INS module must provide standard deviation values (its error / RMSE) for the position and orientation.
+- It probably needs RTK support to provide accurate position and orientation information.
+- You need to have a geo-referenced map.
+- GNSS/INS module and the base_link frame must be calibrated well enough.
+- In an environment where GNSS/INS and NDT systems work well, the `base_link` poses from both systems should be close to
+  each other.
+
+## Description
+
+GNSS and NDT nodes provide the pose with covariance data utilized in an Extended Kalman Filter (EKF).
+
+Accurate covariance values are crucial for the effectiveness of the EKF in estimating the state.
+
+The GNSS system generates reliable standard deviation values, which can be transformed into covariance measures.
+
+But we currently don't have a reliable way to determine the covariance values for the NDT poses.
+And the NDT matching system in Autoware outputs poses with preset covariance values.
+
+For this reason, this package is designed to manage the selection of the pose source,
+based on the standard deviation values provided by the GNSS system.
+
+It also tunes the covariance values of the NDT poses, based on the GNSS standard deviation values.
+
+## Flowcharts
+
+### Without this package
+
+Only NDT pose is used in localization. GNSS pose is only used for initialization.
+
+```mermaid
+graph TD
+    ndt_scan_matcher["ndt_scan_matcher"] --> |"/localization/pose_estimator/pose_with_covariance"| ekf_localizer["ekf_localizer"]
+
+classDef cl_node fill:#FFF2CC,stroke-width:3px,stroke:#D6B656;
+
+class ndt_scan_matcher cl_node;
+class ekf_localizer cl_node;
+```
+
+### With this package
+
+Both NDT and GNSS poses are used in localization, depending on the standard deviation values coming from the GNSS
+system.
+
+Here is a flowchart depicting the process and the predefined thresholds:
+
+```mermaid
+graph TD
+    gnss_poser["gnss_poser"] --> |"/sensing/gnss/\npose_with_covariance"| pose_covariance_modifier_node
+    ndt_scan_matcher["ndt_scan_matcher"] --> |"/localization/pose_estimator/ndt_scan_matcher/\npose_with_covariance"| pose_covariance_modifier_node
+
+    subgraph pose_covariance_modifier_node ["Pose Covariance Modifier Node"]
+        pc1{{"gnss_pose_yaw\nstddev"}}
+        pc1 -->|"<= 0.3 rad"| pc2{{"gnss_pose_z\nstddev"}}
+        pc2 -->|"<= 0.1 m"| pc3{{"gnss_pose_xy\nstddev"}}
+        pc2 -->|"> 0.1 m"| ndt_pose("NDT Pose")
+        pc3 -->|"<= 0.1 m"| gnss_pose("GNSS Pose")
+        pc3 -->|"0.1 m < x <= 0.2 m"| gnss_ndt_pose("`Both GNSS and NDT Pose
+        (_with modified covariance_)`")
+        pc3 -->|"> 0.2 m"| ndt_pose
+        pc1 -->|"> 0.3 rad"| ndt_pose
+    end
+
+    pose_covariance_modifier_node -->|"/localization/pose_estimator/pose_with_covariance"| ekf_localizer["ekf_localizer"]
+
+classDef cl_node fill:#FFF2CC,stroke-width:3px,stroke:#D6B656;
+classDef cl_conditional fill:#FFE6CC,stroke-width:3px,stroke:#D79B00;
+classDef cl_output fill:#D5E8D4,stroke-width:3px,stroke:#82B366;
+
+class gnss_poser cl_node;
+class ndt_scan_matcher cl_node;
+class ekf_localizer cl_node;
+class pose_covariance_modifier_node cl_node;
+
+class pc1 cl_conditional;
+class pc2 cl_conditional;
+class pc3 cl_conditional;
+
+class ndt_pose cl_output;
+class gnss_pose cl_output;
+class gnss_ndt_pose cl_output;
+```
+
+## How to use this package
+
+> **This package is disabled by default in Autoware, you need to manually enable it.**
+
+To enable this package, you need to change the `use_autoware_pose_covariance_modifier` parameter to `true` within
+the [pose_twist_estimator.launch.xml](../../launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml#L3).
+
+### Without this condition (default)
+
+- The output of the [ndt_scan_matcher](../../localization/ndt_scan_matcher) is directly sent
+  to [ekf_localizer](../../localization/ekf_localizer).
+  - It has a preset covariance value.
+  - **topic name:** `/localization/pose_estimator/pose_with_covariance`
+- The GNSS pose does not enter the ekf_localizer.
+- This node does not launch.
+
+### With this condition
+
+- The output of the [ndt_scan_matcher](../../localization/ndt_scan_matcher) is renamed
+  - **from:** `/localization/pose_estimator/pose_with_covariance`.
+  - **to:** `/localization/pose_estimator/ndt_scan_matcher/pose_with_covariance`.
+- The `ndt_scan_matcher` output enters the `autoware_pose_covariance_modifier`.
+- The output of this package goes to [ekf_localizer](../../localization/ekf_localizer) with:
+  - **topic name:** `/localization/pose_estimator/pose_with_covariance`.
+
+## Node
+
+### Subscribed topics
+
+| Name                             | Type                                            | Description            |
+| -------------------------------- | ----------------------------------------------- | ---------------------- |
+| `input_gnss_pose_with_cov_topic` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Input GNSS pose topic. |
+| `input_ndt_pose_with_cov_topic`  | `geometry_msgs::msg::PoseWithCovarianceStamped` | Input NDT pose topic.  |
+
+### Published topics
+
+| Name                                | Type                                            | Description                                                                                                            |
+| ----------------------------------- | ----------------------------------------------- | ---------------------------------------------------------------------------------------------------------------------- |
+| `output_pose_with_covariance_topic` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Output pose topic. This topic is used by the ekf_localizer package.                                                    |
+| `selected_pose_type`                | `std_msgs::msg::String`                         | Declares which pose sources are used in the output of this package                                                     |
+| `output/ndt_position_stddev`        | `std_msgs::msg::Float64`                        | Output pose ndt average standard deviation in position xy. It is published only when the enable_debug_topics is true.  |
+| `output/gnss_position_stddev`       | `std_msgs::msg::Float64`                        | Output pose gnss average standard deviation in position xy. It is published only when the enable_debug_topics is true. |
+
+### Parameters
+
+The parameters are set
+in [config/autoware_pose_covariance_modifier.param.yaml](config/autoware_pose_covariance_modifier.param.yaml) .
+
+{{ json_to_markdown("
+localization/autoware_pose_covariance_modifier/schema/pose_covariance_modifier.schema.json") }}
+
+## FAQ
+
+### How are varying frequency rates handled?
+
+The GNSS and NDT pose topics may have different frequencies.
+The GNSS pose topic may have a higher frequency than the NDT.
+
+Let's assume that the inputs have the following frequencies:
+
+| Source | Frequency |
+| ------ | --------- |
+| GNSS   | 200 Hz    |
+| NDT    | 10 Hz     |
+
+This package publishes the output poses as they come in, depending on the mode.
+
+End result:
+
+| Mode       | Output Freq |
+| ---------- | ----------- |
+| GNSS Only  | 200 Hz      |
+| GNSS + NDT | 210 Hz      |
+| NDT Only   | 10 Hz       |
+
+### How and when are the NDT covariance values overwritten?
+
+| Mode       | Outputs, Covariance                         |
+| ---------- | ------------------------------------------- |
+| GNSS Only  | GNSS, Unmodified                            |
+| GNSS + NDT | **GNSS:** Unmodified, **NDT:** Interpolated |
+| NDT Only   | NDT, Unmodified                             |
+
+NDT covariance values overwritten only for the `GNSS + NDT` mode.
+
+This enables a smooth transition between `GNSS Only` and `NDT Only` modes.
+
+In this mode, both NDT and GNSS poses are published from this node.
+
+#### NDT covariance calculation
+
+As the `gnss_std_dev` increases within its bounds, `ndt_std_dev` should proportionally decrease within its own bounds.
+
+To achieve this, we first linearly interpolate:
+
+- Base value: `gnss_std_dev`
+- Base range: [`threshold_gnss_stddev_xy_bound_lower`, `threshold_gnss_stddev_xy_bound_upper`]
+- Target range: [`ndt_std_dev_bound_lower`, `ndt_std_dev_bound_upper`]
+- Target value: `ndt_std_dev_target`
+
+- Final value = `ndt_std_dev_bound_lower` + `ndt_std_dev_bound_upper` - `ndt_std_dev_target` (to get the inverse)
+
+<img width="300" src="media/range_lerp.svg" alt="range to range lerp animation">

localization/autoware_pose_covariance_modifier/config/pose_covariance_modifier.param.yaml

@@ -0,0 +1,24 @@
+/**:
+  ros__parameters:
+    # If GNSS yaw standard deviation values are larger than this, trust only NDT
+    threshold_gnss_stddev_yaw_deg_max: 0.3
+
+    # If GNSS position Z standard deviation values are larger than this, trust only NDT
+    threshold_gnss_stddev_z_max: 0.1
+
+    # If GNSS position XY standard deviation values are lower than this, trust only GNSS
+    threshold_gnss_stddev_xy_bound_lower: 0.1
+
+    # If GNSS position XY standard deviation values are higher than this, trust only NDT
+    threshold_gnss_stddev_xy_bound_upper: 0.25
+
+    # Lower bound value for standard deviation of NDT positions (x, y, z) when used with GNSS
+    ndt_std_dev_bound_lower: 0.14
+
+    # Upper bound value for standard deviation of NDT positions (x, y, z) when used with GNSS
+    ndt_std_dev_bound_upper: 0.30
+
+    # If GNSS data is not received for this duration, trust only NDT
+    gnss_pose_timeout_sec: 1.0
+
+    enable_debug_topics: true

localization/autoware_pose_covariance_modifier/launch/pose_covariance_modifier.launch.xml

@@ -0,0 +1,14 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="autoware_pose_covariance_modifier/input_gnss_pose_with_cov_topic" default="/sensing/gnss/pose_with_covariance"/>
+  <arg name="autoware_pose_covariance_modifier/input_ndt_pose_with_cov_topic" default="/localization/pose_estimator/ndt_scan_matcher/pose_with_covariance"/>
+  <arg name="autoware_pose_covariance_modifier/output_pose_with_covariance_topic" default="/localization/pose_estimator/pose_with_covariance"/>
+  <arg name="param_file" default="$(find-pkg-share autoware_pose_covariance_modifier)/config/pose_covariance_modifier.param.yaml"/>
+
+  <node pkg="autoware_pose_covariance_modifier" exec="autoware_pose_covariance_modifier_node" name="autoware_pose_covariance_modifier_node" output="screen">
+    <remap from="input_gnss_pose_with_cov_topic" to="$(var autoware_pose_covariance_modifier/input_gnss_pose_with_cov_topic)"/>
+    <remap from="input_ndt_pose_with_cov_topic" to="$(var autoware_pose_covariance_modifier/input_ndt_pose_with_cov_topic)"/>
+    <remap from="output_pose_with_covariance_topic" to="$(var autoware_pose_covariance_modifier/output_pose_with_covariance_topic)"/>
+    <param from="$(var param_file)"/>
+  </node>
+</launch>

localization/autoware_pose_covariance_modifier/package.xml

@@ -0,0 +1,24 @@
+<?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_pose_covariance_modifier</name>
+  <version>1.0.0</version>
+  <description>Add a description.</description>
+
+  <maintainer email="melike@leodrive.ai">Melike Tanrikulu</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>interpolation</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>std_msgs</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

localization/autoware_pose_covariance_modifier/schema/pose_covariance_modifier.schema.json

@@ -0,0 +1,73 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Pose Covariance Modifier Node Parameters",
+  "type": "object",
+  "actual_parameters": {
+    "type": "object",
+    "properties": {
+      "threshold_gnss_stddev_yaw_deg_max": {
+        "type": "number",
+        "default": 0.3,
+        "description": "If GNSS yaw standard deviation values are larger than this, trust only NDT"
+      },
+      "threshold_gnss_stddev_z_max": {
+        "type": "number",
+        "default": 0.1,
+        "description": "If GNSS position Z standard deviation values are larger than this, trust only NDT"
+      },
+      "threshold_gnss_stddev_xy_bound_lower": {
+        "type": "number",
+        "default": 0.1,
+        "description": "If GNSS position XY standard deviation values are lower than this, trust only GNSS"
+      },
+      "threshold_gnss_stddev_xy_bound_upper": {
+        "type": "number",
+        "default": 0.25,
+        "description": "If GNSS position XY standard deviation values are higher than this, trust only NDT"
+      },
+      "ndt_std_dev_bound_lower": {
+        "type": "number",
+        "default": 0.15,
+        "description": "Lower bound value for standard deviation of NDT positions (x, y, z) when used with GNSS"
+      },
+      "ndt_std_dev_bound_upper": {
+        "type": "number",
+        "default": 0.3,
+        "description": "Upper bound value for standard deviation of NDT positions (x, y, z) when used with GNSS"
+      },
+      "gnss_pose_timeout_sec": {
+        "type": "number",
+        "default": 1.0,
+        "description": "If GNSS data is not received for this duration, trust only NDT"
+      },
+      "enable_debug_topics": {
+        "type": "boolean",
+        "default": true,
+        "description": "Publish additional debug topics"
+      }
+    },
+    "required": [
+      "threshold_gnss_stddev_yaw_deg_max",
+      "threshold_gnss_stddev_z_max",
+      "threshold_gnss_stddev_xy_bound_lower",
+      "threshold_gnss_stddev_xy_bound_upper",
+      "gnss_pose_timeout_sec",
+      "enable_debug_topics"
+    ],
+    "additionalProperties": false
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/actual_parameters"
+        }
+      },
+      "required": ["ros__parameters"],
+      "additionalProperties": false
+    }
+  },
+  "required": ["/**"],
+  "additionalProperties": false
+}

localization/autoware_pose_covariance_modifier/src/include/pose_covariance_modifier.hpp

@@ -0,0 +1,86 @@
+// Copyright 2024 The Autoware Foundation
+//
+// 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 POSE_COVARIANCE_MODIFIER_HPP_
+#define POSE_COVARIANCE_MODIFIER_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <std_msgs/msg/float64.hpp>
+#include <std_msgs/msg/string.hpp>
+
+namespace autoware::pose_covariance_modifier
+{
+class PoseCovarianceModifierNode : public rclcpp::Node
+{
+public:
+  explicit PoseCovarianceModifierNode(const rclcpp::NodeOptions & node_options);
+
+  enum class PoseSource {
+    GNSS = 0,
+    GNSS_NDT = 1,
+    NDT = 2,
+  };
+
+private:
+  // covariance matrix indexes
+  const int X_POS_IDX_ = 0;
+  const int Y_POS_IDX_ = 7;
+  const int Z_POS_IDX_ = 14;
+  const int YAW_POS_IDX_ = 35;
+
+  // parameters
+  double threshold_gnss_stddev_yaw_deg_max_;
+  double threshold_gnss_stddev_z_max_;
+  double threshold_gnss_stddev_xy_bound_lower_;
+  double threshold_gnss_stddev_xy_bound_upper_;
+  double ndt_std_dev_bound_lower_;
+  double ndt_std_dev_bound_upper_;
+  double gnss_pose_timeout_sec_;
+  bool debug_mode_;
+
+  rclcpp::Time gnss_pose_received_time_last_;
+  geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr gnss_pose_with_cov_last_;
+  PoseSource pose_source_;
+
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+    sub_gnss_pose_with_cov_;
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+    sub_ndt_pose_with_cov_;
+  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+    pub_pose_with_covariance_stamped_;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_str_pose_source_;
+  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr pub_double_ndt_position_stddev_;
+  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr pub_double_gnss_position_stddev_;
+
+  void callback_gnss_pose_with_cov(
+    const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg);
+
+  void callback_ndt_pose_with_cov(
+    const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg_pose_with_cov_in);
+
+  bool gnss_pose_has_timed_out(const rclcpp::Time & gnss_pose_received_time_last);
+
+  PoseSource pose_source_from_gnss_stddev(
+    double gnss_pose_yaw_stddev_deg, double gnss_pose_stddev_z, double gnss_pose_stddev_xy) const;
+
+  std::array<double, 36> update_ndt_covariances_from_gnss(
+    const std::array<double, 36> & ndt_covariance_in);
+
+  void publish_pose_type(const PoseSource & pose_source);
+};
+
+}  // namespace autoware::pose_covariance_modifier
+
+#endif  // POSE_COVARIANCE_MODIFIER_HPP_

localization/autoware_pose_covariance_modifier/src/pose_covariance_modifier.cpp

@@ -0,0 +1,246 @@
+// Copyright 2024 The Autoware Foundation
+//
+// 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 "include/pose_covariance_modifier.hpp"
+
+#include <interpolation/linear_interpolation.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <std_msgs/msg/float64.hpp>
+#include <std_msgs/msg/string.hpp>
+
+namespace autoware::pose_covariance_modifier
+{
+using PoseSource = PoseCovarianceModifierNode::PoseSource;
+
+PoseCovarianceModifierNode::PoseCovarianceModifierNode(const rclcpp::NodeOptions & node_options)
+: Node("PoseCovarianceModifierNode", node_options),
+  gnss_pose_received_time_last_(this->now()),
+  pose_source_(PoseSource::NDT)
+{
+  // parameters
+  threshold_gnss_stddev_yaw_deg_max_ =
+    this->declare_parameter<double>("threshold_gnss_stddev_yaw_deg_max");
+  threshold_gnss_stddev_z_max_ = this->declare_parameter<double>("threshold_gnss_stddev_z_max");
+  threshold_gnss_stddev_xy_bound_lower_ =
+    this->declare_parameter<double>("threshold_gnss_stddev_xy_bound_lower");
+  threshold_gnss_stddev_xy_bound_upper_ =
+    this->declare_parameter<double>("threshold_gnss_stddev_xy_bound_upper");
+  ndt_std_dev_bound_lower_ = this->declare_parameter<double>("ndt_std_dev_bound_lower");
+  ndt_std_dev_bound_upper_ = this->declare_parameter<double>("ndt_std_dev_bound_upper");
+  gnss_pose_timeout_sec_ = this->declare_parameter<double>("gnss_pose_timeout_sec");
+  debug_mode_ = this->declare_parameter<bool>("enable_debug_topics");
+
+  // subscribers
+  sub_gnss_pose_with_cov_ =
+    this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+      "input_gnss_pose_with_cov_topic", 10,
+      std::bind(
+        &PoseCovarianceModifierNode::callback_gnss_pose_with_cov, this, std::placeholders::_1));
+
+  sub_ndt_pose_with_cov_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+    "input_ndt_pose_with_cov_topic", 10,
+    std::bind(
+      &PoseCovarianceModifierNode::callback_ndt_pose_with_cov, this, std::placeholders::_1));
+
+  // publishers
+  pub_pose_with_covariance_stamped_ =
+    this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
+      "output_pose_with_covariance_topic", 10);
+
+  pub_str_pose_source_ = this->create_publisher<std_msgs::msg::String>("~/selected_pose_type", 10);
+
+  if (debug_mode_) {
+    pub_double_ndt_position_stddev_ =
+      this->create_publisher<std_msgs::msg::Float64>("~/debug/ndt_position_stddev", 10);
+    pub_double_gnss_position_stddev_ =
+      this->create_publisher<std_msgs::msg::Float64>("~/debug/gnss_position_stddev", 10);
+  }
+}
+
+void PoseCovarianceModifierNode::callback_gnss_pose_with_cov(
+  const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg_pose_with_cov_in)
+{
+  // will be used to check if GNSS pose has timed out in the NDT pose callback
+  gnss_pose_received_time_last_ = this->now();
+
+  // if the pose source is not GNSS, it will be used to calculate the NDT covariance in the NDT pose
+  // callback
+  gnss_pose_with_cov_last_ = msg_pose_with_cov_in;
+
+  const double gnss_pose_yaw_stddev_deg =
+    std::sqrt(msg_pose_with_cov_in->pose.covariance[YAW_POS_IDX_]) * 180 / M_PI;
+
+  const double gnss_pose_stddev_z = std::sqrt(msg_pose_with_cov_in->pose.covariance[Z_POS_IDX_]);
+
+  const double gnss_pose_stddev_xy =
+    (std::sqrt(msg_pose_with_cov_in->pose.covariance[X_POS_IDX_]) +
+     std::sqrt(msg_pose_with_cov_in->pose.covariance[Y_POS_IDX_])) /
+    2;
+
+  pose_source_ =
+    pose_source_from_gnss_stddev(gnss_pose_yaw_stddev_deg, gnss_pose_stddev_z, gnss_pose_stddev_xy);
+  publish_pose_type(pose_source_);
+
+  if (pose_source_ == PoseSource::NDT) {
+    // if the pose source is only NDT, don't publish GNSS poses
+    return;
+  }
+
+  pub_pose_with_covariance_stamped_->publish(*msg_pose_with_cov_in);
+
+  if (debug_mode_) {
+    std_msgs::msg::Float64 msg_double;
+    msg_double.data = gnss_pose_stddev_xy;
+    pub_double_gnss_position_stddev_->publish(msg_double);
+  }
+}
+
+void PoseCovarianceModifierNode::callback_ndt_pose_with_cov(
+  const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg_pose_with_cov_in)
+{
+  if (pose_source_ == PoseSource::GNSS) {
+    // if the pose source is only gnss, GNSS pose will be used in the GNSS pose callback
+    return;
+  }
+  geometry_msgs::msg::PoseWithCovarianceStamped msg_pose_with_cov_out;
+
+  // pose_source_ was determined in the GNSS callback
+  if (gnss_pose_has_timed_out(gnss_pose_received_time_last_) || pose_source_ == PoseSource::NDT) {
+    msg_pose_with_cov_out = *msg_pose_with_cov_in;
+  } else if (pose_source_ == PoseSource::GNSS_NDT) {
+    auto ndt_pose_with_cov_updated = *msg_pose_with_cov_in;
+    ndt_pose_with_cov_updated.pose.covariance =
+      update_ndt_covariances_from_gnss(msg_pose_with_cov_in->pose.covariance);
+    msg_pose_with_cov_out = ndt_pose_with_cov_updated;
+  }
+
+  pub_pose_with_covariance_stamped_->publish(msg_pose_with_cov_out);
+
+  if (debug_mode_) {
+    std_msgs::msg::Float64 msg_double;
+    msg_double.data = (std::sqrt(msg_pose_with_cov_out.pose.covariance[X_POS_IDX_]) +
+                       std::sqrt(msg_pose_with_cov_out.pose.covariance[Y_POS_IDX_])) /
+                      2.0;
+    pub_double_ndt_position_stddev_->publish(msg_double);
+  }
+}
+
+bool PoseCovarianceModifierNode::gnss_pose_has_timed_out(
+  const rclcpp::Time & gnss_pose_received_time_last)
+{
+  auto duration = this->now() - gnss_pose_received_time_last;
+  if (duration.seconds() > gnss_pose_timeout_sec_) {
+    RCLCPP_WARN(this->get_logger(), "GNSS pose has timed out");
+    return true;
+  }
+  return false;
+}
+
+PoseSource PoseCovarianceModifierNode::pose_source_from_gnss_stddev(
+  const double gnss_pose_yaw_stddev_deg, const double gnss_pose_stddev_z,
+  const double gnss_pose_stddev_xy) const
+{
+  // If the GNSS pose z or yaw has a high standard deviation, use NDT pose
+  if (
+    gnss_pose_yaw_stddev_deg > threshold_gnss_stddev_yaw_deg_max_ ||
+    gnss_pose_stddev_z > threshold_gnss_stddev_z_max_) {
+    return PoseSource::NDT;
+  }
+
+  if (gnss_pose_stddev_xy <= threshold_gnss_stddev_xy_bound_lower_) {
+    return PoseSource::GNSS;
+  } else if (gnss_pose_stddev_xy <= threshold_gnss_stddev_xy_bound_upper_) {
+    return PoseSource::GNSS_NDT;
+  }
+  // If the gnss xy standard deviation is above the upper bound, use NDT pose
+  return PoseSource::NDT;
+}
+
+std::array<double, 36> PoseCovarianceModifierNode::update_ndt_covariances_from_gnss(
+  const std::array<double, 36> & ndt_covariance_in)
+{
+  // See the ../README.md#NDT-covariance-calculation for detailed explanation
+
+  auto lerp_range_to_range = [](double x, double x_min, double x_max, double y_min, double y_max) {
+    // Normalize input value to range [0, 1]
+    const double input_normalized = (x - x_min) / (x_max - x_min);
+
+    // Interpolate to the output range
+    return interpolation::lerp(y_min, y_max, input_normalized);
+  };
+
+  auto ndt_variance_from_gnss_variance = [&](double ndt_variance, double gnss_variance) {
+    // Check NDT stddev bound values.
+    double ndt_stddev = std::sqrt(ndt_variance);
+    if (ndt_stddev > ndt_std_dev_bound_upper_ || ndt_stddev < ndt_std_dev_bound_lower_) {
+      RCLCPP_ERROR(
+        get_logger(),
+        "Input variance of NDT exceeds bound values. Variance values of NDT were not modified. "
+        "Check your bound values for NDT stddev.");
+      return ndt_variance;
+    }
+    // calculate NDT covariance value based on gnss covariance
+    const double gnss_std_dev = std::sqrt(gnss_variance);
+
+    // interpolate the gnss_std_dev from gnss ranges to ndt ranges
+    const double interpolated_std_dev = lerp_range_to_range(
+      gnss_std_dev, threshold_gnss_stddev_xy_bound_lower_, threshold_gnss_stddev_xy_bound_upper_,
+      ndt_std_dev_bound_lower_, ndt_std_dev_bound_upper_);
+
+    // As the gnss error increases, the ndt error should decrease
+    const double reversed_std_dev =
+      ndt_std_dev_bound_lower_ + ndt_std_dev_bound_upper_ - interpolated_std_dev;
+
+    const double interpolated_variance = std::pow(reversed_std_dev, 2);
+
+    // Make sure the ndt covariance is not below the lower bounds of ndt covariance value and return
+    return (std::max(interpolated_variance, std::pow(ndt_std_dev_bound_lower_, 2)));
+  };
+
+  std::array<double, 36> ndt_covariance = ndt_covariance_in;
+  std::array<int, 3> indices = {X_POS_IDX_, Y_POS_IDX_, Z_POS_IDX_};
+  for (int idx : indices) {
+    ndt_covariance[idx] = ndt_variance_from_gnss_variance(
+      ndt_covariance_in[idx], gnss_pose_with_cov_last_->pose.covariance[idx]);
+  }
+
+  return ndt_covariance;
+}
+
+void PoseCovarianceModifierNode::publish_pose_type(const PoseSource & pose_source)
+{
+  std_msgs::msg::String selected_pose_type;
+  switch (pose_source) {
+    case PoseSource::GNSS:
+      selected_pose_type.data = "GNSS";
+      break;
+    case PoseSource::GNSS_NDT:
+      selected_pose_type.data = "GNSS_NDT";
+      break;
+    case PoseSource::NDT:
+      selected_pose_type.data = "NDT";
+      break;
+    default:
+      selected_pose_type.data = "NOT_DEFINED";
+      break;
+  }
+  pub_str_pose_source_->publish(selected_pose_type);
+}
+
+}  // namespace autoware::pose_covariance_modifier
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(autoware::pose_covariance_modifier::PoseCovarianceModifierNode)