fix(ekf_localizer): add_delay_compensation_for_roll_and_pitch #8095

meliketanrikulu · 2024-07-18T09:46:39Z

Description

We can see that there is a wobbling of ego vehicle points as the vehicle drives through speed bumps.I repeated the tests in the simulation environment and obtained similar results. When I tried to optimize parameters in its current EKF , I saw that I could not achieve good results in its current packet. You can find the tests I have previously done to solve this problem in the #6993 comments

Delay compensation has been added to improve roll and pitch estimation.

How was this PR tested?

1.) I tested this PR with rosbag file . Added delay compensation for roll and pitch estimation. This PR includes this changes.
2.) I updated 1D Filter params for pitch estimation.
Changed pitch_filter_proc_dev param as 0.5
3.) Enabled 3D Distortion Corrector (optionally)

Test Procedure

Installation

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

Download the test rosbag files.

(Test without 3D Distortion Corrector -default-)You can also download the rosbag file manually.

gdown --id 17qpFAmRT_iKnll8IGzApIs7qmJN1bgoD -O ~/autoware_data/  
unzip -d ~/autoware_data/ ~/autoware_data/rosbag2_2024_07_23-12_46_03.zip

(Test with 3D Distortion Corrector -optional-)You can also download the rosbag file manually.

gdown --id 1LjF49QosxdoZDRtRSyy--jR9GMyfq9pZ -O ~/autoware_data/  
unzip -d ~/autoware_data/ ~/autoware_data/rosbag2_2024_07_23-12_36_27.zip

Prepare Autoware to test PR:

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 "fix(ekf_localizer)add_delay_compensation_for_roll_and_pitch"

Checkout autoware_launch :

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

Compile required packages :

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

Launch Autoware

Run Autoware :

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

Run bag file :

Run this for testing without 3D Distortion Corrector - Default-

source ~/autoware/install/setup.bash
ros2 bag play ~/autoware_data/rosbag2_2024_07_23-12_46_03/rosbag2_2024_07_23-12_46_03_0.db3

Run this for testing with 3D Distortion Corrector. It can be improve the results. -Optional-

source ~/autoware/install/setup.bash
ros2 bag play ~/autoware_data/rosbag2_2024_07_23-12_36_27/rosbag2_2024_07_23-12_36_27_0.db3

Test Results:

You can see changes Before/After this PR video is here
After this test I added 3D Distortion Corrector too . Final Test Video is here.

You can see below, the delay compensation effect on EKF output poses with roll and pitch delay compensation :

Comparison with Previous Works:

I have previously tried to solve the problem by simply changing parameters with the existing autoware, but I could not achieve successful results. You can find the tests I performed at this stage under the related issue.

Previous PR :
To solve this problem, I previously suggested and tested adding imu to EKF. The addition of IMU also contributed positively to the result. However, since changing Autoware's architecture was not our first choice, I worked on a new method. As a result, I saw that the problem could be solved by making delay compensation for roll and pitch in the ekf_localizer package and I created this PR. I aim to explain in the image below how close the PR I have worked on before is to the result we expected (NDT only localization) and how close this PR is to the result we expected not to happen (NDT only localization).

Below, you can review the images captured at the moments when the difference between the instantaneous point cloud and the map was maximized while passing through the speed bump. (Normally instant pc and map points are matching but they move away when passing through a speed bump. You can see here the maximum distances (errors)):
Note : Ignore the horizontal differences between the two point clouds here.

Notes for reviewers

Interface changes

None.

Effects on system behavior

None.

github-actions · 2024-07-18T09:46:59Z

Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

You've checked our contribution guidelines.
Your PR follows our pull request guidelines.
All required CI checks pass before marking the PR ready for review.

meliketanrikulu · 2024-07-18T10:14:47Z

/review

github-actions · 2024-07-18T10:15:37Z

PR Reviewer Guide 🔍

(Review updated until commit `cff8ac9`)

