[Autonomy] Convert Route into Drive Commands

[SwapnilPande]

Which section of robot code is this for?
Route Planning and Execution

Description of feature

• Develop software to generate a drive command based on the calculated route and the current robot position
• Determine how often the route is recalculated
• Build ROS node to perform this functionality

Additional Notes
This will require that you work closely with the team working on robot localization, the high level controller, and route planning to determine how all the software will interface. For example, how often will the route planner update the route, how often will the drive commands be updated, etc. are all questions that need to be addressed.

[partlygloudy]

An algorithm to check out that seems promising is called "pure pursuit". I'll just summarize it briefly here. This assumes that you've already computed the whole route

Repeat until you've reached the end of the path:

1. Find the point on the path which is closest to the robot
2. Figure out the point on the path which is some fixed distance further than the point found in (1)
3. Command the robot to drive towards the point found in (2)

One way to envision what this is doing is to image that you're pulling the robot with a fixed length string. You pull the string along the route you've computed and the robot follows. A nice thing about this approach is that the route you end up driving is a smoothed version of the original path. The shorter the 'string,' the more closely the robot follows the computed path.

Video of this in action: https://www.youtube.com/watch?v=9fzzp6oxid4

[partlygloudy]

If we used that approach, we'd probably only need to update the route when we discovered the location of a new obstacle.

[partlygloudy]

An example which goes more in depth and explains some of the math / implementation: https://www.youtube.com/watch?v=2qGsBClh3hE. The algorithm I described is the same as the one from this video if you set r = 0.

[partlygloudy]

Some experimentation going on at https://github.com/vanderbiltrobotics/research-experimentation/tree/master/route_planning

[BaibhaVatsa]

pure_pursuit.py in https://github.com/vanderbiltrobotics/research-experimentation/tree/master/route_planning is where we are implementing this. Almost done with the helper functions. Have to implement the suggestions Jake made about using more optimized numpy functions for calculations. Plan to work out the target generating function and the function for changing the "string" length dynamically.

[partlygloudy]

I also need to finish implementing a simulator to help with testing this. I plan to have it done by the end of Saturday