I'm starting to port this to ROS2. It is needed. @JStech or @RoboticsYY, would you mind creation of a `foxy` branch?