Revamp the Goal Setting Algorithm

[zghera]

Description

The current goal-setting algorithm is able to navigate around the autocross map in simulation using real-time 2D laser scans (for the first lap when we don't have a map).

However, there are a number of problems that have been noted with the goal-setting algorithm. These include but are not limited to

• (flaky/inconsistent) The kart can find itself too close to a wall, resulting in one of the following:
  • The simulation ends because the kart is too close to the wall
  • The kart turns around 180 and drives in the opposite direction
  • The kart stops, backs up, and resumes driving
• Very sensitive to wall distance deviations when the kart is far from the wall. This has caused overcorrections that lead to the previously listed item.

Please feel free to create new issues that break this one up into multiple issues.

Sub-tasks

• Update the goal setting design document in the AMP_ASSv2 Wiki. Fill in each of the sections with more detail, especially:

  • Issues with the existing approach were noticed before and during development.
  • The algorithm and parameters regardless of how much you modified the original algorithm/params.
  • ...

• Once the research is complete to understand the current state of the goal-setting algorithm, either:

  • Re-writing the goal-setting algorithm from scratch OR (not both).
  • Tune/modify the existing algorithm.
    To fix the critical issues.

• Make sure your code follows the style guide.

Other Useful Information

• This document described some of the issues seen with goal setting. Some of the items listed (namely number 4) may not be in scope for this PR.

[YKim0172]

For one of the issues mentioned above ("The kart stops , backs up, and resumes driving"), one solution I thought of was modifying the step size of the kart so that there's more time for the kart to process its surroundings and set a new goal.

Of course, the main caveat of this fix (assuming it will resolve the issue) is that the kart won't be going as fast and move in a more jerky fashion.

[YKim0172]

One other issue that I think is crucial to solve our issues outlined above is to find a way to make the Lidar make a much more accurate image of its surroundings. This issue is evident starting from 0:25 in the video that's mentioned by Zach in Issue Autonomous-Motorsports-Purdue/AMP_ASSv1#18. Despite there being plenty of space in the bottom right part of the autocross map, for some reason, the kart thinks that there is a wall instead, so it tries to squeeze between the narrow gap.

[zghera]

@YKim0172 Nice observations so far. I updated the issue description, with the link to the GH Wiki for the goal setting algorithm that you and the team should update.

So please put these observations and future stuff in that wiki.