vocal_interactive_promps.py 文件源码

def set_goal(self):
        if self.promp.num_primitives > 0:
            self.say('Move the robot and say ready to set the goal')

            choice = ""
            while choice != 'ready' and not rospy.is_shutdown():
                choice = self.read_user_input(['ready'])

            eef = self.arm.endpoint_pose()
            state = self.arm.get_current_state()
            goal_set = self.promp.set_goal(eef, state)
            for result in self.promp.goal_log:
                rospy.loginfo(result)
            if goal_set:
                self.say('I can reach this object, let me demonstrate', blocking=False)
                self.arm.move_to_controlled(self.init)
                self.arm.open()
                trajectory = self.promp.generate_trajectory()
                self.arm.execute(trajectory)
                self.arm.close()
                self.arm.translate_to_cartesian([0, 0, 0.2], 'base', 2)
                if self.arm.gripping():
                    self.say('Take it!')
                    self.arm.wait_for_human_grasp(ignore_gripping=False)
                self.arm.open()
            else:
                self.say("I don't know how to reach this object. {}".format(self.promp.status_writing))
        else:
            self.say('There is no demonstration yet, please record at least one demo')