Code in python using Smach, ROS libraries