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

[meliketanrikulu]

Description

Closes:

• Developing a suitable method to use NDT and GNSS together in localization #6846

Related discussion:

• [Proposal] Using GNSS/INS and NDT Together in Localization #4134

This PR was created to use NDT and GNSS together in localization. In order for EKF to work properly, the covariance values of the given inputs must be provided correctly as input. In Default AW, NDT covariance values are provided with default values. GNSS, on the other hand, provides its own error in a healthy way.

This PR also suggests that we drive with reference to the trusted source (GNSS), which can reliably calculate the error. It aims to drive with GNSS alone when the GNSS error is low, to use NDT-GNSS together when the GNSS error increases, and to use only NDT alone when the GNSS error goes beyond acceptable limits.

Without this package

Only NDT pose is used in localization. GNSS pose is only used for initialization.

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;

Loading

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:

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;

Loading

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.

Assumptions

• Incoming NDT poses have a fixed covariance value.
• Covariance provided by GNSS is reliable.

NDT covariance calculation

Goal

• 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)

Illustration

Tests performed

The testing steps are shown in the video below:

Installation

1. Download and unpack a test map files.

• You can also download the map manually.

gdown --id 1Xx4H6qou_Z764_vXpQTYsh6gio4aOJ8C -O ~/autoware_data/  
unzip -d ~/autoware_data/ ~/autoware_data/autoware_map.zip

• You can install this map for testing challenging environments for NDT.

2. Download the test rosbag files.

• You can also download the rosbag files manually.

gdown --id 1E88tbUZcpVKzoQOOiQ7Jvcsx-IcgrS4H -O ~/autoware_data/  
unzip -d ~/autoware_data/ ~/autoware_data/rosbag2_2024_04_17-18_36_41.zip

Prepare Autoware to test PR:

1. Checkout autoware.universe :

cd ~/autoware/src/universe/autoware.universe/
git remote add autoware.universe https://github.com/meliketanrikulu/autoware.universe.git
git remote update
git checkout feat/add_autoware_pose_covariance_modifier_node

2. Checkout autoware_launch (Added only to test PR. Not required for PR to work):

cd ~/autoware/src/launcher/autoware_launch/
git remote add autoware_launch https://github.com/meliketanrikulu/autoware_launch.git
git remote update
git checkout pr_test-6570

3. Compile new package:

cd ~/autoware
colcon build --symlink-install --packages-select autoware_pose_covariance_modifier_node autoware_launch

Parameter settings:

1. Set use_autoware_pose_covariance_modifier param to true.

    <arg name="use_autoware_pose_covariance_modifier" default="true"/>

4. Set enable_debug_topics parameter to true.

  enable_debug_topics: true

Launch Autoware:

1. Run autoware :

source ~/autoware/install/setup.bash
ros2 launch autoware_launch autoware_pose_cov_modifier_logging_simulator.launch.xml map_path:=$HOME/autoware_data/autoware_map

2. Run bag file.

source ~/autoware/install/setup.bash 

# Run this to observe results for all data
ros2 bag play ~/autoware_data/rosbag2_2024_04_17-18_36_41/rosbag2_2024_04_17-18_36_41_0.db3 

# If you want to observe switch moments between NDT and GNSS run this
ros2 bag play ~/autoware_data/rosbag2_2024_04_17-18_36_41/rosbag2_2024_04_17-18_36_41_0.db3  --start-offset 300

Check Out The Results:

Test Video

Note

Explanations for the video:

• Test environments: In the video, PR is tested in two different environments. One of these is the environment where NDT does not work well. The other is the environment where GNSS does not work well.
• What do we want to see: We see that the default autoware begins to break down in an environment where NDT does not work well. However, since driving is done with GNSS when using PR, it is possible to drive easily in these environments. Likewise, when the GNSS error increases in the second environment, driving is switched to NDT, so it does not cause any disruption in the current system.
• Ground Truth: In this video, the output of the GNSS/INS sensor and its error in x,y,z are visualized. It can be considered ground truth in places where the error is low.

Check Out Debug Topics

• Follow up which poses are used as a pose source:
  ros2 topic echo /localization/autoware_pose_covariance_modifier/selected_pose_type
• Follow up debug topics:
  ros2 topic echo /autoware_pose_covariance_modifier/output/gnss_position_stddev
ros2 topic echo /autoware_pose_covariance_modifier/output/ndt_position_stddev

• Instead of this you can follow up these values on plotjuggler screen .
  Below you can see the results when you start using ndt while using gnss:
  

Notes for reviewers

Interface changes

Effects on system behavior

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.
• The PR has been properly tested.
• The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.
• The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.

[codecov]

Codecov Report

Attention: Patch coverage is 0% with 90 lines in your changes are missing coverage. Please review.

Project coverage is 0.00%. Comparing base (b6d7477) to head (bf40cca).
Report is 2 commits behind head on main.

Files	Patch %	Lines
...variance_modifier/src/pose_covariance_modifier.cpp	0.00%	90 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main   #6570       +/-   ##
==========================================
- Coverage   15.41%   0.00%   -15.42%     
==========================================
  Files        1904       1     -1903     
  Lines      132758      90   -132668     
  Branches    42265       0    -42265     
==========================================
- Hits        20468       0    -20468     
+ Misses      89541      90    -89451     
+ Partials    22749       0    -22749     

Flag	Coverage Δ
differential	0.00% <0.00%> (?)
total	?

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[meliketanrikulu]

Its ready for the review. Could you check. I would be happy if you give feedback.

[xmfcx]

Your CI is failing:

• https://results.pre-commit.ci/run/github/429651616/1710355300.bRirR-8cSQiZ7Hk9IjlQLQ

[TaikiYamada4]

@meliketanrikulu
Thank you for your contribution!
However, I want this package to follow the architecture written in the Autoware documents.
We want to keep this structure in shape, and I'm afraid to say that it is not acceptable for me to put a node between the ndt_scan_matcher (Pose Estimator) and ekf_localizer (Kinematics Fusion Filter). To say things short, I think it is rather better to create a new pose_estimator that realize your proposal.

I understand that plugging in a simple node between NDT and EKF is more efficient implementation-wise, but a bit of architecture change can grow to a chaos. So could you please try to create a node that fit this architecture?

[xmfcx]

Rebased it to latest main. @TaikiYamada4 I've applied your suggestions.

#6570 (comment) See this for the final test results.

If you are fine with it, we can merge it.

[TaikiYamada4]

@meliketanrikulu @xmfcx
Thank you for accommodating my revision requests for so long!
Looks good to me!