Merge branch 'ros2' of https://gitlab.rob.uni-luebeck.de/robPublic/navigation_with_polytopes into ros2