Auton overtake feature, as well as pylint fixes

[Jackack]

What type of PR is this? (check all applicable)

• [x] Refactor
• [x] Feature
• [x] Bug Fix
• [x] Optimization
• [ ] Documentation Update

Description

• Added path planner.
• Refactored autonsystem to run path planning.
• Extended trajectory.py, world.py with methods required by path planner.
• Added the ability to initialize new trajectory from a numpy array.
• Fixed bug in path projection and vectorized the code.
• Added command line options to autonsystem and integrated these options into the sim launch files.
• Modified topic names so that NAND/SC topics do not conflict.
• Added half-plane constraints to MPC code (this is not used for now, but might be interfaced with path planning later)
• Adjusted Stanley controller heading error weight to reduce oscillation
• Cleaned up code to conform to pylint

Related Tickets & Documents

• Related Issue #
• Closes #

QA Instructions, Screenshots, Recordings

To test auton overtaking in sim: run sim_2d_2buggies.launch You will be able to visualize the movements of the 2 simulated buggies in Foxglove.

What you should see on the map visualization are 2 buggies. One will move faster and overtake another, staying 2 meters away at the closest.

Here's the ROSbag of the demo run: https://drive.google.com/file/d/1913GAiZg_PoAF8P0pv_pyOv-dJqz8pLd/view?usp=drive_link

Added/updated tests?

We encourage you to keep the code coverage percentage at 80% and above.

• [ ] Yes
• [ ] No, and this is why: please replace this line with details on why tests have not been included
• [ ] I need help with writing tests

[Jackack]

Due to my git rebase skill issue, the CI commit, which was merged into this branch from main, got rebased :/

A lot of the delta should be due to fixing pylint errors (trailing whitespace, import orders, etc)

[Jackack]

Updated PR template, feel free to check out the demo ROSbag and/or testing this code for yourself.

[christianvluu]

Tested this with a few scenarios on my own computer - looks good.