New pathfinding algorithm
Since I don't think a true optimal route is necessary here I would have checked out RRT instead of a*. There are variants of it that are much much faster that can find almost optimal routes.If you guys want to explore this further you could look into integrating ompl[1]. It has a variety of planners that could be used. I believe you could get an order of magnitude improvement using something like that.[1] https://github.com/ompl/ompl