⏱️ Estimated effort to review: 3 🔵🔵🔵⚪⚪
🧪 No relevant tests
🔒 No security concerns identified
⚡ Key issues to review Code Smell The function `compensate_roll_pitch_height_with_delay` is added but it is not clear if it handles edge cases such as when `delay_time` is negative or extremely large. Consider adding validation or comments to clarify this. Possible Bug The variable `pose_with_delay` is auto-deduced. Ensure that it is of the correct type `geometry_msgs::msg::PoseWithCovarianceStamped` to avoid potential type mismatches. Code Smell The function `compensate_roll_pitch_height_with_delay` directly modifies the `pose_with_delay` object. Consider using a more functional approach to avoid side effects.

meliketanrikulu · 2024-07-18T10:20:21Z

/improve --extended

localization/ekf_localizer/src/ekf_module.cpp

github-actions · 2024-07-18T10:21:23Z

localization/ekf_localizer/src/ekf_module.cpp

+  roll_rate_ = twist.twist.twist.angular.x;
+  pitch_rate_ = twist.twist.twist.angular.y;


Suggestion: To avoid potential issues with uninitialized variables, ensure that roll_rate_ and pitch_rate_ are initialized directly with the values from twist. [possible issue, importance: 7]

Suggested change

roll_rate_ = twist.twist.twist.angular.x;

pitch_rate_ = twist.twist.twist.angular.y;

roll_rate_ = twist.twist.twist.angular.x != 0.0 ? twist.twist.twist.angular.x : roll_rate_;

pitch_rate_ = twist.twist.twist.angular.y != 0.0 ? twist.twist.twist.angular.y : pitch_rate_;

codecov · 2024-07-23T08:03:35Z

Codecov Report

Attention: Patch coverage is 9.52381% with 19 lines in your changes missing coverage. Please review.

Project coverage is 29.27%. Comparing base (3d849e9) to head (396753d).
Report is 978 commits behind head on main.

Files	Patch %	Lines
localization/ekf_localizer/src/ekf_module.cpp	0.00%	15 Missing ⚠️
localization/ekf_localizer/src/ekf_localizer.cpp	33.33%	4 Missing ⚠️

Additional details and impacted files

@@             Coverage Diff             @@
##             main    #8095       +/-   ##
===========================================
+ Coverage   15.09%   29.27%   +14.18%     
===========================================
  Files        1967     1595      -372     
  Lines      135941   117487    -18454     
  Branches    42122    50609     +8487     
===========================================
+ Hits        20520    34395    +13875     
+ Misses      92700    73895    -18805     
+ Partials    22721     9197    -13524

Flag	Coverage Δ		*Carryforward flag
differential	`22.14% <9.52%> (?)`
total	`29.27% <ø> (+14.18%)`	⬆️	Carriedforward from e33ab61

*This pull request uses carry forward flags. Click here to find out more.

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

TaikiYamada4

LGTM!

YamatoAndo · 2024-07-30T08:20:08Z

@meliketanrikulu I am concerned about the case when the twist msg is interrupted during operation.
Please make sure it does not retain the previous values.

Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai>

meliketanrikulu · 2024-07-30T09:08:49Z

@meliketanrikulu I am concerned about the case when the twist msg is interrupted during operation. Please make sure it does not retain the previous values.

Hello @YamatoAndo . I tested without twist msg.
Here delta_orientation comes as (0,0,0,1) so comes out curr_orientation = prew_orientation as a result of this line . It actually works the same way as before the delay compensation.

TaikiYamada4 · 2024-07-30T10:08:41Z

@meliketanrikulu I am concerned about the case when the twist msg is interrupted during operation. Please make sure it does not retain the previous values.

Hello @YamatoAndo . I tested without twist msg. Here delta_orientation comes as (0,0,0,1) so comes out curr_orientation = prew_orientation as a result of this line . It actually works the same way as before the delay compensation.

He means that the last_angular_velocity_ will be kept forever even when the twist measurement is ceased.
Then, the ekf_localizer will keep calculation with that outdated twist, so its better to reset the last_angular_velocity_ to zero after the compensation.

Plus, I feel it is not appropriate to substitute last_angular_velocity_ in the measurement_update_twist function since the process and the function name doesn't match. So how about

Adding a reset line of last_angular_velocity_ at the bottom of compensate_rph_with_delay
Move the following line to after this line

last_angular_velocity_ = tf2::Vector3(
    twist.twist.twist.angular.x, twist.twist.twist.angular.y, twist.twist.twist.angular.z);

