second_project_state.py 文件源码

def execute(self, ud):
        if self.preempt_requested():
            self.service_preempt()
            return 'failed'
        rospy.sleep(0.5)
        (x,y,theta) = find_volleyball.find_volleyball().find_volleyball()
        rospy.loginfo("x = %s,y = %s ",x,y)
        self.move_cmd.turn_to(theta*0.95)
        self.move_cmd.move_to(x = math.sqrt(x*x+y*y) - 0.25)
        # self.move_cmd.move_to(y = y)
        # self.move_cmd.move_to(x = x - 0.2 )
        return 'successed'


############################################
################Control#####################
############################################