|
| 1 | +# Autoware Pose Covariance Modifier Node |
| 2 | + |
| 3 | +## Purpose |
| 4 | + |
| 5 | +This package makes it possible to use GNSS and NDT poses together in real time localization. |
| 6 | + |
| 7 | +## Function |
| 8 | + |
| 9 | +This package takes in GNSS (Global Navigation Satellite System) |
| 10 | +and NDT (Normal Distribution Transform) poses with covariances. |
| 11 | + |
| 12 | +It outputs a single pose with covariance: |
| 13 | + |
| 14 | +- Directly the GNSS pose and its covariance. |
| 15 | +- Directly the NDT pose and its covariance. |
| 16 | +- Both GNSS and NDT poses with modified covariances. |
| 17 | + |
| 18 | +> - This package doesn't modify the pose information it receives. |
| 19 | +> - It only modifies NDT covariance values under certain conditions. |
| 20 | +
|
| 21 | +## Assumptions |
| 22 | + |
| 23 | +- The NDT matcher provides a pose with a fixed covariance. |
| 24 | +- The NDT matcher is unable to provide a dynamic, reliable covariance value. |
| 25 | + |
| 26 | +## Requirements |
| 27 | + |
| 28 | +- The GNSS/INS module must provide standard deviation values (its error / RMSE) for the position and orientation. |
| 29 | +- It probably needs RTK support to provide accurate position and orientation information. |
| 30 | +- You need to have a geo-referenced map. |
| 31 | +- GNSS/INS module and the base_link frame must be calibrated well enough. |
| 32 | +- In an environment where GNSS/INS and NDT systems work well, the `base_link` poses from both systems should be close to |
| 33 | + each other. |
| 34 | + |
| 35 | +## Description |
| 36 | + |
| 37 | +GNSS and NDT nodes provide the pose with covariance data utilized in an Extended Kalman Filter (EKF). |
| 38 | + |
| 39 | +Accurate covariance values are crucial for the effectiveness of the EKF in estimating the state. |
| 40 | + |
| 41 | +The GNSS system generates reliable standard deviation values, which can be transformed into covariance measures. |
| 42 | + |
| 43 | +But we currently don't have a reliable way to determine the covariance values for the NDT poses. |
| 44 | +And the NDT matching system in Autoware outputs poses with preset covariance values. |
| 45 | + |
| 46 | +For this reason, this package is designed to manage the selection of the pose source, |
| 47 | +based on the standard deviation values provided by the GNSS system. |
| 48 | + |
| 49 | +It also tunes the covariance values of the NDT poses, based on the GNSS standard deviation values. |
| 50 | + |
| 51 | +## Flowcharts |
| 52 | + |
| 53 | +### Without this package |
| 54 | + |
| 55 | +Only NDT pose is used in localization. GNSS pose is only used for initialization. |
| 56 | + |
| 57 | +```mermaid |
| 58 | +graph TD |
| 59 | + ndt_scan_matcher["ndt_scan_matcher"] --> |"/localization/pose_estimator/pose_with_covariance"| ekf_localizer["ekf_localizer"] |
| 60 | +
|
| 61 | +classDef cl_node fill:#FFF2CC,stroke-width:3px,stroke:#D6B656; |
| 62 | +
|
| 63 | +class ndt_scan_matcher cl_node; |
| 64 | +class ekf_localizer cl_node; |
| 65 | +``` |
| 66 | + |
| 67 | +### With this package |
| 68 | + |
| 69 | +Both NDT and GNSS poses are used in localization, depending on the standard deviation values coming from the GNSS |
| 70 | +system. |
| 71 | + |
| 72 | +Here is a flowchart depicting the process and the predefined thresholds: |
| 73 | + |
| 74 | +```mermaid |
| 75 | +graph TD |
| 76 | + gnss_poser["gnss_poser"] --> |"/sensing/gnss/\npose_with_covariance"| pose_covariance_modifier_node |
| 77 | + ndt_scan_matcher["ndt_scan_matcher"] --> |"/localization/pose_estimator/ndt_scan_matcher/\npose_with_covariance"| pose_covariance_modifier_node |
| 78 | +
|
| 79 | + subgraph pose_covariance_modifier_node ["Pose Covariance Modifier Node"] |
| 80 | + pc1{{"gnss_pose_yaw\nstddev"}} |
| 81 | + pc1 -->|"<= 0.3 rad"| pc2{{"gnss_pose_z\nstddev"}} |
| 82 | + pc2 -->|"<= 0.1 m"| pc3{{"gnss_pose_xy\nstddev"}} |
| 83 | + pc2 -->|"> 0.1 m"| ndt_pose("NDT Pose") |
| 84 | + pc3 -->|"<= 0.1 m"| gnss_pose("GNSS Pose") |
| 85 | + pc3 -->|"0.1 m < x <= 0.2 m"| gnss_ndt_pose("`Both GNSS and NDT Pose |
| 86 | + (_with modified covariance_)`") |
| 87 | + pc3 -->|"> 0.2 m"| ndt_pose |
| 88 | + pc1 -->|"> 0.3 rad"| ndt_pose |
| 89 | + end |
| 90 | +
|
| 91 | + pose_covariance_modifier_node -->|"/localization/pose_estimator/pose_with_covariance"| ekf_localizer["ekf_localizer"] |
| 92 | +
|
| 93 | +classDef cl_node fill:#FFF2CC,stroke-width:3px,stroke:#D6B656; |
| 94 | +classDef cl_conditional fill:#FFE6CC,stroke-width:3px,stroke:#D79B00; |
| 95 | +classDef cl_output fill:#D5E8D4,stroke-width:3px,stroke:#82B366; |
| 96 | +
|
| 97 | +class gnss_poser cl_node; |
| 98 | +class ndt_scan_matcher cl_node; |
| 99 | +class ekf_localizer cl_node; |
| 100 | +class pose_covariance_modifier_node cl_node; |
| 101 | +
|
| 102 | +class pc1 cl_conditional; |
| 103 | +class pc2 cl_conditional; |
| 104 | +class pc3 cl_conditional; |
| 105 | +
|
| 106 | +class ndt_pose cl_output; |
| 107 | +class gnss_pose cl_output; |
| 108 | +class gnss_ndt_pose cl_output; |
| 109 | +``` |
| 110 | + |
| 111 | +## How to use this package |
| 112 | + |
| 113 | +> **This package is disabled by default in Autoware, you need to manually enable it.** |
| 114 | +
|
| 115 | +To enable this package, you need to change the `use_autoware_pose_covariance_modifier` parameter to `true` within |
| 116 | +the [pose_twist_estimator.launch.xml](../../launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml#L3). |
| 117 | + |
| 118 | +### Without this condition (default) |
| 119 | + |
| 120 | +- The output of the [ndt_scan_matcher](../../localization/ndt_scan_matcher) is directly sent |
| 121 | + to [ekf_localizer](../../localization/ekf_localizer). |
| 122 | + - It has a preset covariance value. |
| 123 | + - **topic name:** `/localization/pose_estimator/pose_with_covariance` |
| 124 | +- The GNSS pose does not enter the ekf_localizer. |
| 125 | +- This node does not launch. |
| 126 | + |
| 127 | +### With this condition |
| 128 | + |
| 129 | +- The output of the [ndt_scan_matcher](../../localization/ndt_scan_matcher) is renamed |
| 130 | + - **from:** `/localization/pose_estimator/pose_with_covariance`. |
| 131 | + - **to:** `/localization/pose_estimator/ndt_scan_matcher/pose_with_covariance`. |
| 132 | +- The `ndt_scan_matcher` output enters the `autoware_pose_covariance_modifier`. |
| 133 | +- The output of this package goes to [ekf_localizer](../../localization/ekf_localizer) with: |
| 134 | + - **topic name:** `/localization/pose_estimator/pose_with_covariance`. |
| 135 | + |
| 136 | +## Node |
| 137 | + |
| 138 | +### Subscribed topics |
| 139 | + |
| 140 | +| Name | Type | Description | |
| 141 | +| -------------------------------- | ----------------------------------------------- | ---------------------- | |
| 142 | +| `input_gnss_pose_with_cov_topic` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Input GNSS pose topic. | |
| 143 | +| `input_ndt_pose_with_cov_topic` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Input NDT pose topic. | |
| 144 | + |
| 145 | +### Published topics |
| 146 | + |
| 147 | +| Name | Type | Description | |
| 148 | +| ----------------------------------- | ----------------------------------------------- | ---------------------------------------------------------------------------------------------------------------------- | |
| 149 | +| `output_pose_with_covariance_topic` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Output pose topic. This topic is used by the ekf_localizer package. | |
| 150 | +| `selected_pose_type` | `std_msgs::msg::String` | Declares which pose sources are used in the output of this package | |
| 151 | +| `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. | |
| 152 | +| `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. | |
| 153 | + |
| 154 | +### Parameters |
| 155 | + |
| 156 | +The parameters are set |
| 157 | +in [config/pose_covariance_modifier.param.yaml](config/pose_covariance_modifier.param.yaml) . |
| 158 | + |
| 159 | +{{ json_to_markdown(" |
| 160 | +localization/autoware_pose_covariance_modifier/schema/pose_covariance_modifier.schema.json") }} |
| 161 | + |
| 162 | +## FAQ |
| 163 | + |
| 164 | +### How are varying frequency rates handled? |
| 165 | + |
| 166 | +The GNSS and NDT pose topics may have different frequencies. |
| 167 | +The GNSS pose topic may have a higher frequency than the NDT. |
| 168 | + |
| 169 | +Let's assume that the inputs have the following frequencies: |
| 170 | + |
| 171 | +| Source | Frequency | |
| 172 | +| ------ | --------- | |
| 173 | +| GNSS | 200 Hz | |
| 174 | +| NDT | 10 Hz | |
| 175 | + |
| 176 | +This package publishes the output poses as they come in, depending on the mode. |
| 177 | + |
| 178 | +End result: |
| 179 | + |
| 180 | +| Mode | Output Freq | |
| 181 | +| ---------- | ----------- | |
| 182 | +| GNSS Only | 200 Hz | |
| 183 | +| GNSS + NDT | 210 Hz | |
| 184 | +| NDT Only | 10 Hz | |
| 185 | + |
| 186 | +### How and when are the NDT covariance values overwritten? |
| 187 | + |
| 188 | +| Mode | Outputs, Covariance | |
| 189 | +| ---------- | ------------------------------------------- | |
| 190 | +| GNSS Only | GNSS, Unmodified | |
| 191 | +| GNSS + NDT | **GNSS:** Unmodified, **NDT:** Interpolated | |
| 192 | +| NDT Only | NDT, Unmodified | |
| 193 | + |
| 194 | +NDT covariance values overwritten only for the `GNSS + NDT` mode. |
| 195 | + |
| 196 | +This enables a smooth transition between `GNSS Only` and `NDT Only` modes. |
| 197 | + |
| 198 | +In this mode, both NDT and GNSS poses are published from this node. |
| 199 | + |
| 200 | +#### NDT covariance calculation |
| 201 | + |
| 202 | +As the `gnss_std_dev` increases within its bounds, `ndt_std_dev` should proportionally decrease within its own bounds. |
| 203 | + |
| 204 | +To achieve this, we first linearly interpolate: |
| 205 | + |
| 206 | +- Base value: `gnss_std_dev` |
| 207 | +- Base range: [`threshold_gnss_stddev_xy_bound_lower`, `threshold_gnss_stddev_xy_bound_upper`] |
| 208 | +- Target range: [`ndt_std_dev_bound_lower`, `ndt_std_dev_bound_upper`] |
| 209 | +- Target value: `ndt_std_dev_target` |
| 210 | + |
| 211 | +- Final value = `ndt_std_dev_bound_lower` + `ndt_std_dev_bound_upper` - `ndt_std_dev_target` (to get the inverse) |
| 212 | + |
| 213 | +<img width="300" src="doc/range_lerp.svg" alt="range to range lerp animation"> |
0 commit comments