Refactor to be used together with ROS 2 Embedded