Generate true path heuristic for STA* (save this for each scenario)

[Elucidation]

No description provided.

[Elucidation]

This is becoming necessary as we scale up to a 32x83 warehouse with ~186 robots, a single path planning step often takes > 100ms on failure, because it's going through many dead ends

With a better heuristic we'd likely be finding solutions faster as well as reducing overall wasted time

[Elucidation]

Running on desktop for a few minutes with ~ 47 x 47 grid and 88 robots, tracked distribution of generate_path costs

Generally pretty short < a few ms, but another distribution around 74 ms and then a few long tails > 100ms

Considering a generate_path may get called between 0 to 2 times per robot, a 74ms generate path means only 1-2 calls will be allowed for a < 100ms job processing update.

Need to see where this cost comes, is it from bad heuristics, is it from the quantity of dynamic obstacles / dead ends, or just the total size of the world?

[Elucidation]

Testing existing euclidean heuristic vs true heuristic on a fresh world 32 x 36 grid, 64 robots, 48 item zones, 28 stations. 500 new orders all at once.

Euclidean

Gets stuck after around 700 steps with 17 finished orders, with robot allocator failing to find paths (500 max time, 10,000 max iters) for robots. Generally generate_path was taking > 100ms so only 1 robot job was processed per time step

True Heuristic (takes ~3.5 seconds to pre-compute)

Looks to be functioning well, most robots being activated and used

robot_allocator - INFO - Built true heuristic grid in 3461.91 ms

Somewhat of a bi-modal distribution for both, but we clearly see the true heuristic takes around 1/10th the time of the euclidean heuristic (and also completes where the other fails on maxiter(?))

Bi-modal is possibly due to astar failing out quickly due to a dynamic obstacle blocking all cells or similar

[Elucidation]

With this change, I expected to see CPU relatively unchanged (instead of a quick math calc, it's doing a hash lookup + 2d array lookup per heuristic call), and slightly increased memory for the size of the heuristic table.

What we see is prior had lower CPU usuage, and higher memory usage, so opposite. This sample may be unrelated perhaps.

docker ps on GCE micro instance before change

3dda12edb03d   aw_robot_allocator          5.69%     295.6MiB / 973.9MiB   30.35%    1.8GB / 2.21GB    3.15MB / 0B       2

After

3dda12edb03d   aw_robot_allocator          16.12%    33.1MiB / 973.9MiB    3.40%     956kB / 1.32MB    0B / 0B           2

Looking into the change we also increased max time steps from 200 to 500, so while previously the euclidean was failing out at finding a path and taking >100ms for a single search, the new true heuristic is likely doing much more computation and finding actual paths, with enough time for multiple robots.
This will translate to more IO calls and updates etc. which may be why we're seeing higher CPU usage, and lower memory usage since it's not filling out the A* search grid constantly?

After things have settled looks like it's no longer an issue

3dda12edb03d   aw_robot_allocator          1.29%     29.32MiB / 973.9MiB   3.01%     243MB / 334MB     20.9MB / 28.7kB   2