ROS: Implemented state machine