first_project_state.py 文件源码

def execute(self, ud):
        rospy.loginfo('Start Return to home!!')
        if self.preempt_requested():
            self.service_preempt()
            return 'failed'
        (current_x,current_y) = self.cmd_position.get_robot_current_x_y()
        self.cmd_move.move_to(x = 2-current_x,y = -1.5-current_y)
        # self.cmd_move.move_to(y =  -current_y)
        self.cmd_turn.turn_to(math.pi)
        rospy.sleep(0.5)
        self.cmd_return.go_close_line()
        # self.cmd_move.move_to(x = -2.6)

        return 'successed'

#??????????????????????