feat(pid_longitudinal_controller): improve delay and slope compensation #4712

brkay54 · 2023-08-22T15:48:14Z

Description

In the current implementation on Autoware, while calculating the output command, we are compensating for the slope and delay in pid_longitudinal_controller.

We are doing the delay compensation by using the estimation of the drive distance of the vehicle after delay_compensation_time and we are looking at the future TrajectoryPoint and calculating the output command with respect to this future point.

However, before sending the output command, we are also doing slope compensation with respect to Current TrajectoryPoint. However, in some scenarios on hilly roads, this way is causing some conflicts because of the time like below.

As you can see in this scenario, there is a conflict between slope and delay compensation because they are using different reference times. In this scenario, the node will send FF with respect to the target point however it sums negative slope compensation value because of the current position of the vehicle and it causes undesired behavior. To avoid this behavior, I made some changes to delay and slope compensation in this package to sum compensations at the same time.

Also, after this PR, the following PRs should be merged by order here:

1- #5077
2- #5079
3- #5080
4- #5081

Tests performed

I tested on the real vehicle and also on the planning simulator. It solves our problems in the real environment and there is no behavioral change in the planning simulator.

Notes for reviewers

Interface changes

Effects on system behavior

The only effect is that we will use the raw pitch info at only starting (while not moving), if the vehicle is moving, it needs the future pitch info, to achieve this, it depends on the pitch values of the trajectory points.

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.

Summary by CodeRabbit

New Feature: Improved delay and slope compensation in the pid_longitudinal_controller package, resulting in more accurate output commands. The changes include updates to the getPitchByTraj function, introduction of the findTrajectoryPoseAfterDistance function, and modification of the return type of the lerpTrajectoryPoint function. These enhancements may impact the behavior of the code.
Bug Fix: Addressed delay and slope compensation issues in the PidLongitudinalController class of Autoware. The modifications involve using drive distance estimation after a specified delay time for delay compensation and adjusting slope compensation to avoid conflicts. The changes also introduce the StateAfterDelay struct and a new SlopeSource enum.

…mpensation autowarefoundation#4712 Signed-off-by: Berkay Karaman <[email protected]>

…mpensation autowarefoundation#4712 Signed-off-by: Berkay Karaman <[email protected]> update d

…mpensation autowarefoundation#4712 Signed-off-by: Berkay Karaman <[email protected]> update d not sure stopped

…mpensation autowarefoundation#4712 Signed-off-by: Berkay Karaman <[email protected]> update d not sure stopped style(pre-commit): autofix

…mpensation autowarefoundation#4712 Signed-off-by: Berkay Karaman <[email protected]> update d not sure stopped style(pre-commit): autofix add copysign

…mpensation autowarefoundation#4712 Signed-off-by: Berkay Karaman <[email protected]> update d not sure stopped style(pre-commit): autofix add copysign denem

…mpensation autowarefoundation#4712 Signed-off-by: Berkay Karaman <[email protected]> update d not sure stopped style(pre-commit): autofix add copysign denem style(pre-commit): autofix

…mpensation autowarefoundation#4712 Signed-off-by: Berkay Karaman <[email protected]> update d not sure stopped style(pre-commit): autofix add copysign denem style(pre-commit): autofix ufak düzenleme

TakaHoribe · 2023-11-13T08:25:29Z

@brkay54 Thank you for addressing my comments.

because we will need the future slope only for higher velocities, for lower velocities we can use vehicle_pitch

This is only in the ideal case. We need to care about users who can not use an accurate map or rich sensor for pitch estimation.

Trajectory-only option is necessary for users with poor sensors.
Pitch-only option is necessary for uses with poor maps.
Integral-team in PID (No specific slope compensation) can be used for users with both poor sensors and poor maps.
Adaptive can be available only for users with both rich sensors and rich maps.

The pitch angle of the vehicle does not accurately represent the road surface gradient. Larger vehicles, such as buses, also have the ability to adjust air pressure to keep their posture as parallel as possible on a slope. With such functions, we can not use the pitch angle for the slope compensation. The parameters should be determined with an understanding of such pros/cons.

