Fix and add motion planner server tests

Builds on top of:

I uncommented the old launch test, and discovered it was passing.

Then I tried adding a test for the motion plan cache, and had it fail because we need access to the robot joint state.

Turns out the old tests weren't loading a ros2_control controller, so I fixed it (maybe that's what broke in the first place?)

After building, test with:

clear && colcon test --packages-select nexus_motion_planner --event-handlers=console_direct+

osrf / nexus