TaikiYamada4

@meliketanrikulu
Sorry for leaving a comment after approving, but could you check these out too?

localization/ekf_localizer/src/ekf_module.cpp

Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai>

meliketanrikulu · 2024-07-30T13:14:21Z

@meliketanrikulu I am concerned about the case when the twist msg is interrupted during operation. Please make sure it does not retain the previous values.

Hello @YamatoAndo . I tested without twist msg. Here delta_orientation comes as (0,0,0,1) so comes out curr_orientation = prew_orientation as a result of this line . It actually works the same way as before the delay compensation.

He means that the last_angular_velocity_ will be kept forever even when the twist measurement is ceased. Then, the ekf_localizer will keep calculation with that outdated twist, so its better to reset the last_angular_velocity_ to zero after the compensation.

Plus, I feel it is not appropriate to substitute last_angular_velocity_ in the measurement_update_twist function since the process and the function name doesn't match. So how about

Adding a reset line of last_angular_velocity_ at the bottom of compensate_rph_with_delay

Move the following line to after this line
last_angular_velocity_ = tf2::Vector3(
    twist.twist.twist.angular.x, twist.twist.twist.angular.y, twist.twist.twist.angular.z);

I understood your point and updated the code accordingly.
Related commit -> a061f42
Could you check it again? @TaikiYamada4 @YamatoAndo

Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai>

meliketanrikulu · 2024-07-30T13:43:21Z

I updated pose_with_delay header stamp with adding delay_time.
Related commit --> 622c076
Could you check this changes too.

localization/ekf_localizer/src/ekf_module.cpp

Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai>

meliketanrikulu · 2024-08-02T08:10:20Z

@TaikiYamada4 @YamatoAndo Could you review again. Its ready for the review.

…_roll_and_pitch

YamatoAndo · 2024-08-05T07:40:14Z

@meliketanrikulu
Thank you for the revision. The source code looks fine.

I have two questions.
In section "How was this PR tested?", it says that "pitch_filter_proc_dev" was set to 0.5.
But is the "pitch_filter_proc_dev" of the Test Results "Without Delay Compensation" remained at 0.01?
Also, is there a reason why "roll_filter_proc_dev" was not changed?

2.) I updated 1D Filter params for pitch estimation.
Changed pitch_filter_proc_dev param as 0.5

meliketanrikulu · 2024-08-05T08:03:29Z

@meliketanrikulu
Thank you for the revision. The source code looks fine.

I have two questions.
In section "How was this PR tested?", it says that "pitch_filter_proc_dev" was set to 0.5.
But is the "pitch_filter_proc_dev" of the Test Results "Without Delay Compensation" remained at 0.01?
Also, is there a reason why "roll_filter_proc_dev" was not changed?

2.) I updated 1D Filter params for pitch estimation.
Changed pitch_filter_proc_dev param as 0.5

Hello @YamatoAndo . Thanks for your review and comments
1.) Both With/Without Delay Compensation tests were performed with a pitch_filter_proc_dev value of 0.5. because I got the best results by adjusting this parameter.
2.) I did not make any changes to the roll_filter_proc_dev parameter because I did not observe that changing it had a positive effect on the results. roll_filter_proc_dev parameter was 0.01 for all tests.

YamatoAndo · 2024-08-05T11:24:00Z

@meliketanrikulu Thank you for the response.
It seems we need to either find an appropriate process noise, change the estimated covariance value of NDT, or change the algorithm to use the estimated variance value of NDT as is. I would like to consider this separately from this PR.

meliketanrikulu · 2024-08-07T18:24:39Z

@meliketanrikulu Thank you for the response. It seems we need to either find an appropriate process noise, change the estimated covariance value of NDT, or change the algorithm to use the estimated variance value of NDT as is. I would like to consider this separately from this PR.

Thanks for the review and support @YamatoAndo . As it is, I have merged this PR and am closing the issue. Based on your suggestion, I will check the NDT covariance values.