brkay54 · 2023-12-05T13:43:18Z

Hi @TakaHoribe -san, sorry to be late I couldn't find time to update the PR in the last weeks.

I updated the slope source option as you said above. You can find the changes in the launch file here. It is ready for review, please let me know if any issue exists.

Also, after this PR, the following PRs should be merged by order here:

1- #5077
2- #5079
3- #5080
4- #5081

github-actions · 2024-01-17T06:26:11Z

CodeRabbit

Walkthrough

The changes aim to improve the delay and slope compensation in the pid_longitudinal_controller package of Autoware. The modifications include updating the getPitchByTraj function, introducing a new function findTrajectoryPoseAfterDistance, and modifying the return type of the lerpTrajectoryPoint function. These changes may impact the behavior of the code by enhancing the accuracy and reliability of the output command calculation.

Changes

File	Summary
control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp	Changes involve improving delay and slope compensation during output command calculation. Updates `getPitchByTraj` function, introduces `findTrajectoryPoseAfterDistance` function, and modifies return type of `lerpTrajectoryPoint` function.
control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp	Improvements to delay and slope compensation in `PidLongitudinalController` class. Modifies delay compensation to estimate drive distance after specified delay time. Adjusts slope compensation to avoid conflicts with delay compensation. Introduces `StateAfterDelay` struct and adds `SlopeSource` enum.

🐇 "Hop, hop, hop, the code does change,
With delay and slope, we rearrange.
Accuracy and reliability we seek,
To make the output command peak.
Functions updated, types modified,
Bugs squashed, and logic clarified.
A rabbit's touch, a code refined,
Let Autoware's performance shine!" 🥕

Uplevel your code reviews with CodeRabbit Pro

CodeRabbit Pro

If you like this project, please support us by purchasing the Pro version. The Pro version has advanced context, superior noise reduction and several proprietary improvements compared to the open source version. Moreover, CodeRabbit Pro is free for open source projects.

github-actions

CodeRabbit

Commits

Files that changed from the base of the PR and between 8265e50 and 75bca30 commits.

Files selected (5)

control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp (6)
control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp (7)
control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp (2)
control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp (16)
control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp (3)

Files ignored due to filter (1)

control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml

Files not summarized due to errors (2)

control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp (diff tokens exceeds limit)
control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp (diff tokens exceeds limit)

Files skipped from review due to trivial changes (1)

control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp

Review comments generated (5)

Review: 2
LGTM: 3

Tips

Chat with CodeRabbit Bot (`@coderabbitai`)

Reply on review comments left by this bot to ask follow-up questions. A review comment is a comment on a diff or a file.
Invite the bot into a review comment chain by tagging @coderabbitai in a reply.

Code suggestions

The bot may make code suggestions, but please review them carefully before committing since the line number ranges may be misaligned.
You can edit the comment made by the bot and manually tweak the suggestion if it is slightly off.

Pausing incremental reviews

Add @coderabbitai: ignore anywhere in the PR description to pause further reviews from the bot.

github-actions · 2024-01-17T06:26:53Z

control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp

+  point.pose.position.x = 0.6;
  point.pose.position.y = 0.0;
-  point.pose.position.z = 1.0;
+  point.pose.position.z = 0.8;
  traj.points.push_back(point);
-  point.pose.position.x = 2.0;
+  point.pose.position.x = 1.2;
  point.pose.position.y = 0.0;
  point.pose.position.z = 0.0;
  traj.points.push_back(point);
-  point.pose.position.x = 3.0;
+  point.pose.position.x = 1.8;
  point.pose.position.y = 0.0;
