Speed up planner plotting

[sea-bass]

This PR significantly speeds up plotting using the technique described in https://stackoverflow.com/questions/53035858/plot-multiple-values-with-matplotlib-without-loop

To test, you can specifically try some of the multirobot demos / demos with PRMs in them.

[ibrahiminfinite]

There is a small bug from #119 which causes a "referenced before assignment" bug.
Since tgt_loc was replaced with goal_node. this line should also have that change.

I ran the ROS multirobot demo after adding that, there were no errors, but the rendering was spooky, with robots teleporting to different locations, see this video

[sea-bass]

Nice catch on the bug! I pushed that fix directly to main.

The teleporting error is also happening on main right now... I suspect something happened with removing those extra variables in robot.py... will look into it

[sea-bass]

Actually, this issue is because we're publishing commands before the GUI is started, and that causes things to go out of sync. I'll find a fix to wait til the GUI is running before sending this out.

[sea-bass]

@ibrahiminfinite Try again? Just made some changes.

[ibrahiminfinite]

@ibrahiminfinite Try again? Just made some changes.

Ran it again, works smooth and the graph plotting is FAST.
Also very nice to see that the GUI is synced up.

[ibrahiminfinite]

LGTM