67 feature implement pure pursuit controller #132

schwalga · 2023-01-08T19:28:42Z

Description

This PR implements the pure pursuit controller. The first tests suggest, that it is working as intended, however due to problems with the GPS signal the car is not able to hold course at the moment.

While i tried fixing the problem at first under this issue, after a few hours of trial and error I realise, that this warrants a seperate issue. The documentation added in this PR gives a first overview over the problem it will be amended with the final filter in #131. For now I have left the simple filter added to DummyTrajectorySub.py so that the controller can be tested by a reviewer.

Fixes #67

Time invested

16h @schwalga (Most of the work has been done in 66-test due to a misunderstanding (see cc59a2e))
7h @Julian-Graf

Type of change

New feature (non-breaking change which adds functionality)

Does this PR introduce a breaking change?

No

Most important changes

The most important file is pure_pursuit_controller.py

Checklist:

My code follows the style guidelines of this project
I have performed a self-review of my own code
I have commented my code, particularly in hard-to-understand areas
I have made corresponding changes to the documentation
My changes generate no new warnings
I have added tests that prove my fix is effective or that my feature works (might be obsolete with CI later on)
New and existing unit tests pass locally with my changes (might be obsolete with CI later on)

…t-controller' into 67-feature-implement-pure-pursuit-controller

Also incorporated feedback from #126.

…ould be. Positions are only updated when there was a significant change. Started filtering GPS signals.

…d to a different issue)

JoKircher

Nice new functionality for the action portion of the agent.

Unfortunately, due to an issue with the docker container. I couldn't test the controller so far. There are still quite some todos so the PR is blocked for now.

If you have any questions please let me know!

One last note:

The localization you implemented will probably need at different corners of the project, so a refactor to a more centralized spot might be a good idea in the future.

Review time: 1,5 hours

code/acting/src/acting/DummyTrajectorySub.py

code/acting/src/acting/helper_functions.py

code/acting/src/acting/pure_pursuit_controller.py

github-actions · 2023-01-09T16:29:08Z

Simulation results

Metric	Value
Avg. driving score	0.069615
Avg. route completion	0.255
Avg. infraction penalty	0.273
Collisions with pedestrians	0.0
Collisions with vehicles	54.151
Collisions with layout	54.151
Red lights infractions	0.0
Stop sign infractions	0.0
Off-road infractions	0
Route deviations	0.0
Route timeouts	54.151
Agent blocked	0.0
Yield emergency vehicles infractions	0.0
Scenario timeouts	54.151
Min speed infractions	0.0

schwalga · 2023-01-09T16:37:55Z

I'm not sure how changing the md document managed to break the build?
Apart from that, I fixed all the issues mentioned in the review (ca. $1 \frac{1}{2}$ h by @schwalga)

JoKircher · 2023-01-10T08:47:44Z

For some reason, the leaderboard evaluation failed. Maybe just rerunning the tests should suffice otherwise @nylser might be of help.

nylser · 2023-01-10T08:57:38Z

For some reason, the leaderboard evaluation failed. Maybe just rerunning the tests should suffice otherwise @nylser might be of help.
Looks like this might be an issue because I didn't get to switching it to the right machine yet? VK_DEVICE_LOST is the error code, which might hint to the GPU already being used differently

JoKircher

Good job, all of the suggested changes have been incorporated. However, for now the ego vehicle drives to the right side of the road and hit a parking clock. I guess that is related to the GPS problem?

…ctory. Added one more publisher for debugging.

github-actions · 2023-01-10T19:22:36Z

Simulation results

Metric	Value
Avg. driving score	0.0588
Avg. route completion	0.175
Avg. infraction penalty	0.3465
Collisions with pedestrians	0.0
Collisions with vehicles	76.358
Collisions with layout	38.179
Red lights infractions	0.0
Stop sign infractions	0.0
Off-road infractions	0
Route deviations	0.0
Route timeouts	76.358
Agent blocked	0.0
Yield emergency vehicles infractions	0.0
Scenario timeouts	76.358
Min speed infractions	0.0

schwalga · 2023-01-10T19:26:04Z

As suggested I have set the GPS noise to 0 for the time being.
I found/fixed another issue, where points in the trajectory would be targeted even though they had already been reached.
At the moment the car is capable of following the trajectory defined in DummyTrajectoryPub.py.
Since some more problems have come to light, I think this PR is not ready to be merged.
2h @schwalga

schwalga · 2023-01-23T21:16:57Z

Fixed a few errors, tested some more parameters, and added a PID controller for the steering angle.
I think this PR should be merged in this sprint so that others can test their features.

~ 6h by @schwalga

github-actions · 2023-01-24T10:58:42Z

Simulation results