-  point.pose.position.z = 0.5;
+  point.pose.position.z = 0.8;
  traj.points.push_back(point);
  size_t closest_idx = 0;
  EXPECT_DOUBLE_EQ(
-    std::abs(longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base)), M_PI_4);
+    longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base), std::atan2(-0.8, 0.6));
  closest_idx = 1;
  EXPECT_DOUBLE_EQ(
-    std::abs(longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base)), M_PI_4);
+    longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base), std::atan2(0.8, 0.6));
  closest_idx = 2;
  EXPECT_DOUBLE_EQ(
-    std::abs(longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base)),
-    std::atan2(0.5, 1));
+    longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base), std::atan2(-0.8, 0.6));
  closest_idx = 3;
  EXPECT_DOUBLE_EQ(
-    std::abs(longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base)),
-    std::atan2(0.5, 1));
+    longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base), std::atan2(-0.8, 0.6));


CodeRabbit

The test case TEST(TestLongitudinalControllerUtils, calcPoseAfterTimeDelay) has been modified. The trajectory points and expected pitch values have changed. Please review these changes to ensure they are correct and aligned with the intended behavior. If the changes are intentional, no further action is needed. Otherwise, consider reverting this test case to its original state or updating it accordingly.

...ongitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp

TakaHoribe

@brkay54 LGTM and almost ready to merge.

Would you address the followings?

Fix test code. This seems to fail due to the pos/neg definition mismatch.
Make a PR to the autoware_launch for new parameters.
Make sure unrelated parameters are not changed in this PR: parameters attached in the screenshot should not change.

control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

codecov · 2024-02-05T22:51:13Z

Codecov Report

Attention: 132 lines in your changes are missing coverage. Please review.

Comparison is base (0b90a97) 14.90% compared to head (27d25df) 14.92%.
Report is 1 commits behind head on main.

Files	Patch %	Lines
...roller/test/test_longitudinal_controller_utils.cpp	30.85%	0 Missing and 65 partials ⚠️
...nal_controller/src/pid_longitudinal_controller.cpp	33.33%	33 Missing and 31 partials ⚠️
...dinal_controller/longitudinal_controller_utils.hpp	75.00%	0 Missing and 1 partial ⚠️
...tudinal_controller/pid_longitudinal_controller.hpp	50.00%	0 Missing and 1 partial ⚠️
...l_controller/src/longitudinal_controller_utils.cpp	95.45%	0 Missing and 1 partial ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #4712      +/-   ##
==========================================
+ Coverage   14.90%   14.92%   +0.02%     
==========================================
  Files        1817     1817              
  Lines      125466   125585     +119     
  Branches    37706    37788      +82     
==========================================
+ Hits        18697    18742      +45     
- Misses      85752    85768      +16     
- Partials    21017    21075      +58

Flag	Coverage Δ		*Carryforward flag
differential	`39.95% <39.44%> (?)`
total	`14.87% <ø> (-0.03%)`	⬇️	Carriedforward from 0b90a97

*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.

Signed-off-by: Berkay Karaman <[email protected]>

brkay54 · 2024-02-06T00:16:07Z

@TakaHoribe-san, Just to remind: Also, after this PR, the following PRs should be merged by order here:

1- #5077
2- #5079
3- #5080
4- #5081

Also, Horibe-san, I made some critical changes to the package. If you don't mind, I would like to maintain this package. Can I add myself as a maintainer?

TakaHoribe

LGTM!

TakaHoribe · 2024-02-09T04:56:03Z

@brkay54

Can I add myself as a maintainer?

I think it is fine. I'll talk with @takayuki5168

