Pure Pursuit updates

[SteveMacenski]

Added

• Kinematic acceleration limits
• Slow down on approach
• Collision checking
• Dynamically scaling look ahead distance by speed optionally
• breaking out functions for more easy testing
• Simplify transformations
• carrot position publisher
• collision arc publisher
• constraints on linear velocity on path curvature and costmap costs
• rotate to headings

Tested working...

• [x] collision checking
• [x] dynamic scaling of look ahead point
• [x] kinematic limiting
• [x] slow down on approach
• [x] test generally working after all done: follow path well, doesnt veer off, defaults, can get on paths OK
• [x] potentially look into linear velocity scaling based on curvature (e.g. slow down if lots of curves so we can follow well)
• [x] potentially slow linear vel when close to obstacles
• [x] rotate to path headings
• [x] readme with context and params

Future...

• unit test coverage
• rotate to goal at end
• interpolated lookahead distances

[SteveMacenski]

Nice little visualization of the collision checker points, projecting the velocities forward in time til the carrot (carrot also published as a point). Also just a good little visualization tool for the pure pursuit planner to see what's going on.

[shrijitsingh99]

The visualization look awesome! WIll lookup other implementations/literature of pure pursuit, to see how they are handling some of the issues we are looking to resolve.

[SteveMacenski]

@Rayman @shrijitsingh99 I'm not 100% sure about the velocity scaling based on cost https://github.com/ros-planning/navigation2_tutorials/pull/17/files#diff-897735e06a9135ba25ccaf49336bea674e13ef232a5097224f65868723e5fbe6R402-R415. I think it works (?) but its hard to tell how beneficial it is given the TB3 world has so many obstacles so close together it mostly goes to minimum. The idea if that if you're in a really tight area, you probably want to be going slower. I think its a helpful feature, though open to suggestions or removal.

The linear velocity scaling based on curvature definitely has a positive improvement in performance. Combined with the time-variable collision checking arc, it actually makes for a uniquely good setup. It helps rotate to the path orientation itself without needing the 'rotate to pose' part (we'll still want it but makes it less sensitive since the controller can handle some of it itself) because its a high curvature path, so angular velocity skyrockets and linear velocity drops, so you get an almost rotate to heading behavior. Since the collision checker distance on the arc is proportional to speed, then since this is low, we don't collision check too far head to artificially call for a "collision!" in tight spaces knowing the velocities will drive away from targets. Its not perfect and when highly constrained it doesn't quite get to the path angle, so we'll still need a rotate to heading object. Though it could probably handle itself if not in a constrained space.

They can both be disabled by setting use regulated velocity scaling to false.

[SteveMacenski]

Fun working :-)

[SteveMacenski]

@shrijitsingh99 I'm going to merge this in now to get it building and we can address the heuristics question in a Nav2 PR to follow. I need to keep the ball rolling and there's still another gateway before this is merged into a usable repo

[SteveMacenski]

https://github.com/ros-planning/navigation2/pull/2152 follow up in Nav2