…refoundation#8095) * fix(ekf_localizer)add_delay_compensation_for_roll_and_pitch Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai> * rename compensate_pose_with_delay function Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai> * rename variables Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai> * update comment Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai> * rename compensated pose name Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai> * rename function name Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai> * style(pre-commit): autofix * calculate delta_orientation with orientation Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai> * rename - angular velocity vector Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai> * add z element delay compensation Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai> * fix typo Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai> * check twist msg for last_angular_velocity_ Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai> * use corrected pitch for delta_z calculation Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai> * update pose_with_delay header stamp Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai> * style(pre-commit): autofix * fix local variable naming Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai> --------- Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Yamato Ando <yamato.ando@gmail.com>

…refoundation#8095) * fix(ekf_localizer)add_delay_compensation_for_roll_and_pitch Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai> * rename compensate_pose_with_delay function Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai> * rename variables Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai> * update comment Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai> * rename compensated pose name Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai> * rename function name Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai> * style(pre-commit): autofix * calculate delta_orientation with orientation Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai> * rename - angular velocity vector Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai> * add z element delay compensation Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai> * fix typo Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai> * check twist msg for last_angular_velocity_ Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai> * use corrected pitch for delta_z calculation Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai> * update pose_with_delay header stamp Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai> * style(pre-commit): autofix * fix local variable naming Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai> --------- Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Yamato Ando <yamato.ando@gmail.com> Signed-off-by: xtk8532704 <1041084556@qq.com>

meliketanrikulu self-assigned this Jul 18, 2024

meliketanrikulu requested review from anhnv3991, KYabuuchi, Motsu-san, RyuYamamoto, SakodaShintaro, TaikiYamada4, TakaHoribe, IshitaTakeshi and YamatoAndo as code owners July 18, 2024 09:46

github-actions bot added the component:localization Vehicle's position determination in its environment. (auto-assigned) label Jul 18, 2024

meliketanrikulu marked this pull request as draft July 18, 2024 09:47

meliketanrikulu force-pushed the fix(ekf_localizer)add_delay_compensation_for_roll_and_pitch branch from 7f2b5ee to cbc2977 Compare July 18, 2024 09:47

github-actions bot added the Review effort [1-5]: 3 label Jul 18, 2024

meliketanrikulu changed the title ~~fix(ekf_localizer)add_delay_compensation_for_roll_and_pitch~~ fix(ekf_localizer): add_delay_compensation_for_roll_and_pitch Jul 18, 2024

github-actions bot reviewed Jul 18, 2024

View reviewed changes

meliketanrikulu force-pushed the fix(ekf_localizer)add_delay_compensation_for_roll_and_pitch branch 2 times, most recently from c793a8f to e3ca5b4 Compare July 18, 2024 10:56

SakodaShintaro added the tag:run-build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) label Jul 23, 2024

meliketanrikulu marked this pull request as ready for review July 23, 2024 10:14

This was referenced Jul 23, 2024

fix(ekf_localizer): add imu option for roll pitch and height estimation #7875

Closed

Improve Roll and Pitch estimation in Localization #6993

Closed

meliketanrikulu force-pushed the fix(ekf_localizer)add_delay_compensation_for_roll_and_pitch branch 3 times, most recently from 619d025 to cff8ac9 Compare July 23, 2024 12:45

meliketanrikulu requested a review from TaikiYamada4 July 29, 2024 10:41

TaikiYamada4 approved these changes Jul 30, 2024

View reviewed changes

add z element delay compensation

1ddbee8

Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai>

TaikiYamada4 reviewed Jul 30, 2024

View reviewed changes

localization/ekf_localizer/src/ekf_module.cpp Outdated Show resolved Hide resolved

localization/ekf_localizer/src/ekf_module.cpp Outdated Show resolved Hide resolved

meliketanrikulu added 2 commits July 30, 2024 13:32

fix typo

3398927

Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai>

check twist msg for last_angular_velocity_

a061f42

Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai>

meliketanrikulu force-pushed the fix(ekf_localizer)add_delay_compensation_for_roll_and_pitch branch from e430b16 to 26f46c4 Compare July 30, 2024 13:26

meliketanrikulu requested a review from TaikiYamada4 July 30, 2024 13:33

meliketanrikulu added 2 commits July 30, 2024 16:41

use corrected pitch for delta_z calculation

5af1316

Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai>

update pose_with_delay header stamp

622c076

Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai>