…on (autowarefoundation#4712) Signed-off-by: Berkay Karaman <[email protected]>

brkay54 requested review from TakaHoribe and takayuki5168 as code owners August 22, 2023 15:48

brkay54 marked this pull request as draft August 22, 2023 15:48

github-actions bot added the component:control Vehicle control algorithms and mechanisms. (auto-assigned) label Aug 22, 2023

mehmetdogru assigned brkay54 Aug 23, 2023

brkay54 added a commit to brkay54/autoware.universe that referenced this pull request Aug 23, 2023

feat(pid_longitudinal_controller): fix some bugs and improve delay co…

8c60c02

…mpensation autowarefoundation#4712 Signed-off-by: Berkay Karaman <[email protected]>

brkay54 force-pushed the pid/change-state-behavior branch from 07c2737 to 8c60c02 Compare August 23, 2023 08:52

brkay54 added a commit to leo-drive/autoware.universe.golf that referenced this pull request Aug 23, 2023

feat(pid_longitudinal_controller): fix some bugs and improve delay co…

ed7298b

…mpensation autowarefoundation#4712 Signed-off-by: Berkay Karaman <[email protected]>

LeoDriveProject added a commit to leo-drive/autoware.universe.golf that referenced this pull request Aug 23, 2023

feat(pid_longitudinal_controller): fix some bugs and improve delay co…

90bc35f

…mpensation autowarefoundation#4712 Signed-off-by: Berkay Karaman <[email protected]>

brkay54 force-pushed the pid/change-state-behavior branch from 8c60c02 to 00b96cc Compare August 24, 2023 14:45

brkay54 added a commit to brkay54/autoware.universe that referenced this pull request Aug 24, 2023

feat(pid_longitudinal_controller): fix some bugs and improve delay co…

00b96cc

…mpensation autowarefoundation#4712 Signed-off-by: Berkay Karaman <[email protected]>

brkay54 force-pushed the pid/change-state-behavior branch from 00b96cc to 8e53dba Compare August 28, 2023 07:07

brkay54 added a commit to brkay54/autoware.universe that referenced this pull request Aug 28, 2023

feat(pid_longitudinal_controller): fix some bugs and improve delay co…

8e53dba

…mpensation autowarefoundation#4712 Signed-off-by: Berkay Karaman <[email protected]>

brkay54 added a commit to brkay54/autoware.universe that referenced this pull request Aug 28, 2023

feat(pid_longitudinal_controller): fix some bugs and improve delay co…

7398f36

…mpensation autowarefoundation#4712 Signed-off-by: Berkay Karaman <[email protected]> update d

brkay54 force-pushed the pid/change-state-behavior branch from 8e53dba to 4e1a86a Compare August 28, 2023 08:46

brkay54 added a commit to brkay54/autoware.universe that referenced this pull request Aug 28, 2023

feat(pid_longitudinal_controller): fix some bugs and improve delay co…

a487f48

…mpensation autowarefoundation#4712 Signed-off-by: Berkay Karaman <[email protected]> update d not sure stopped

brkay54 force-pushed the pid/change-state-behavior branch from 4e1a86a to a487f48 Compare August 28, 2023 08:46

brkay54 force-pushed the pid/change-state-behavior branch from 499467f to e7e9f48 Compare August 28, 2023 08:55

brkay54 force-pushed the pid/change-state-behavior branch from e7e9f48 to 98751cd Compare August 28, 2023 10:21

brkay54 force-pushed the pid/change-state-behavior branch from 98751cd to 78c2c93 Compare August 28, 2023 12:25

brkay54 force-pushed the pid/change-state-behavior branch from bcda784 to 55c30d5 Compare August 28, 2023 13:08

brkay54 force-pushed the pid/change-state-behavior branch from 55c30d5 to ab174e3 Compare August 29, 2023 13:23

brkay54 force-pushed the pid/change-state-behavior branch from e00b520 to 5bcff34 Compare November 10, 2023 14:23

brkay54 requested a review from TakaHoribe November 11, 2023 11:02

brkay54 force-pushed the pid/change-state-behavior branch from 5bcff34 to 4842216 Compare December 5, 2023 12:24

brkay54 force-pushed the pid/change-state-behavior branch from 4842216 to 75bca30 Compare December 21, 2023 07:01

TakaHoribe added tag:run-build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) tag:openai-pr-reviewer labels Jan 17, 2024

github-actions bot reviewed Jan 17, 2024

View reviewed changes

takayuki5168 assigned TakaHoribe Jan 18, 2024

brkay54 force-pushed the pid/change-state-behavior branch 2 times, most recently from 65c1762 to f65ced5 Compare February 1, 2024 10:45

TakaHoribe reviewed Feb 5, 2024

View reviewed changes

control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp Outdated Show resolved Hide resolved

brkay54 force-pushed the pid/change-state-behavior branch from f65ced5 to 870fc09 Compare February 5, 2024 22:29

