solves #8 (closed)
based on https://gitlab.lrz.de/cps-robotics/mcs/-/merge_requests/28#587d266bb27a4dc3022bbed44dfa19849df3044c