SBPL test environment is working using a first-pass motion primitive file

Run the test sbpl launch file to test sbpl (using rviz)