brkay54 force-pushed the pid/change-state-behavior branch from 870fc09 to d6ce419 Compare February 5, 2024 23:47

feat(pid_longitudinal_controller): improve delay and slope compensation

27d25df

Signed-off-by: Berkay Karaman <[email protected]>

brkay54 force-pushed the pid/change-state-behavior branch from 9ebc3ab to 27d25df Compare February 6, 2024 00:08

brkay54 requested a review from TakaHoribe February 6, 2024 08:14

TakaHoribe approved these changes Feb 9, 2024

View reviewed changes

brkay54 merged commit 7ebf71a into autowarefoundation:main Feb 9, 2024
22 of 25 checks passed

StepTurtle pushed a commit to StepTurtle/autoware.universe that referenced this pull request Feb 28, 2024

feat(pid_longitudinal_controller): improve delay and slope compensati…

482488a

…on (autowarefoundation#4712) Signed-off-by: Berkay Karaman <[email protected]>

karishma1911 pushed a commit to Interplai/autoware.universe that referenced this pull request Jun 3, 2024

feat(pid_longitudinal_controller): improve delay and slope compensati…

4d01ba3

…on (autowarefoundation#4712) Signed-off-by: Berkay Karaman <[email protected]>

yuki-takagi-66 mentioned this pull request Sep 4, 2024

fix(pid_longitudinal_controller): fix the same point error #8758

Merged

takayuki5168 mentioned this pull request Dec 17, 2024

feat(pid_longitudinal_controller): add virtual wall for dry steering and emergency #9685

Merged

This was referenced Dec 19, 2024

fix(pid_longitudinal_controller): fix overrun distance calculation against goal position tier4/autoware.universe#1713

Open

fix(pid_longitudinal_controller): fix overrun distance calculation against goal position tier4/autoware.universe#1714

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

feat(pid_longitudinal_controller): improve delay and slope compensation #4712

feat(pid_longitudinal_controller): improve delay and slope compensation #4712

brkay54 commented Aug 22, 2023 •

edited by TakaHoribe

Loading

TakaHoribe commented Nov 13, 2023 •

edited

Loading

brkay54 commented Dec 5, 2023

github-actions bot commented Jan 17, 2024 •

edited

Loading

CodeRabbit Pro

github-actions bot left a comment

github-actions bot Jan 17, 2024

TakaHoribe left a comment

codecov bot commented Feb 5, 2024 •

edited

Loading

brkay54 commented Feb 6, 2024

TakaHoribe left a comment

TakaHoribe commented Feb 9, 2024

feat(pid_longitudinal_controller): improve delay and slope compensation #4712

feat(pid_longitudinal_controller): improve delay and slope compensation #4712

Conversation

brkay54 commented Aug 22, 2023 • edited by TakaHoribe Loading

Description

Related links

Tests performed

Notes for reviewers

Interface changes

Effects on system behavior

Pre-review checklist for the PR author

In-review checklist for the PR reviewers

Post-review checklist for the PR author

Summary by CodeRabbit

TakaHoribe commented Nov 13, 2023 • edited Loading

brkay54 commented Dec 5, 2023

github-actions bot commented Jan 17, 2024 • edited Loading

Walkthrough

Changes

CodeRabbit Pro

github-actions bot left a comment

Choose a reason for hiding this comment

Chat with CodeRabbit Bot (@coderabbitai)

Code suggestions

Pausing incremental reviews

github-actions bot Jan 17, 2024

Choose a reason for hiding this comment

TakaHoribe left a comment

Choose a reason for hiding this comment

codecov bot commented Feb 5, 2024 • edited Loading

Codecov Report

brkay54 commented Feb 6, 2024

TakaHoribe left a comment

Choose a reason for hiding this comment

TakaHoribe commented Feb 9, 2024

brkay54 commented Aug 22, 2023 •

edited by TakaHoribe

Loading

TakaHoribe commented Nov 13, 2023 •

edited

Loading

github-actions bot commented Jan 17, 2024 •

edited

Loading

Chat with CodeRabbit Bot (`@coderabbitai`)

codecov bot commented Feb 5, 2024 •

edited

Loading