Metric	Value
Avg. driving score	0.06006
Avg. route completion	0.22
Avg. infraction penalty	0.273
Collisions with pedestrians	0.0
Collisions with vehicles	62.046
Collisions with layout	62.046
Red lights infractions	0.0
Stop sign infractions	0.0
Off-road infractions	0
Route deviations	0.0
Route timeouts	62.046
Agent blocked	0.0
Yield emergency vehicles infractions	0.0
Scenario timeouts	62.046
Min speed infractions	0.0

github-actions · 2023-01-25T14:29:39Z

Simulation results

Metric	Value
Avg. driving score	0.0
Avg. route completion	0.0
Avg. infraction penalty	0.7
Collisions with pedestrians	0.0
Collisions with vehicles	0.0
Collisions with layout	0.0
Red lights infractions	0.0
Stop sign infractions	0.0
Off-road infractions	0
Route deviations	0.0
Route timeouts	2000.0
Agent blocked	0.0
Yield emergency vehicles infractions	0.0
Scenario timeouts	2000.0
Min speed infractions	0.0

* feat(#131): Added dedicated node for the publishing of the current position * feat(#131): Moved the publishing from DummySub to PositionPublisher * feat(#131): Added dedicated node for the publishing of the current position * feat(#131): Move PositionPublisher to perception * feat(#131): Added publishing of heading * feat(#131): Install and launch robot-pose-ekf * feat(#131): Fix install robot-pose-ekf * feat(#132): Started adding translation node for the EKF * fix(#131): Got EKF to work. Values for covariance need tweaking * feat(#131): Move installation in dockerfile * feat(#131): Adapt PositionPublisher to use filtered values * fix(#131): Added an average filter to the gps signal to improve the ekf * fix(#131): minor fix to rerun CI * fix(#131): fixed imu orientation, fixed gps quaternion * feat(#131): Delete unnecessary comment Co-authored-by: schwalga <[email protected]> Co-authored-by: Julian-Graf <[email protected]> Co-authored-by: Julian Graf <[email protected]>

schwalga · 2023-01-27T22:33:27Z

During testing with a more complex trajectory, a few errors appeared, which have now been fixed.
To test, simply enter b5 install and then b5 run, and the car will take the first turn :).
I would maybe clean up the code in a few places, but in my eyes the PR is ready for merging.

I also added a custom message that useful for #151

~ 4h by @schwalga

JoKircher

Nice PR, the pure pursuit controller seems to work nicely. One last question before I approve the PR. Is is intended to cut the corner at the left turn and drive in the middle of the two lanes afterwards?

Review time: 30 min

code/acting/src/acting/pure_pursuit_controller.py

JoKircher · 2023-01-30T08:39:29Z

Also could you please resolve the merge conflicts?

code/acting/src/acting/coordinate_transformation.py

JoKircher

Looks good. Happily approve! Still some conflicts though

# Conflicts: # code/acting/launch/acting.launch

github-actions · 2023-01-31T17:01:43Z

Simulation results

Metric	Value
Avg. driving score	0.0
Avg. route completion	0.0
Avg. infraction penalty	0.7
Collisions with pedestrians	0.0
Collisions with vehicles	0.0
Collisions with layout	0.0
Red lights infractions	0.0
Stop sign infractions	0.0
Off-road infractions	0
Route deviations	0.0
Route timeouts	2000.0
Agent blocked	0.0
Yield emergency vehicles infractions	0.0
Scenario timeouts	2000.0
Min speed infractions	0.0

Julian-Graf and others added 13 commits December 6, 2022 15:55