meliketanrikulu force-pushed the fix(ekf_localizer)add_delay_compensation_for_roll_and_pitch branch from a8aafbc to 622c076 Compare July 30, 2024 13:41

style(pre-commit): autofix

1acb118

YamatoAndo reviewed Jul 31, 2024

View reviewed changes

localization/ekf_localizer/src/ekf_module.cpp Outdated Show resolved Hide resolved

fix local variable naming

e33ab61

Signed-off-by: Melike Tanrıkulu <melike@leodrive.ai>

meliketanrikulu requested a review from YamatoAndo July 31, 2024 12:25

Merge branch 'main' into fix(ekf_localizer)add_delay_compensation_for…

396753d

…_roll_and_pitch

YamatoAndo approved these changes Aug 5, 2024

View reviewed changes

meliketanrikulu merged commit 1640c06 into autowarefoundation:main Aug 7, 2024
28 of 30 checks passed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

fix(ekf_localizer): add_delay_compensation_for_roll_and_pitch #8095

fix(ekf_localizer): add_delay_compensation_for_roll_and_pitch #8095

meliketanrikulu commented Jul 18, 2024 •

edited

Loading

github-actions bot commented Jul 18, 2024 •

edited

Loading

meliketanrikulu commented Jul 18, 2024

github-actions bot commented Jul 18, 2024 •

edited

Loading

meliketanrikulu commented Jul 18, 2024

github-actions bot Jul 18, 2024

codecov bot commented Jul 23, 2024 •

edited

Loading

TaikiYamada4 left a comment

YamatoAndo commented Jul 30, 2024

meliketanrikulu commented Jul 30, 2024

TaikiYamada4 commented Jul 30, 2024

TaikiYamada4 left a comment

meliketanrikulu commented Jul 30, 2024

meliketanrikulu commented Jul 30, 2024

meliketanrikulu commented Aug 2, 2024

YamatoAndo commented Aug 5, 2024

meliketanrikulu commented Aug 5, 2024 •

edited

Loading

YamatoAndo commented Aug 5, 2024

meliketanrikulu commented Aug 7, 2024

		roll_rate_ = twist.twist.twist.angular.x;
		pitch_rate_ = twist.twist.twist.angular.y;

fix(ekf_localizer): add_delay_compensation_for_roll_and_pitch #8095

fix(ekf_localizer): add_delay_compensation_for_roll_and_pitch #8095

Conversation

meliketanrikulu commented Jul 18, 2024 • edited Loading

Description

Related links

How was this PR tested?

Test Procedure

Installation

Prepare Autoware to test PR:

Launch Autoware

Test Results:

Comparison with Previous Works:

Notes for reviewers

Interface changes

Effects on system behavior

github-actions bot commented Jul 18, 2024 • edited Loading

meliketanrikulu commented Jul 18, 2024

github-actions bot commented Jul 18, 2024 • edited Loading

PR Reviewer Guide 🔍

(Review updated until commit cff8ac9)

meliketanrikulu commented Jul 18, 2024

github-actions bot Jul 18, 2024

Choose a reason for hiding this comment

codecov bot commented Jul 23, 2024 • edited Loading

Codecov Report

TaikiYamada4 left a comment

Choose a reason for hiding this comment

YamatoAndo commented Jul 30, 2024

meliketanrikulu commented Jul 30, 2024

TaikiYamada4 commented Jul 30, 2024

TaikiYamada4 left a comment

Choose a reason for hiding this comment

meliketanrikulu commented Jul 30, 2024

meliketanrikulu commented Jul 30, 2024

meliketanrikulu commented Aug 2, 2024

YamatoAndo commented Aug 5, 2024

meliketanrikulu commented Aug 5, 2024 • edited Loading

YamatoAndo commented Aug 5, 2024

meliketanrikulu commented Aug 7, 2024

meliketanrikulu commented Jul 18, 2024 •

edited

Loading

github-actions bot commented Jul 18, 2024 •

edited

Loading

github-actions bot commented Jul 18, 2024 •

edited

Loading

(Review updated until commit `cff8ac9`)

codecov bot commented Jul 23, 2024 •

edited

Loading

meliketanrikulu commented Aug 5, 2024 •

edited

Loading