ROS Smach statemachine for the RoboCup demo challenge