feat(#67): Added Pure Pursuit Controller stub

02c03f2

feat(#67): Added first implementation of Pure Pursuit Controller

b7acf2b

feat(#67): Added Pure Pursuit Controller stub

a31cef8

feat(#67): Added first implementation of Pure Pursuit Controller

1e5e0b4

Merge remote-tracking branch 'origin/67-feature-implement-pure-pursui…

358ecbb

…t-controller' into 67-feature-implement-pure-pursuit-controller

feat(#67): Manually added all the work done in 66-test.

cc59a2e

Also incorporated feedback from #126.

fix(#67): Forgot to add files to the commit :|

2ac34f2

feat(#67): Heading updates are now based on new positions, as they sh…

adfe0ba

…ould be. Positions are only updated when there was a significant change. Started filtering GPS signals.

fix(#67): Tested GPS smoothing for different filter settings

49900cf

fix(#67): Fixes to comply with coding guidelines

4560331

feat(#67): Started adding documentation for GPS sensor (might be move…

7522954

…d to a different issue)

feat(#67): Documentation for GPS sensor

c4c8d71

fix(#67): Version for testing. Removed a lot of unused code.

7379fe2

schwalga linked an issue Jan 8, 2023 that may be closed by this pull request

[Feature]: Implement Pure Pursuit Controller #67

Closed

schwalga added 2 commits January 8, 2023 21:54

fix(#67) 80 > 79 :)

0cfca64

fix(#67) removed trailing whitespaces

73ef8fc

JoKircher suggested changes Jan 9, 2023

View reviewed changes

schwalga and others added 2 commits January 9, 2023 17:17

fix(#67): Implemented feedback.

426f8db

fix(#67) fixed documentation

58f7565

schwalga requested a review from JoKircher January 9, 2023 16:30

JoKircher reviewed Jan 10, 2023

View reviewed changes

fix(#67): Removed noise from GNSS sensor. Fix an issue with the traje…

af31c0f

…ctory. Added one more publisher for debugging.

Julian-Graf mentioned this pull request Jan 11, 2023

66 feature implement stanley controller #102

Closed

7 tasks

Update helper_functions.py

f95c118

schwalga pushed a commit that referenced this pull request Jan 17, 2023

feat(#132): Started adding translation node for the EKF

a1880de

schwalga added 2 commits January 22, 2023 16:39

fix(#67): alpha was calculated using the wrong position

da67b42

feat(#67): added a pid controller to the steering angle

8418119

Julian-Graf requested a review from JoKircher January 25, 2023 08:31

Merge branch 'main' into 67-feature-implement-pure-pursuit-controller

6733afa

schwalga added 2 commits January 27, 2023 22:14

fix(#67): found and fixed a few critical mistakes

b5ceb5b

fix(#67): tweaked some parameters

b2d7820

schwalga requested a review from Julian-Graf January 27, 2023 22:33

fix(#67): added GPS references for Towns: 1, 2, 3, 4, 5 and 10

e3b21c8

JoKircher suggested changes Jan 30, 2023

View reviewed changes

code/acting/src/acting/pure_pursuit_controller.py Show resolved Hide resolved

Julian-Graf reviewed Jan 30, 2023

View reviewed changes

code/acting/src/acting/coordinate_transformation.py Outdated Show resolved Hide resolved

schwalga added 2 commits January 30, 2023 19:41

fix(#67): tuned parameters

d33aba5

fix(#67): resolved conflict

17d8593

JoKircher approved these changes Jan 31, 2023

View reviewed changes

Merge branch 'main' into 67-feature-implement-pure-pursuit-controller

2bcd5a3

# Conflicts: # code/acting/launch/acting.launch

schwalga merged commit 0d1aecb into main Jan 31, 2023

schwalga deleted the 67-feature-implement-pure-pursuit-controller branch January 31, 2023 17:04

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

67 feature implement pure pursuit controller #132

67 feature implement pure pursuit controller #132

schwalga commented Jan 8, 2023

JoKircher left a comment •

edited

Loading

github-actions bot commented Jan 9, 2023

schwalga commented Jan 9, 2023

JoKircher commented Jan 10, 2023

nylser commented Jan 10, 2023

JoKircher left a comment

github-actions bot commented Jan 10, 2023

schwalga commented Jan 10, 2023 •

edited

Loading

schwalga commented Jan 23, 2023 •

edited

Loading

github-actions bot commented Jan 24, 2023

github-actions bot commented Jan 25, 2023

schwalga commented Jan 27, 2023 •

edited

Loading

JoKircher left a comment

JoKircher commented Jan 30, 2023

JoKircher left a comment

github-actions bot commented Jan 31, 2023

67 feature implement pure pursuit controller #132

67 feature implement pure pursuit controller #132

Conversation

schwalga commented Jan 8, 2023

Description

Time invested

Type of change

Does this PR introduce a breaking change?

Most important changes

Checklist:

JoKircher left a comment • edited Loading

Choose a reason for hiding this comment

github-actions bot commented Jan 9, 2023

Simulation results

schwalga commented Jan 9, 2023

JoKircher commented Jan 10, 2023

nylser commented Jan 10, 2023

JoKircher left a comment

Choose a reason for hiding this comment

github-actions bot commented Jan 10, 2023

Simulation results

schwalga commented Jan 10, 2023 • edited Loading

schwalga commented Jan 23, 2023 • edited Loading

github-actions bot commented Jan 24, 2023

Simulation results

github-actions bot commented Jan 25, 2023

Simulation results

schwalga commented Jan 27, 2023 • edited Loading

JoKircher left a comment

Choose a reason for hiding this comment

JoKircher commented Jan 30, 2023

JoKircher left a comment

Choose a reason for hiding this comment

github-actions bot commented Jan 31, 2023

Simulation results

JoKircher left a comment •

edited

Loading

schwalga commented Jan 10, 2023 •

edited

Loading

schwalga commented Jan 23, 2023 •

edited

Loading

schwalga commented Jan 27, 2023 •

edited

Loading