Fail simulation tests if a cone is hit
In order to improve the simulation tests, the tests now check to make sure that no cone moves while completing the missions.
To do this I created a new topic /ground_truth/track to which I publish all cones. This topic is subscribed to by the simulation tests. They compare the message with the previous one to see if none of the cones move. I added an uncertainty to the difference in the position of the cones because the cones move very slightly between messages.
In the tests I also added messages to the asserts to make it more clear what happened if a test fails.
I also added the launch_group argument to the empty.launch file because this is used by the inspection mission test, and is required for the gazebo ground truth plugin.
In the gazebo plugin I also added an unknown color cone to the cone color enum, which is returned by the get cone color function when it cannot be figured out what color a cone is. I found that when a cone moves something happens to a link, but that solved the problem.
This branch is branched off from the improve-groun-truth-cones branch, so the merge request for that should be merged first (!36 (merged)).