Additional optimizations for Hybrid-A* / Smac planner

[SteveMacenski]

Precompute

• precompute the dubin/reeds-shepp distances for use in heuristic shouldn't do since no static graph anymore on initialization
• if exact increments of theta, we can precompute the getProjections for all bins. DONE
• if exact increments of theta, we can precompute the footprint collision detection rotations. too involved in costmap 2d, would actually be useful, but out of scope for just this planner. Maybe something to consider for another time.

Parallelize

• parallelize collision checking in analytic expansion -- too many atomic operations to test quickly and too rarely run
• parallelize collision checking lines -- didn't help
• parallelize 3 neighbor find/validate/expand (I dont think 3 is enough to parallelize and be effective with overhead)

General

• find ways to minimize the number of nodes expanded (motion models move more? lower costmap resolution? hueristics drive better, etc) Heuristics very good at it and with the costmap downsampler, this is essentially as good as it gets!
• profile code and see where we can make things faster.
• remove compute of heuristic twice in each node on visitation (e.g. Add waitForServer to TaskClient #45) these are on different nodes
• approximating cos / sin / tan functions

Smoother:

• Look at it with fresh eyes and see if we can simplify / make it better through more standard iterative approaches we have better control over. This currently seems a little rigid and doesn't do all that I'd like it to do (though might just be tuning)
• Look at other options like https://github.com/LiJiangnanBit/path_optimizer https://github.com/gkouros/path-smoothing-ros https://github.com/MithunNallana/path-smoothing https://github.com/linxigjs/path_smoother

[SteveMacenski]

These have been evaluated and precomputing the heading bins is in progress, closing because rest did not either make sense, impact performance enough to be worthwhile, or too time consuming for the marginal benefit. Path smoother ticket branched out into a new ticket.