Example #1
0
class ProgramStore(object):
    def __init__(self):
        self.art = ArtApiHelper()

    def store_programs(self):
        program_headers = self.art.get_program_headers()
        programs = []
        file = open("JSONprograms_" + str(rospy.Time.now().to_nsec()), "w")
        bag = rosbag.Bag(
            'BAGprograms_' + str(rospy.Time.now().to_nsec()) + '.bag', 'w')
        for header in program_headers:
            program = self.art.load_program(header.id)
            programs.append(program)
            to_print = json.loads(
                json_message_converter.convert_ros_message_to_json(program))
            file.write(">>>>>>>>>>>>>>>>>>>>>>>>>>    program id " +
                       str(header.id) + "   <<<<<<<<<<<<<<<<<<<<<<<<<<<<<\n")
            file.write(json.dumps(to_print, indent=2, sort_keys=False))
            file.write("\n\n\n")
            bag.write("program", program)
        file.close()
        bag.close()
Example #2
0
class ArtBrain(object):
    def __init__(self):

        self.ih = InstructionsHelper()
        states = []
        transitions = []
        self.instruction_fsm = {}
        for instruction in self.ih.known_instructions():
            states += self.ih[instruction].brain.fsm.states
            transitions += self.ih[instruction].brain.fsm.transitions
        self.fsm = ArtBrainMachine(states, transitions)

        for instruction in self.ih.known_instructions():
            self.instruction_fsm[instruction] = self.ih[instruction].brain.fsm(
                self)

        for _, fsm in self.instruction_fsm.iteritems():

            for state_function in fsm.state_functions:
                setattr(self.fsm, state_function, getattr(fsm, state_function))

        self.fsm.check_robot_in = self.check_robot_in
        self.fsm.check_robot_out = self.check_robot_out
        self.fsm.is_everything_calibrated = self.is_everything_calibrated
        self.fsm.state_init_ros = self.state_init_ros
        self.fsm.state_waiting_for_action = self.state_waiting_for_action
        self.fsm.state_program_init = self.state_program_init
        self.fsm.state_program_run = self.state_program_run
        self.fsm.state_learning_init = self.state_learning_init
        self.fsm.state_learning_run = self.state_learning_run
        self.fsm.learning_load_block_id = self.learning_load_block_id
        self.fsm.state_program_error = self.state_program_error
        self.fsm.state_program_paused = self.state_program_paused
        self.fsm.state_program_finished = self.state_program_finished
        self.fsm.state_program_load_instruction = self.state_program_load_instruction
        self.fsm.state_learning_step_done = self.state_learning_step_done
        self.fsm.state_learning_step_error = self.state_learning_step_error
        self.fsm.state_learning_done = self.state_learning_done
        self.fsm.state_update_program_item = self.state_update_program_item
        self.fsm.visualize_load_block_id = self.visualize_load_block_id
        self.fsm.state_visualize_run = self.state_visualize_run
        self.fsm.state_visualize_done = self.state_visualize_done

        self.block_id = None
        self.user_id = 0
        self.objects = InstancesArray()
        self.executing_program = False
        self.program_paused = False
        self.program_pause_request = False
        self.learning = False
        self.instruction = None  # type: ProgramItem
        self.holding_object_left = None
        self.holding_object_right = None
        self.stop_server = False
        self.recalibrate = False
        self.table_calibrated = False
        self.table_calibrating = False
        self.cells_calibrated = False
        self.system_calibrated = False
        self.initialized = False
        self.projectors_calibrated = False
        self.last_drill_arm_id = None

        self.learning_block_id = None
        self.learning_item_id = None

        self.quit = False

        self.user_activity = None

        rospy.loginfo('Waiting for other nodes to come up...')

        self.program_resume_after_restart = rospy.get_param(
            "executing_program", False)
        self.learning_resume_after_restart = rospy.get_param(
            "learning_program", False)
        self.rh = None

        while not self.rh and not rospy.is_shutdown():
            try:
                self.rh = ArtRobotHelper()
            except UnknownRobot:
                ArtBrain.fatal("Unknown robot")
                return
            except RobotParametersNotOnParameterServer:
                rospy.logerr("Robot parameters not on parameter server yet...")
                rospy.sleep(1)

        try:
            p, m = rospy.get_param("robot_interface").rsplit('.', 1)
        except (KeyError, ValueError) as e:
            ArtBrain.fatal("Robot interface not set!")
            return
        try:
            mod = importlib.import_module(p)
            self.robot = getattr(mod, m)(self.rh)
        except (ImportError, AttributeError, TypeError) as e:
            ArtBrain.fatal("Failed to import robot interface: " + str(e))
            return

        if not isinstance(self.robot, ArtBrainRobotInterface):
            ArtBrain.fatal("Invalid robot interface.")
            return

        rospy.loginfo("Robot initialized")
        self.user_status_sub = rospy.Subscriber("/art/user/status", UserStatus,
                                                self.user_status_cb)
        self.user_activity_sub = rospy.Subscriber("/art/user/activity",
                                                  UserActivity,
                                                  self.user_activity_cb)
        self.table_calibrated_sub = rospy.Subscriber(
            "/art/interface/touchtable/calibrated", Bool,
            self.table_calibrated_cb)
        self.table_calibrating_sub = rospy.Subscriber(
            "/art/interface/touchtable/calibrating", Bool,
            self.table_calibrating_cb)
        self.system_calibrated_sub = rospy.Subscriber(
            "/art/system/calibrated", Bool, self.system_calibrated_cb)

        self.projectors_calibrated_sub = rospy.Subscriber(
            "/art/interface/projected_gui/projectors_calibrated", Bool,
            self.projectors_calibrated_cb)

        self.srv_program_start = rospy.Service('program/start',
                                               ProgramIdTrigger,
                                               self.program_start_cb)
        self.srv_program_stop = rospy.Service('program/stop', Trigger,
                                              self.program_stop_cb)

        self.srv_program_pause = rospy.Service('program/pause', Trigger,
                                               self.program_pause_cb)
        self.srv_program_resume = rospy.Service('program/resume', Trigger,
                                                self.program_resume_cb)

        self.srv_learning_start = rospy.Service('learning/start',
                                                ProgramIdTrigger,
                                                self.learning_start_cb)
        self.srv_learning_stop = rospy.Service('learning/stop', Trigger,
                                               self.learning_stop_cb)

        self.srv_learning_start = rospy.Service('/art/brain/visualize/start',
                                                ProgramIdTrigger,
                                                self.visualize_start_cb)
        self.srv_learning_stop = rospy.Service('/art/brain/visualize/stop',
                                               Trigger, self.visualize_stop_cb)

        self.srv_program_error_response = rospy.Service(
            'program/error_response', ProgramErrorResolve,
            self.program_error_response_cb)

        self.as_learning_request = actionlib.SimpleActionServer(
            "learning_request",
            LearningRequestAction,
            execute_cb=self.learning_request_cb,
            auto_start=True)

        self.state_manager = InterfaceStateManager(
            InterfaceState.BRAIN_ID,
            cb=self.interface_state_manager_cb)  # TODO callback?
        self.state_manager.set_system_state(InterfaceState.STATE_INITIALIZING)

        self.art = ArtApiHelper(brain=True)
        self.ph = ProgramHelper()

        self.objects_sub = rospy.Subscriber(
            "/art/object_detector/object_filtered", InstancesArray,
            self.objects_cb)

        # TODO use this topic instead of system_state in InterfaceState (duplication) ??
        # TODO move (pub/sub) to InterfaceStateManager?
        self.state_publisher = rospy.Publisher("system_state",
                                               SystemState,
                                               queue_size=1)

        self.tf_listener = TransformListener()
        self.art.wait_for_api()

        self.get_obj_type_srv_client = ArtBrainUtils.create_service_client(
            '/art/db/object_type/get', getObjectType)
        # self.select_arm_srv_client = ArtBrainUtils.create_service_client(
        #    '/art/fuzzy/select_arm', SelectArm)
        self.clear_all_object_flags_srv_client = ArtBrainUtils.create_service_client(
            '/art/object_detector/flag/clear_all', Empty)
        self.clear_object_flag_srv_client = ArtBrainUtils.create_service_client(
            '/art/object_detector/flag/clear', ObjectFlagClear)
        self.set_object_flag_srv_client = ArtBrainUtils.create_service_client(
            '/art/object_detector/flag/set', ObjectFlagSet)

        # TODO 'hack' for experiment
        self.forearm_enable_srv_client = ArtBrainUtils.create_service_client(
            '/art/object_detector/forearm/enable', Empty)
        self.forearm_disable_srv_client = ArtBrainUtils.create_service_client(
            '/art/object_detector/forearm/disable', Empty)

        self.forearm_disable_srv_client.call()

        r = rospy.Rate(1)
        if not self.system_calibrated or not self.projectors_calibrated:
            self.robot.prepare_for_calibration()
        while not self.system_calibrated:
            if rospy.is_shutdown():
                return
            rospy.loginfo("Waiting for system calibration")
            r.sleep()

        self.calibrate_projectors_srv_client = ArtBrainUtils.create_service_client(
            '/art/interface/projected_gui/calibrate_projectors', Trigger)

        if not self.projectors_calibrated:

            resp = self.calibrate_projectors_srv_client.call()

            if not resp.success:

                rospy.logerr("Failed to start projector calibration: " +
                             resp.message)
                # TODO what to do?

            rospy.loginfo("Waiting for projectors to calibrate...")
            while not self.projectors_calibrated:
                if rospy.is_shutdown():
                    return
                rospy.sleep(1)

        if not self.table_calibrated:
            rospy.loginfo(
                'Waiting for /art/interface/touchtable/calibrate service')
            rospy.wait_for_service('/art/interface/touchtable/calibrate')
            rospy.loginfo('Get /art/interface/touchtable/calibrate service')
            self.calibrate_table_srv_client = ArtBrainUtils.create_service_client(
                '/art/interface/touchtable/calibrate', Empty)

            attempt = 1
            rospy.sleep(1)
            while not self.table_calibrated:
                rospy.loginfo("Trying to calibrate table, attempt " +
                              str(attempt))
                self.calibrate_table_srv_client.call()
                rospy.sleep(1)
                attempt += 1
                while not self.table_calibrated and self.table_calibrating:
                    rospy.sleep(1)
        r = rospy.Rate(1)
        while self.robot.halted and not rospy.is_shutdown():
            rospy.logwarn("Robot halted! Please unhalt!")
            r.sleep()

        self.try_robot_arms_get_ready()
        rospy.loginfo("Brain init done")
        self.initialized = True
        self.fsm.init()

    @staticmethod
    def fatal(msg):

        rospy.logfatal(msg)
        rospy.signal_shutdown(msg)

    # ***************************************************************************************
    #                                       STATES
    # ***************************************************************************************

    def state_init_ros(self, event):
        rospy.logdebug('Current state: state_init_ros')
        self.fsm.init_done()

    def state_waiting_for_action(self, event):
        rospy.logdebug('Current state: state_waiting_for_action')

        if self.program_resume_after_restart or self.learning_resume_after_restart:
            program_id = rospy.get_param("program_id", None)
            self.block_id = rospy.get_param("block_id", None)
            item_id = rospy.get_param("item_id", None)
            if self.block_id is None or item_id is None or program_id is None:
                rospy.logwarn("Could not resume program!")
                self.learning_resume_after_restart = False
                self.program_resume_after_restart = False
                return
            program = self.art.load_program(program_id)

            if not self.ph.load(program):
                rospy.logwarn("Could not resume program!")
                rospy.delete_param('program_id')
                rospy.delete_param('block_id')
                rospy.delete_param('item_id')
                self.state_manager.set_system_state(InterfaceState.STATE_IDLE)
                return
            if self.program_resume_after_restart:
                rospy.logdebug('Starting program')

                rospy.Timer(rospy.Duration(1),
                            self.program_start_timer_cb,
                            oneshot=True)
                rospy.logdebug("program started")
            elif self.learning_resume_after_restart:
                self.learning_resume_after_restart = False
                rospy.logdebug('Starting learning')
                self.state_manager.update_program_item(program_id,
                                                       self.block_id,
                                                       self.ph.get_item_msg(
                                                           self.block_id,
                                                           item_id),
                                                       auto_send=True)
                self.state_manager.set_system_state(
                    InterfaceState.STATE_LEARNING)
                self.fsm.learning_start()
        else:
            self.state_manager.set_system_state(InterfaceState.STATE_IDLE)

    def state_shutdown(self, event):
        rospy.logdebug('Current state: state_shutdown')
        sys.exit()

    # ***************************************************************************************
    #                                  STATES PROGRAM
    # ***************************************************************************************

    def state_program_init(self, event):
        rospy.logdebug('Current state: state_program_init')
        rospy.logdebug('New program ready!')
        rospy.set_param("executing_program", True)

        if self.ph is None:
            self.fsm.error(severity=ArtBrainErrorSeverities.SEVERE,
                           error=ArtBrainErrors.ERROR_NO_PROGRAM_HELPER)
            return
        item_id = None
        if self.program_resume_after_restart:
            item_id = rospy.get_param("item_id", None)
        else:
            self.clear_all_object_flags_srv_client.call(EmptyRequest())

            (self.block_id, item_id) = self.ph.get_first_item_id()
        self.instruction = self.ph.get_item_msg(self.block_id, item_id)

        rospy.set_param("program_id", self.ph.get_program_id())
        rospy.set_param("block_id", self.block_id)
        rospy.set_param("item_id", item_id)
        self.executing_program = True
        self.program_paused = False
        self.program_pause_request = False
        if self.program_resume_after_restart:
            self.robot.init_arms(reset_holding_object=False)
        else:
            self.robot.init_arms(reset_holding_object=True)
        self.state_manager.set_system_state(
            InterfaceState.STATE_PROGRAM_RUNNING, auto_send=False)
        self.fsm.program_init_done()

    def state_program_run(self, event):
        rospy.logdebug('Current state: state_program_run')

        if not self.executing_program:
            self.fsm.finished()
            return
        if self.instruction is None:
            self.fsm.error(severity=ArtBrainErrorSeverities.SEVERE,
                           error=ArtBrainErrors.ERROR_NO_INSTRUCTION)
            return
        if not self.check_robot():
            return
        rospy.logdebug('Program id: ' + str(self.ph.get_program_id()) +
                       ', item id: ' + str(self.instruction.id) +
                       ', item type: ' + str(self.instruction.type))

        for flag in self.instruction.flags:
            if flag.key == "CLEAR_OBJECT_FLAGS" and flag.value == "true":
                self.clear_all_object_flags_srv_client.call(EmptyRequest())
        '''while self.program_resume_after_restart and self.instruction.type in [ProgramItem.PLACE_TO_POSE,
                                                                           ProgramItem.PLACE_TO_GRID]:

            (self.block_id, item_id) = self.ph.get_id_on_success(
                self.block_id, self.instruction.id)

            if self.block_id == 0:
                self.fsm.finished()
                return

            self.instruction = self.ph.get_item_msg(self.block_id, item_id)
        '''
        self.program_resume_after_restart = False

        rospy.set_param("program_id", self.ph.get_program_id())
        rospy.set_param("block_id", self.block_id)
        rospy.set_param("item_id", self.instruction.id)
        try:
            self.instruction_fsm[self.instruction.type].run()
        except InstructionsHelperException:
            self.fsm.error()
            return

    def state_program_load_instruction(self, event):
        rospy.logdebug('Current state: state_program_load_instruction')
        success = event.kwargs.get('success', True)
        self.state_manager.set_error(0, 0, auto_send=False)
        if not self.executing_program:
            self.fsm.finished()
            return
        if success:
            self.block_id, item_id = self.ph.get_id_on_success(
                self.block_id, self.instruction.id)
        else:
            self.block_id, item_id = self.ph.get_id_on_failure(
                self.block_id, self.instruction.id)

        if self.block_id == 0:
            self.fsm.finished()
            return

        self.instruction = self.ph.get_item_msg(self.block_id, item_id)

        if self.instruction is None:
            self.executing_program = False
            self.fsm.error()
            return
        if self.program_pause_request:
            self.fsm.pause()
            return
        self.fsm.done()

    def state_update_program_item(self, event):
        rospy.logerr("state_update_program_item")
        self.state_manager.update_program_item(self.ph.get_program_id(),
                                               self.block_id,
                                               self.instruction,
                                               auto_send=False)

    def state_program_paused(self, event):
        rospy.logdebug('Current state: state_program_paused')
        self.state_manager.set_system_state(
            InterfaceState.STATE_PROGRAM_STOPPED)
        self.state_manager.send()
        self.program_paused = True
        self.program_pause_request = False

    def state_program_finished(self, event):
        rospy.logdebug('Current state: state_program_finished')
        self.state_manager.set_system_state(
            InterfaceState.STATE_PROGRAM_FINISHED)
        self.robot.arms_reinit()
        self.state_manager.send()
        self.executing_program = False
        self.robot.arms_reinit()
        rospy.set_param("executing_program", False)
        rospy.delete_param("program_id")
        rospy.delete_param("block_id")
        rospy.delete_param("item_id")
        self.program_resume_after_restart = False
        self.fsm.done()

    def state_program_error(self, event):
        rospy.logdebug('Current state: state_program_error')
        severity = event.kwargs.get('severity', ArtBrainErrorSeverities.SEVERE)
        error = event.kwargs.get('error', None)
        rospy.logerr("Error: " + str(error))
        rospy.logerr("Severity of error: " + str(repr(severity)))

        if severity is None or error is None:
            severity = ArtBrainErrorSeverities.SEVERE
            error = ArtBrainErrors.ERROR_UNKNOWN
        self.state_manager.set_error(severity, error)

        if severity == ArtBrainErrorSeverities.SEVERE:
            # handle
            self.fsm.program_error_shutdown()
            return

        elif severity == ArtBrainErrorSeverities.ERROR:
            self.fsm.program_error_fatal()
            return

        elif severity == ArtBrainErrorSeverities.WARNING:
            if error == ArtBrainErrors.ERROR_OBJECT_MISSING or \
               error == ArtBrainErrors.ERROR_OBJECT_MISSING_IN_POLYGON:
                rospy.logwarn("Object is missing")
            elif error == ArtBrainErrors.ERROR_PICK_FAILED:
                rospy.logwarn("Pick failed")
                self.try_robot_arms_get_ready()
                self.robot.init_arms()

            rospy.logwarn("Waiting for user response")

            return

        elif severity == ArtBrainErrorSeverities.INFO:
            (self.block_id,
             item_id) = self.ph.get_id_on_failure(self.block_id,
                                                  self.instruction.id)
            self.fsm.done(success=False)
        else:
            pass

        self.executing_program = False
        self.program_error_handled()

    # ***************************************************************************************
    #                                  STATES TEACHING
    # ***************************************************************************************

    def state_learning_init(self, event):
        rospy.logdebug('Current state: Teaching init')
        self.learning = True
        rospy.set_param("learning_program", True)
        self.fsm.init_done()

    def learning_load_block_id(self, event):
        self.block_id = self.state_manager.state.block_id

    def state_learning_run(self, event):
        rospy.logdebug('Current state: state_learning_run')

    def state_learning_step_error(self, event):
        rospy.logdebug('Current state: state_learning_step_error')
        severity = event.kwargs.get('severity', ArtBrainErrorSeverities.SEVERE)
        error = event.kwargs.get('error', ArtBrainErrors.ERROR_UNKNOWN)
        rospy.logdebug("Severity of error: " + str(severity))
        rospy.logdebug("Severity of error: " + str(error))
        self.state_manager.set_error(severity, error)
        self.state_manager.set_error(0, 0)

        if error is None:
            pass
            # TODO: kill brain
        if severity == ArtBrainErrorSeverities.SEVERE:
            pass
            # TODO: stop learning
        elif severity == ArtBrainErrorSeverities.ERROR:
            pass
        elif severity == ArtBrainErrorSeverities.WARNING:
            if error == ArtBrainErrors.ERROR_OBJECT_MISSING or \
               error == ArtBrainErrors.ERROR_OBJECT_MISSING_IN_POLYGON:
                rospy.logwarn("Object is missing")
            elif error == ArtBrainErrors.ERROR_PICK_FAILED:
                rospy.logwarn("Pick failed")
                self.try_robot_arms_get_ready()
                self.robot.init_arms()

        elif severity == ArtBrainErrorSeverities.INFO:

            pass
        self.fsm.error_handled()

    def state_learning_step_done(self, event):
        rospy.logdebug('Current state: state_learning_step_done')
        self.fsm.done()
        pass

    def state_learning_done(self, event):
        rospy.logdebug('Current state: state_learning_done')
        self.fsm.done()
        pass

    # ***************************************************************************************
    #                                  STATES VISUALIZING
    # ***************************************************************************************
    '''def state_visualize_init(self, event):
        rospy.logdebug('Current state: Visualize init')
        self.visualizing = True
        rospy.set_param("visualize_program", True)
        #self.fsm.init_done()'''

    def visualize_load_block_id(self, event):
        self.block_id = self.state_manager.state.block_id

    def state_visualize_run(self, event):
        rospy.logdebug('Current state: state_visualize_run')

    def state_visualize_done(self, event):
        rospy.logdebug('Current state: state_visualize_done')
        self.fsm.done()
        pass

    # ***************************************************************************************
    #                                     MANIPULATION
    # ***************************************************************************************

    def try_robot_arms_get_ready(self, arm_ids=[], max_attempts=3):
        assert isinstance(arm_ids, list)
        if self.robot.halted:
            return False

        attempt = 0
        while True:
            if attempt >= max_attempts:
                rospy.logerr("Failed to get ready")
                return False
            severity, error, arm_id = self.robot.arms_get_ready(arm_ids)
            if error is not None:
                attempt += 1
                rospy.logwarn("Error while getting ready: " + str(arm_id) +
                              " , attempt: " + str(attempt))
                continue
            else:
                rospy.loginfo("Robot ready")
                return True

    # ***************************************************************************************
    #                                        OTHERS
    # ***************************************************************************************

    def check_robot_in(self, event):
        self.check_robot()

    def check_robot_out(self, event):
        halted = event.kwargs.get('halted', False)
        # print halted
        if halted:
            return
        self.check_robot()

    def program_start_timer_cb(self, event):
        self.fsm.program_start()

    def program_resume_timer_cb(self, event):
        self.state_manager.set_system_state(
            InterfaceState.STATE_PROGRAM_RUNNING)
        self.fsm.resume()

    def program_try_again_timer_cb(self, event):
        self.fsm.try_again()

    def program_skip_instruction_timer_cb(self, event):
        self.fsm.skip(success=True)

    def program_fail_instruction_timer_cb(self, event):
        self.fsm.fail(success=False)

    def update_state_manager(self):
        self.state_manager.update_program_item(self.ph.get_program_id(),
                                               self.block_id, self.instruction)

    def is_table_calibrated(self):
        return self.table_calibrated

    def is_everything_calibrated(self, event=None):

        return self.table_calibrated and self.system_calibrated

    def check_robot(self):
        if self.robot.is_halted():
            self.fsm.error(severity=ArtBrainErrorSeverities.WARNING,
                           error=ArtBrainErrors.ERROR_ROBOT_HALTED)
            return False
        else:
            return True

    def check_place_pose(self, place_pose, obj):
        w1 = self.get_object_max_width(obj)
        if w1 is None:
            return False
        for o in self.objects.instances:
            if o.object_id == obj.object_id:
                continue
            w2 = self.get_object_max_width(o)
            if w2 is None:
                # TODO: how to deal with this
                return False

            d = ArtBrainUtils.distance_2d(place_pose.pose, o.pose)
            if d < (w1 + w2):
                rospy.logerr('Another object too close to desired place pose')
                return False
        return True

    def check_gripper(self, gripper):
        if gripper is None:
            rospy.logerr("No gripper!")
            self.fsm.error(severity=ArtBrainErrorSeverities.SEVERE,
                           error=ArtBrainErrors.ERROR_NO_GRIPPER_AVAILABLE)
            return False

        if gripper.pp_client is None:
            rospy.logerr("No pick place client!")
            self.fsm.error(
                severity=ArtBrainErrorSeverities.SEVERE,
                error=ArtBrainErrors.ERROR_GRIPPER_PP_CLIENT_MISSING)
            return False

        if not gripper.pp_client.wait_for_server(rospy.Duration(2)):
            rospy.logerr("Pick place server is not running!")
            self.fsm.error(
                severity=ArtBrainErrorSeverities.WARNING,
                error=ArtBrainErrors.ERROR_PICK_PLACE_SERVER_NOT_READY)
            return False

        return True

    def check_gripper_for_place(self, gripper):
        if not self.check_gripper(gripper):
            return False

        if gripper.holding_object is None:
            rospy.logwarn("Place: gripper " + gripper.name +
                          " is not holding object")
            self.fsm.error(severity=ArtBrainErrorSeverities.WARNING,
                           error=ArtBrainErrors.ERROR_NO_OBJECT_IN_GRIPPER)
            return False

        return True

    def check_gripper_for_pick(self, gripper):
        if not self.check_gripper(gripper):
            return False
        if gripper.holding_object is not None:
            rospy.logwarn("Pick: gripper " + gripper.name +
                          " already holding an object (" +
                          gripper.holding_object.object_id + ")")
            self.fsm.error(severity=ArtBrainErrorSeverities.WARNING,
                           error=ArtBrainErrors.ERROR_OBJECT_IN_GRIPPER)
            return False

        return True

    # ***************************************************************************************
    #                                     ROS COMMUNICATION
    # ***************************************************************************************

    def program_pause_cb(self, req):
        resp = TriggerResponse()
        resp.success = True
        if not self.executing_program:
            resp.success = False
            resp.message = "Program si not running!"
        else:
            self.program_pause_request = True
        return resp

    def program_resume_cb(self, req):
        resp = TriggerResponse()
        resp.success = True
        if not self.executing_program:
            resp.success = False
            resp.message = "Program si not running!"
        elif not self.program_paused:
            resp.success = False
            resp.message = "Program is not paused"
        else:
            self.program_paused = False
            rospy.Timer(rospy.Duration(1),
                        self.program_resume_timer_cb,
                        oneshot=True)
        return resp

    def program_start_cb(self, req):

        resp = ProgramIdTriggerResponse()

        if not self.is_everything_calibrated():
            resp.success = False
            resp.error = 'Something is not calibrated'
            rospy.logwarn('Something is not calibrated')
            return resp

        if self.executing_program:

            resp.success = False
            resp.error = 'Program already running'
            return resp

        if not self.fsm.is_waiting_for_action():
            resp.success = False
            resp.error = 'Not ready for program start!'
            return resp

        rospy.logdebug('Loading program ' + str(req.program_id) +
                       ' from db...')

        program = self.art.load_program(req.program_id)

        if not self.ph.load(program):
            resp.success = False
            resp.error = 'Cannot get program'
            return resp

        rospy.logdebug('Starting program')

        rospy.Timer(rospy.Duration(1),
                    self.program_start_timer_cb,
                    oneshot=True)
        rospy.logdebug("program started")
        resp.success = True
        return resp

    def program_stop_cb(self, req):
        resp = TriggerResponse()
        resp.success = True
        if self.executing_program:
            rospy.logdebug('Stopping program')
            self.executing_program = False
            if self.program_paused:
                self.program_paused = False
                rospy.Timer(rospy.Duration(0, 100),
                            self.program_resume_timer_cb,
                            oneshot=True)
        else:
            resp.success = False
            resp.message = "Program is not running"

        return resp

    def program_error_response_cb(self, req):
        resp = ProgramErrorResolveResponse()
        if not self.fsm.is_program_error():
            resp.success = False
            return resp
        rospy.logdebug('We\'ve got response to our program error')
        if req.user_response_type == ProgramErrorResolveRequest.TRY_AGAIN:
            self.state_manager.set_error(0, 0)
            rospy.Timer(rospy.Duration(1),
                        self.program_try_again_timer_cb,
                        oneshot=True)
        elif req.user_response_type == ProgramErrorResolveRequest.SKIP_INSTRUCTION:
            rospy.Timer(rospy.Duration(1),
                        self.program_skip_instruction_timer_cb,
                        oneshot=True)
        elif req.user_response_type == ProgramErrorResolveRequest.FAIL_INSTRUCTION:
            rospy.Timer(rospy.Duration(1),
                        self.program_fail_instruction_timer_cb,
                        oneshot=True)
        elif req.user_response_type == ProgramErrorResolveRequest.END_PROGRAM:
            self.fsm.program_error_fatal()
        resp.success = True
        return resp

    def learning_start_cb(self, req):
        resp = ProgramIdTriggerResponse()
        resp.success = False

        if not self.is_everything_calibrated():

            resp.error = 'Something is not calibrated'
            rospy.logwarn('Something is not calibrated')
            return resp

        if not self.fsm.is_waiting_for_action():

            resp.error = 'Not ready for learning start!'
            rospy.logwarn('Not ready for learning start!')
            return resp

        program = self.art.load_program(req.program_id)

        if not self.ph.load(program):
            resp.success = False
            resp.error = 'Cannot get program.'
            return resp

        rospy.logdebug('Starting learning')
        (self.block_id, item_id) = self.ph.get_first_item_id()
        self.state_manager.update_program_item(req.program_id,
                                               self.block_id,
                                               self.ph.get_item_msg(
                                                   self.block_id, item_id),
                                               auto_send=False)
        self.state_manager.set_system_state(InterfaceState.STATE_LEARNING)
        resp.success = True
        self.fsm.learning_start()
        return resp

    def visualize_start_cb(self, req):
        resp = ProgramIdTriggerResponse()
        resp.success = False

        if not self.is_everything_calibrated():

            resp.error = 'Something is not calibrated'
            rospy.logwarn('Something is not calibrated')
            return resp

        if not self.fsm.is_waiting_for_action():

            resp.error = 'Not ready for visualize start!'
            rospy.logwarn('Not ready for visualize start!')
            return resp

        program = self.art.load_program(req.program_id)

        if not self.ph.load(program):
            resp.success = False
            resp.error = 'Cannot get program.'
            return resp

        rospy.logdebug('Starting visualize')
        (self.block_id, item_id) = self.ph.get_first_item_id()
        self.state_manager.update_program_item(req.program_id,
                                               self.block_id,
                                               self.ph.get_item_msg(
                                                   self.block_id, item_id),
                                               auto_send=False)
        self.state_manager.set_system_state(InterfaceState.STATE_VISUALIZE)
        resp.success = True
        self.fsm.visualize_start()
        return resp

    def learning_stop_cb(self, req):
        resp = TriggerResponse()
        if not self.fsm.is_learning_run:
            resp.success = False
        rospy.logdebug('Stopping learning')
        self.learning = False
        rospy.set_param("learning_program", False)
        resp.success = True
        self.fsm.learning_done()
        return resp

    def visualize_stop_cb(self, req):
        resp = TriggerResponse()
        if not self.fsm.is_visualize_run:
            resp.success = False
        rospy.logdebug('Stopping visualize')
        resp.success = True
        self.fsm.visualize_done()
        return resp

    def interface_state_manager_cb(
            self,
            state,  # type: InterfaceState
            msg,  # type: InterfaceState
            flags):
        if msg.interface_id != InterfaceState.BRAIN_ID:
            if msg.system_state == InterfaceState.STATE_LEARNING:
                self.ph.set_item_msg(msg.block_id, msg.program_current_item)
                rospy.set_param("program_id", self.ph.get_program_id())
                rospy.set_param("block_id", self.block_id)
                rospy.set_param("item_id", msg.program_current_item.id)
                self.art.store_program(self.ph.get_program())
        pass

    def projectors_calibrated_cb(self, msg):

        self.projectors_calibrated = msg.data

    def user_status_cb(self, req):
        self.user_id = req.user_id

    def user_activity_cb(self, req):
        self.user_activity = req.activity

    def objects_cb(self, req):
        self.objects = req

    def table_calibrated_cb(self, req):
        self.table_calibrated = req.data

    def table_calibrating_cb(self, req):
        self.table_calibrating = req.data

    def system_calibrated_cb(self, req):
        self.system_calibrated = req.data

    def get_object_max_width(self, obj):
        if obj is None:
            rospy.logerr('No object is specified')
            return None
        try:
            obj_type = self.get_obj_type_srv_client.call(obj.object_type)
            if not obj_type.success:
                rospy.logerr('No object with id ' + str(obj.object_id) +
                             ' found')
                return None
            if obj_type.object_type.bbox.type != SolidPrimitive.BOX:
                rospy.logerr(
                    'Sorry, only BOX type objects are supported at the moment')
            x = obj_type.object_type.bbox.dimensions[SolidPrimitive.BOX_X]
            y = obj_type.object_type.bbox.dimensions[SolidPrimitive.BOX_Y]
            return np.hypot(x / 2, y / 2)
        except rospy.ServiceException as e:
            rospy.logerr('Service call failed: ' + str(e))
            return None

    def learning_request_cb(self, goal):
        result = LearningRequestResult()
        result.success = True

        if not self.fsm.is_learning_run:
            rospy.logerr(
                "Not in learning mode but got learning request goal: " +
                str(goal.request))
            result.success = False
            result.message = "Not in learning mode!"
            self.as_learning_request.set_aborted(result)
            return

        rospy.logdebug("Learning_request goal: " + str(goal.request))

        instruction = self.state_manager.state.program_current_item  # type: ProgramItem
        self.instruction = instruction

        self.state_manager.state.edit_enabled = False
        self.state_manager.send()

        if goal.request == LearningRequestGoal.GET_READY:

            self.state_manager.state.edit_enabled = True

            self.instruction_fsm[
                instruction.type].learning()  # TODO check and handle errors

            self.state_manager.state.edit_enabled = True
            self.state_manager.send()
            self.as_learning_request.set_succeeded(result)

        elif goal.request == LearningRequestGoal.EXECUTE_ITEM:
            self.ph.set_item_msg(self.state_manager.state.block_id,
                                 instruction)

            self.instruction_fsm[instruction.type].learning_run(
            )  # TODO check and handle errors
            self.as_learning_request.set_succeeded(result)

        elif goal.request == LearningRequestGoal.DONE:

            self.fsm.done()  # TODO check and handle errors?
            self.as_learning_request.set_succeeded(result)
        else:

            result.success = False
            result.message = "Unknown request"
            self.as_learning_request.set_aborted(result)
Example #3
0
class UICoreRos(UICore):
    """The class builds on top of UICore and adds ROS-related stuff and application logic.

    Attributes:
        user_status (UserStatus): current user tracking status
        fsm (FSM): state machine maintaining current state of the interface and proper transitions between states
        state_manager (interface_state_manager): synchronization of interfaces within the ARTable system
        scene_pub (rospy.Publisher): publisher for scene images
        last_scene_update (rospy.Time): time when the last scene image was published
        scene_img_deq (Queue.Queue): thread-safe queue for scene images (which are published in separate thread)
        projectors (list): array of ProjectorHelper instances
        art (ArtApiHelper): easy access to ARTable services

    """
    def __init__(self):

        origin = rospy.get_param("scene_origin")
        size = rospy.get_param("scene_size")
        rpm = rospy.get_param("rpm")
        port = rospy.get_param("scene_server_port")

        super(UICoreRos, self).__init__(origin[0], origin[1], size[0], size[1],
                                        rpm, port)

        QtCore.QObject.connect(self, QtCore.SIGNAL('objects'),
                               self.object_cb_evt)
        QtCore.QObject.connect(self, QtCore.SIGNAL('user_status'),
                               self.user_status_cb_evt)
        QtCore.QObject.connect(self, QtCore.SIGNAL('interface_state'),
                               self.interface_state_evt)

        QtCore.QObject.connect(self,
                               QtCore.SIGNAL('touch_calibration_points_evt'),
                               self.touch_calibration_points_evt)
        QtCore.QObject.connect(self, QtCore.SIGNAL('touch_detected_evt'),
                               self.touch_detected_evt)
        QtCore.QObject.connect(self, QtCore.SIGNAL('notify_user_evt'),
                               self.notify_user_evt)
        QtCore.QObject.connect(self,
                               QtCore.SIGNAL('learning_request_done_evt'),
                               self.learning_request_done_evt)

        self.user_state = None

        self.program_list = None
        self.program_vis = None
        self.template = False  # TODO this should be stored in program_vis?
        self.last_prog_pos = (0.2, self.height - 0.2)
        self.last_edited_prog_id = None

        self.ph = ProgramHelper()

        cursors = rospy.get_param("~cursors", [])
        for cur in cursors:
            PoseStampedCursorItem(self.scene, cur)

        TouchTableItem(
            self.scene, '/art/interface/touchtable/touch',
            list(self.get_scene_items_by_type(PoseStampedCursorItem)))

        self.stop_btn = ButtonItem(self.scene, 0, 0, "STOP", None,
                                   self.stop_btn_clicked, 2.0, QtCore.Qt.red)
        self.stop_btn.setPos(
            self.scene.width() - self.stop_btn.boundingRect().width() - 300,
            self.scene.height() - self.stop_btn.boundingRect().height() - 60)
        self.stop_btn.set_enabled(True)

        self.projectors = []

        projs = rospy.get_param("~projectors", [])
        for proj in projs:
            self.projectors.append(ProjectorHelper(proj))

        rospy.loginfo("Waiting for /art/brain/learning_request")
        self.learning_action_cl = actionlib.SimpleActionClient(
            '/art/brain/learning_request', LearningRequestAction)
        self.learning_action_cl.wait_for_server()

        self.art = ArtApiHelper()

        self.projectors_calibrated_pub = rospy.Publisher(
            "~projectors_calibrated", Bool, queue_size=1, latch=True)
        self.projectors_calibrated_pub.publish(False)

        self.start_learning_srv = rospy.ServiceProxy(
            '/art/brain/learning/start',
            startProgram)  # TODO wait for service? where?
        self.stop_learning_srv = rospy.ServiceProxy(
            '/art/brain/learning/stop',
            Trigger)  # TODO wait for service? where?

        self.program_pause_srv = rospy.ServiceProxy('/art/brain/program/pause',
                                                    Trigger)

        self.program_resume_srv = rospy.ServiceProxy(
            '/art/brain/program/resume', Trigger)

        self.program_stop_srv = rospy.ServiceProxy('/art/brain/program/stop',
                                                   Trigger)

        self.emergency_stop_srv = rospy.ServiceProxy(
            '/pr2_ethercat/halt_motors',
            EmptyService)  # TODO wait for service? where?

        self.emergency_stop_reset_srv = rospy.ServiceProxy(
            '/pr2_ethercat/reset_motors',
            EmptyService)  # TODO wait for service? where?

        self.program_error_resolve_srv = rospy.ServiceProxy(
            '/art/brain/program/error_response',
            ProgramErrorResolve)  # TODO wait for service? where?
        self.program_error_dialog = None

        self.grasp_dialog = None

        self.emergency_stopped = False

        rospy.loginfo("Waiting for ART services...")
        self.art.wait_for_api()

        rospy.loginfo("Ready! Starting state machine.")

        # TODO move this to ArtApiHelper ??
        self.obj_sub = rospy.Subscriber('/art/object_detector/object_filtered',
                                        InstancesArray,
                                        self.object_cb,
                                        queue_size=1)
        self.user_status_sub = rospy.Subscriber('/art/user/status',
                                                UserStatus,
                                                self.user_status_cb,
                                                queue_size=1)

        self.touch_points = None
        self.touch_calib_srv = rospy.Service(
            '/art/interface/projected_gui/touch_calibration',
            TouchCalibrationPoints, self.touch_calibration_points_cb)
        self.notify_user_srv = rospy.Service(
            '/art/interface/projected_gui/notify_user', NotifyUser,
            self.notify_user_srv_cb)

        proj_calib = True

        if len(self.projectors) > 0:
            rospy.loginfo("Waiting for projector nodes...")
            for proj in self.projectors:
                proj.wait_until_available()
                if not proj.is_calibrated():
                    proj_calib = False

        if proj_calib:

            rospy.loginfo('Projectors already calibrated.')
            self.projectors_calibrated_pub.publish(True)

        else:

            rospy.loginfo(
                'Projectors not calibrated yet - waiting for command...')

        self.projector_calib_srv = rospy.Service(
            '/art/interface/projected_gui/calibrate_projectors', Trigger,
            self.calibrate_projectors_cb)

        self.state_manager = InterfaceStateManager("PROJECTED UI",
                                                   cb=self.interface_state_cb)

    def touch_calibration_points_evt(self, pts):

        for it in self.scene.items():

            if isinstance(it, LabelItem):
                continue

            it.setVisible(False)  # TODO remember settings (how?)

        self.notif(translate(
            "UICoreRos",
            "Touch table calibration started. Please press the white point."),
                   temp=False)
        self.touch_points = TouchPointsItem(self.scene, pts)

    def save_gripper_pose_cb(self, idx):

        topics = [
            '/art/pr2/right_arm/gripper/pose', '/art/pr2/left_arm/gripper/pose'
        ]

        # wait for message, set pose
        try:
            ps = rospy.wait_for_message(topics[idx], PoseStamped, timeout=2)
        except (rospy.ROSException) as e:
            rospy.logerr(str(e))
            self.notif(translate("UICoreRos", "Failed to store gripper pose."),
                       temp=False)
            return

        self.notif(translate("UICoreRos", "Gripper pose stored."), temp=False)
        self.program_vis.set_pose(ps)

    def touch_calibration_points_cb(self, req):

        resp = TouchCalibrationPointsResponse()

        pts = []

        for pt in req.points:

            pts.append((pt.point.x, pt.point.y))

        self.emit(QtCore.SIGNAL('touch_calibration_points_evt'), pts)
        self.touched_sub = rospy.Subscriber(
            '/art/interface/touchtable/touch_detected',
            Empty,
            self.touch_detected_cb,
            queue_size=10)
        resp.success = True
        return resp

    def touch_detected_evt(self, msg):

        if self.touch_points is None:
            return

        if not self.touch_points.next():

            self.notif(translate("UICoreRos", "Touch saved."), temp=True)

            for it in self.scene.items():

                if isinstance(it, LabelItem):
                    continue

                # TODO fix this - in makes visible even items that are invisible by purpose
                it.setVisible(True)

            self.notif(translate("UICoreRos",
                                 "Touch table calibration finished."),
                       temp=False)
            self.scene.removeItem(self.touch_points)
            self.touch_points = None
            self.touched_sub.unregister()

        else:

            self.notif(translate("UICoreRos", "Touch saved."), temp=True)
            self.notif(translate("UICoreRos", "Please press the next point."),
                       temp=False)

    def touch_detected_cb(self, msg):

        self.emit(QtCore.SIGNAL('touch_detected_evt'), msg)

    def calibrate_projectors_cb(self, req):

        resp = TriggerResponse()
        resp.success = True

        # call to start_projector_calibration is blocking
        self.proj_calib_timer = rospy.Timer(rospy.Duration(0.001),
                                            self.start_projector_calibration,
                                            oneshot=True)

        return resp

    def notify_user_srv_cb(self, req):

        self.emit(QtCore.SIGNAL('notify_user_evt'), req)
        return NotifyUserResponse()

    def notify_user_evt(self, req):

        # TODO message should be displayed until user closes it
        if req.duration == rospy.Duration(0):
            self.notif(req.message, message_type=req.type)
        else:
            self.notif(req.message,
                       min_duration=req.duration.to_sec(),
                       temp=True,
                       message_type=req.type)

    def stop_btn_clicked(self, btn):

        try:

            if self.emergency_stopped:
                self.emergency_stop_reset_srv.call()
                self.emergency_stopped = False
                self.stop_btn.set_caption("STOP")
                self.stop_btn.set_background_color(QtCore.Qt.red)
                self.notif(translate("UICoreRos", "Resetting motors"),
                           temp=True)
            else:
                self.emergency_stop_srv.call()
                self.emergency_stopped = True
                self.stop_btn.set_caption("RUN")
                self.stop_btn.set_background_color(QtCore.Qt.green)
                self.notif(translate("UICoreRos", "Emergency stop pressed"),
                           temp=True)

        except rospy.service.ServiceException:

            self.notif(translate("UICoreRos", "Failed to stop/run robot."),
                       temp=True)

    def program_error_dialog_cb(self, idx):

        req = ProgramErrorResolveRequest()
        req.user_response_type = idx + 1
        resp = None
        try:
            resp = self.program_error_resolve_srv(req)
        except rospy.ServiceException as e:
            rospy.logerr("Service call failed: " + str(e))

        if resp is None or not resp.success:

            self.notif(translate("UICoreRos",
                                 "System failure: failed to resolve error."),
                       temp=True)

        self.scene.removeItem(self.program_error_dialog)
        self.program_error_dialog = None

    def interface_state_evt(self, old_state, state, flags):

        system_state_changed = old_state.system_state != state.system_state

        if system_state_changed:
            rospy.logdebug("New system state: " + str(state.system_state) +
                           ", was: " + str(old_state.system_state))
            self.clear_all(True)

        if state.error_severity == InterfaceState.NONE and self.program_error_dialog is not None:

            # hide dialog
            self.scene.removeItem(self.program_error_dialog)
            self.program_error_dialog = None

        # display info/warning/error if there is any - only once (on change)
        if state.error_severity != old_state.error_severity:

            if state.error_severity == InterfaceState.INFO:

                self.notif(translate("UICoreRos", "Error occurred: ") +
                           error_strings.get_error_string(state.error_code),
                           temp=True)

            elif state.error_severity == InterfaceState.WARNING:

                # TODO translate error number to error message
                self.program_error_dialog = DialogItem(
                    self.scene, self.width / 2, 0.1,
                    translate("UICoreRos", "Handle error: ") +
                    error_strings.get_error_string(state.error_code), [
                        translate("UICoreRos", "Try again"),
                        translate("UICoreRos", "Skip instruction"),
                        translate("UICoreRos", "Fail instruction"),
                        translate("UICoreRos", "End program")
                    ], self.program_error_dialog_cb)

            # TODO what to do with SEVERE?

        if state.system_state == InterfaceState.STATE_PROGRAM_FINISHED:

            if system_state_changed:

                self.notif(translate("UICoreRos", "The program is done."))

        elif state.system_state == InterfaceState.STATE_IDLE:

            if system_state_changed:

                self.show_program_list()

        elif state.system_state == InterfaceState.STATE_LEARNING:

            self.state_learning(old_state, state, flags, system_state_changed)

        elif state.system_state in [
                InterfaceState.STATE_PROGRAM_RUNNING,
                InterfaceState.STATE_PROGRAM_STOPPED
        ]:

            self.state_running(old_state, state, flags, system_state_changed)

    def interface_state_cb(self, old_state, state, flags):

        # print state
        self.emit(QtCore.SIGNAL('interface_state'), old_state, state, flags)

    def state_running(self, old_state, state, flags, system_state_changed):

        if system_state_changed:

            if not self.ph.load(self.art.load_program(state.program_id)):

                self.notif(
                    translate("UICoreRos",
                              "Failed to load program from database."))

                # TODO what to do?
                return

            stopped = state.system_state == InterfaceState.STATE_PROGRAM_STOPPED

            self.show_program_vis(readonly=True, stopped=stopped)

            if stopped:
                self.notif(translate("UICoreRos", "Program paused."),
                           temp=True)

            if not stopped and old_state.system_state == InterfaceState.STATE_PROGRAM_STOPPED:
                self.notif(translate("UICoreRos", "Program resumed."),
                           temp=True)

        # ignore not valid states
        if state.block_id == 0 or state.program_current_item.id == 0:
            rospy.logerr("Invalid state!")
            return

        # TODO if the item id is same - do rather update then clear + add everything?
        self.clear_all()

        self.program_vis.set_active(state.block_id,
                                    state.program_current_item.id)
        it = state.program_current_item

        if it.type == ProgIt.GET_READY:

            self.notif(translate("UICoreRos", "Robot is getting ready"))

        elif it.type == ProgIt.WAIT_FOR_USER:

            self.notif(translate("UICoreRos", "Waiting for user"))

        elif it.type == ProgIt.WAIT_UNTIL_USER_FINISHES:

            self.notif(translate("UICoreRos", "Waiting for user to finish"))

        elif it.type == ProgIt.PICK_FROM_POLYGON:

            obj_id = None
            try:
                obj_id = flags["SELECTED_OBJECT_ID"]
            except KeyError:
                rospy.logerr(
                    "PICK_FROM_POLYGON: SELECTED_OBJECT_ID flag not set")

            if obj_id is not None:
                self.select_object(obj_id)

                obj = self.get_object(obj_id)  # TODO notif - object type
                if obj is not None:
                    self.notif(
                        translate("UICoreRos", "Going to pick object ID ") +
                        obj_id + translate("UICoreRos", " of type ") +
                        obj.object_type.name +
                        translate("UICoreRos", " from polygon."))

            self.add_polygon(
                translate("UICoreRos", "PICK POLYGON"),
                poly_points=conversions.get_pick_polygon_points(it),
                fixed=True)

        elif it.type == ProgIt.PICK_FROM_FEEDER:

            # TODO PICK_FROM_FEEDER
            pass

        elif it.type == ProgIt.PICK_OBJECT_ID:

            self.notif(
                translate("UICoreRos", "Picking object with ID=") +
                it.object[0])
            self.select_object(it.object[0])

        elif it.type == ProgIt.PLACE_TO_POSE:

            try:
                obj_id = flags["SELECTED_OBJECT_ID"]
            except KeyError:
                rospy.logerr("PLACE_TO_POSE: SELECTED_OBJECT_ID flag not set")
                return

            obj = self.get_object(obj_id)

            if obj is not None:
                self.add_place(translate("UICoreRos", "OBJECT PLACE POSE"),
                               it.pose[0],
                               obj.object_type,
                               obj_id,
                               fixed=True)

        elif it.type == ProgIt.PLACE_TO_GRID:

            ref_msg = self.program_vis.get_ref_item(
                it.ref_id)  # obtaining reference instruction

            if ref_msg.type == ProgIt.PICK_OBJECT_ID:

                obj = self.get_object(ref_msg.object[0])
                object_type = obj.object_type
                self.program_vis.get_current_item().object[0] = obj.object_id

            else:

                object_type = self.art.get_object_type(ref_msg.object[0])
                self.program_vis.get_current_item(
                ).object[0] = ref_msg.object[0]
            self.notif(
                translate("UICoreRos", "Going to place object into grid"))
            self.add_square(
                translate("UICoreRos", "PLACE SQUARE GRID"),
                self.width / 2,
                self.height / 2,
                0.1,
                0.075,
                object_type,
                it.pose,
                grid_points=conversions.get_pick_polygon_points(it),
                square_changed=self.square_changed,
                fixed=True)

    def show_program_vis(self, readonly=False, stopped=False):

        rospy.logdebug("Showing ProgramItem with readonly=" + str(readonly) +
                       ", stopped=" + str(stopped))
        self.program_vis = ProgramItem(
            self.scene,
            self.last_prog_pos[0],
            self.last_prog_pos[1],
            self.ph,
            done_cb=self.learning_done_cb,
            item_switched_cb=self.active_item_switched,
            learning_request_cb=self.learning_request_cb,
            stopped=stopped,
            pause_cb=self.pause_cb,
            cancel_cb=self.cancel_cb)

        self.program_vis.set_readonly(readonly)

    def pause_cb(self):

        if self.state_manager.state.system_state == InterfaceState.STATE_PROGRAM_STOPPED:

            # TODO call trigger service method
            try:
                resp = self.program_resume_srv()
            except rospy.ServiceException:
                pass

            if resp is not None and resp.success:
                return True
            else:
                self.notif(translate("UICoreRos", "Failed to resume program."),
                           temp=True)
                return False

        elif self.state_manager.state.system_state == InterfaceState.STATE_PROGRAM_RUNNING:

            try:
                resp = self.program_pause_srv()
            except rospy.ServiceException:
                pass

            if resp is not None and resp.success:
                self.notif(translate("UICoreRos", "Program paused."),
                           temp=True)
                return True

            else:

                self.notif(translate("UICoreRos", "Failed to pause program."),
                           temp=True)
                return True

        else:

            rospy.logdebug(
                "Attempt to pause/resume program in strange state: " +
                str(self.state_manager.state.system_state))
            return False

    def cancel_cb(self):

        if self.state_manager.state.system_state in [
                InterfaceState.STATE_PROGRAM_RUNNING,
                InterfaceState.STATE_PROGRAM_STOPPED
        ]:

            try:
                resp = self.program_stop_srv()
            except rospy.ServiceException:
                pass

            if resp is not None and resp.success:
                self.notif(translate("UICoreRos", "Program stopped."),
                           temp=True)
                return True

            else:

                self.notif(translate("UICoreRos", "Failed to stop program."),
                           temp=True)
                return True

        else:

            rospy.logdebug("Attempt to stop program in strange state: " +
                           str(self.state_manager.state.system_state))
            return False

    def clear_all(self, include_dialogs=False):

        rospy.logdebug("Clear all")

        super(UICoreRos, self).clear_all()

        if include_dialogs:

            for it in [self.program_list, self.program_vis]:

                if it is None:
                    continue
                try:
                    self.last_prog_pos = it.get_pos()
                except AttributeError:
                    pass
                break

            for it in [
                    self.program_error_dialog, self.grasp_dialog,
                    self.program_vis, self.program_list
            ]:

                if it is None:
                    continue
                self.remove_scene_items_by_type(type(it))
                it = None

    def state_learning(self, old_state, state, flags, system_state_changed):

        if system_state_changed:

            self.last_edited_prog_id = state.program_id

            if not self.ph.load(self.art.load_program(state.program_id)):

                self.notif(
                    translate("UICoreRos",
                              "Failed to load program from database."))

                # TODO what to do?
                return

            if state.block_id != 0 and state.program_current_item.id != 0:

                # there may be unsaved changes - let's use ProgramItem from brain
                self.ph.set_item_msg(state.block_id,
                                     state.program_current_item)

            self.show_program_vis()

        if state.block_id == 0 or state.program_current_item.id == 0:
            rospy.logerr("Invalid state!")
            return

        if old_state.block_id != state.block_id or old_state.program_current_item.id != state.program_current_item.id:
            self.clear_all()

        # TODO overit funkcnost - pokud ma state novejsi timestamp nez nas - ulozit ProgramItem
        if old_state.timestamp == rospy.Time(
                0
        ) or old_state.timestamp - state.timestamp > rospy.Duration(0):

            rospy.logdebug('Got state with newer timestamp!')
            item = self.ph.get_item_msg(state.block_id,
                                        state.program_current_item.id)
            item = state.program_current_item
            self.clear_all()

            self.learning_vis(state.block_id, state.program_current_item.id,
                              not state.edit_enabled)

    def learning_vis(self, block_id, item_id, read_only):

        if not self.ph.item_requires_learning(block_id, item_id):
            self.notif(translate("UICoreRos", "Item has no parameters."))
            return

        self.program_vis.editing_item = not read_only

        # TODO Edit/Done button not visible when there is work in progress!
        if block_id != self.program_vis.block_id or item_id != self.program_vis.item_id:
            self.program_vis.set_active(block_id, item_id)

        msg = self.ph.get_item_msg(block_id, item_id)

        if self.ph.item_learned(block_id, item_id):

            self.notif(
                translate("UICoreRos", "This program item seems to be done"))

        else:

            if msg.type in [
                    ProgIt.PICK_FROM_POLYGON, ProgIt.PICK_FROM_FEEDER,
                    ProgIt.PICK_OBJECT_ID, ProgIt.PLACE_TO_POSE
            ]:
                self.notif(
                    translate("UICoreRos",
                              "Program current manipulation task"))

        if msg.type == ProgIt.PICK_FROM_POLYGON:

            if not self.ph.is_object_set(block_id, item_id):

                self.notif(translate("UICoreRos",
                                     "Select object type to be picked up"),
                           temp=True)

            else:

                self.select_object_type(msg.object[0])

            if self.ph.is_polygon_set(block_id, item_id):
                self.add_polygon(
                    translate("UICoreRos", "PICK POLYGON"),
                    poly_points=conversions.get_pick_polygon_points(msg),
                    polygon_changed=self.polygon_changed,
                    fixed=read_only)

        elif msg.type == ProgIt.PICK_FROM_FEEDER:

            if self.state_manager.state.edit_enabled and self.grasp_dialog is None:
                self.grasp_dialog = DialogItem(self.scene, self.width / 2, 0.1,
                                               "Save gripper pose",
                                               ["Right arm", "Left arm"],
                                               self.save_gripper_pose_cb)

            if self.ph.is_object_set(block_id, item_id):
                self.select_object_type(msg.object[0])
            else:
                self.notif(translate("UICoreRos",
                                     "Select object type to be picked up"),
                           temp=True)

                # TODO show pick pose somehow (arrow??)

        elif msg.type == ProgIt.PICK_OBJECT_ID:
            if self.ph.is_object_set(block_id, item_id):
                self.select_object(msg.object[0])
            else:
                self.notif(translate("UICoreRos",
                                     "Select object to be picked up"),
                           temp=True)

        elif msg.type == ProgIt.PLACE_TO_POSE:

            if not self.ph.is_object_set(block_id, msg.ref_id[0]):

                self.notif(
                    translate("UICoreRos",
                              "Select object to be picked up in ID=") +
                    str(msg.ref_id[0]))

            else:

                # TODO what to do with more than 1 reference?
                ref_msg = self.ph.get_item_msg(block_id, msg.ref_id[0])

                if ref_msg.type == ProgIt.PICK_OBJECT_ID:

                    obj = self.get_object(ref_msg.object[0])
                    object_type = obj.object_type
                    object_id = obj.object_id
                    self.select_object(ref_msg.object[0])

                else:

                    object_type = self.art.get_object_type(ref_msg.object[0])
                    object_id = None
                    self.select_object_type(ref_msg.object[0])

                if self.ph.is_object_set(block_id, msg.ref_id[0]):

                    if self.ph.is_pose_set(block_id, item_id):

                        if object_type is not None:
                            self.add_place(translate("UICoreRos",
                                                     "OBJECT PLACE POSE"),
                                           msg.pose[0],
                                           object_type,
                                           object_id,
                                           place_cb=self.place_pose_changed,
                                           fixed=read_only)
                    else:
                        self.notif(
                            translate("UICoreRos",
                                      "Set where to place picked object"))
                        self.add_place(translate("UICoreRos",
                                                 "OBJECT PLACE POSE"),
                                       self.get_def_pose(),
                                       object_type,
                                       object_id,
                                       place_cb=self.place_pose_changed,
                                       fixed=read_only)

        elif msg.type == ProgIt.PLACE_TO_GRID:

            ref_msg = self.program_vis.get_ref_item(
                msg.ref_id)  # obtaining reference instruction

            if ref_msg.type == ProgIt.PICK_OBJECT_ID:

                obj = self.get_object(ref_msg.object[0])
                object_type = obj.object_type
                self.program_vis.get_current_item().object[0] = obj.object_id

            else:

                object_type = self.art.get_object_type(ref_msg.object[0])
                self.program_vis.get_current_item(
                ).object[0] = ref_msg.object[0]
            self.notif(translate("UICoreRos", "Place grid"))
            self.add_square(
                translate("UICoreRos", "PLACE SQUARE GRID"),
                self.width / 2,
                self.height / 2,
                0.1,
                0.075,
                object_type,
                msg.pose,
                grid_points=conversions.get_pick_polygon_points(msg),
                square_changed=self.square_changed,
                fixed=read_only)

    def active_item_switched(self, block_id, item_id, read_only=True):

        rospy.logdebug("Program ID:" + str(self.ph.get_program_id()) +
                       ", active item ID: " + str((block_id, item_id)))

        self.clear_all()

        if item_id is None:
            # TODO hlaska
            return

        self.learning_vis(block_id, item_id, read_only)

        self.state_manager.update_program_item(
            self.ph.get_program_id(), block_id,
            self.ph.get_item_msg(block_id, item_id))

    def get_def_pose(self):

        ps = PoseStamped()
        ps.pose.position.x = self.width / 2
        ps.pose.position.y = self.height / 2
        ps.pose.orientation.w = 1.0
        return ps

    def place_pose_changed(self, place):

        if self.program_vis.editing_item:

            self.program_vis.set_place_pose(place)
            self.state_manager.update_program_item(
                self.ph.get_program_id(), self.program_vis.block_id,
                self.program_vis.get_current_item())

    def calib_done_cb(self, proj):

        if proj.is_calibrated():

            self.calib_proj_cnt += 1

            while self.calib_proj_cnt < len(self.projectors):

                if self.projectors[self.calib_proj_cnt].is_calibrated():
                    self.calib_proj_cnt += 1
                    continue

                self.projectors[self.calib_proj_cnt].calibrate(
                    self.calib_done_cb)
                return

            rospy.loginfo('Projectors calibrated.')
            self.projectors_calibrated_pub.publish(True)

        else:

            # calibration failed - let's try again
            rospy.logerr('Calibration failed for projector: ' + proj.proj_id)
            proj.calibrate(self.calib_done_cb)

    def start_projector_calibration(self, evt):

        if len(self.projectors) == 0:

            rospy.loginfo('No projectors to calibrate.')
            self.projectors_calibrated_pub.publish(True)

        else:

            self.projectors_calibrated_pub.publish(False)
            rospy.loginfo('Starting calibration of ' +
                          str(len(self.projectors)) + ' projector(s)')

            self.calib_proj_cnt = 0

            for proj in self.projectors:

                if proj.is_calibrated():

                    self.calib_proj_cnt += 1
                    continue

                else:

                    if not proj.calibrate(self.calib_done_cb):
                        # TODO what to do?
                        rospy.logerr("Failed to start projector calibration")

                    return

            rospy.loginfo('Projectors calibrated.')
            self.projectors_calibrated_pub.publish(True)

    def is_template(self):

        return self.template

    def learning_done_cb(self):

        prog = self.ph.get_program()

        # if it is template - save it with new id
        if self.is_template():

            self.template = False

            headers = self.art.get_program_headers()
            ids = []

            for h in headers:
                ids.append(h.id)

            # is there a better way how to find not used ID for program?
            for i in range(0, 2**16 - 1):
                if i not in ids:
                    prog.header.id = i
                    break
            else:
                rospy.logerr("Failed to find available program ID")

        if not self.art.store_program(prog):

            self.notif(translate("UICoreRos", "Failed to store program"),
                       temp=True)
            # TODO what to do?

        self.notif(translate("UICoreRos", "Program stored with ID=") +
                   str(prog.header.id),
                   temp=True)

        self.last_edited_prog_id = prog.header.id

        resp = None
        try:
            resp = self.stop_learning_srv()
        except rospy.ServiceException as e:
            print "Service call failed: %s" % e

        if resp is None or not resp.success:

            rospy.logwarn("Failed to stop learning mode.")
            return

    def program_selected_cb(self, prog_id, run=False, template=False):

        self.template = template

        if run:

            (started, error) = self.art.start_program(prog_id)

            if not started:
                self.notif(translate("UICoreRos", "Failed to start program."))
                rospy.logerr("Brain refused to start program: " + error)
                return

            self.notif(translate("UICoreRos",
                                 "Starting program ID=" + str(prog_id)),
                       temp=True)
            self.program_list.set_enabled(False)

        else:

            if not self.ph.load(self.art.load_program(prog_id), template):

                self.notif(
                    translate("UICoreRos",
                              "Failed to load program from database."))

                # TODO what to do?
                return

            req = startProgramRequest()
            req.program_id = prog_id
            resp = None
            try:
                resp = self.start_learning_srv(req)
            except rospy.ServiceException as e:
                print "Service call failed: %s" % e

            if resp is None or not resp.success:

                self.notif(translate("UICoreRos",
                                     "Failed to start edit mode."))

            # else:
            # self.clear_all()
            # self.show_program_vis()

    def learning_request_cb(self, req):

        if req == LearningRequestGoal.GET_READY:
            self.notif(
                translate("UICoreRos", "Robot is getting ready for learning"))
        elif req == LearningRequestGoal.DONE:

            self.notif(
                translate("UICoreRos", "Robot is getting into default state"))

            if self.grasp_dialog is not None:

                self.scene.removeItem(self.grasp_dialog)
                self.grasp_dialog = None

        elif req == LearningRequestGoal.EXECUTE_ITEM:
            self.notif(
                translate("UICoreRos",
                          "Robot is executing current program instruction"))

        g = LearningRequestGoal()
        g.request = req

        self.learning_action_cl.send_goal(
            g,
            done_cb=self.learning_request_done_cb,
            feedback_cb=self.learning_request_feedback_cb)

    def learning_request_feedback_cb(self, fb):

        rospy.logdebug('learning request progress: ' + str(fb.progress))

    def learning_request_done_evt(self, status, result):

        self.program_vis.learning_request_result(result.success)

    def learning_request_done_cb(self, status, result):

        self.emit(QtCore.SIGNAL('learning_request_done_evt'), status, result)

    def show_program_list(self):

        self.notif(translate("UICoreRos", "Please select a program"))

        headers = self.art.get_program_headers()

        d = {}

        headers_to_show = []

        for header in headers:

            ph = ProgramHelper()
            d[header.id] = None

            if ph.load(self.art.load_program(header.id)):

                headers_to_show.append(header)
                d[header.id] = ph.program_learned()

        self.program_list = ProgramListItem(self.scene, self.last_prog_pos[0],
                                            self.last_prog_pos[1],
                                            headers_to_show, d,
                                            self.last_edited_prog_id,
                                            self.program_selected_cb)

    def object_cb(self, msg):

        self.emit(QtCore.SIGNAL('objects'), msg)

    def object_cb_evt(self, msg):

        for obj_id in msg.lost_objects:

            self.remove_object(obj_id)
            self.notif(translate("UICoreRos", "Object") + " ID=" +
                       str(obj_id) + " " +
                       translate("UICoreRos", "disappeared"),
                       temp=True)

        for inst in msg.instances:

            obj = self.get_object(inst.object_id)

            if obj:
                obj.set_pos(inst.pose.position.x, inst.pose.position.y,
                            inst.pose.position.z)
                obj.set_orientation(conversions.q2a(inst.pose.orientation))
            else:

                obj_type = self.art.get_object_type(inst.object_type)
                self.add_object(inst.object_id, obj_type, inst.pose.position.x,
                                inst.pose.position.y, inst.pose.position.z,
                                conversions.q2a(inst.pose.orientation),
                                self.object_selected)
                self.notif(translate("UICoreRos", "New object") + " ID=" +
                           str(inst.object_id),
                           temp=True)

    def polygon_changed(self, pts):

        if self.program_vis.editing_item:

            self.program_vis.set_polygon(pts)
            self.state_manager.update_program_item(
                self.ph.get_program_id(), self.program_vis.block_id,
                self.program_vis.get_current_item())

    '''
        Method which saves grid points and place poses of all objects in grid.
    '''

    def square_changed(self, pts, poses=None):

        self.program_vis.set_place_grid(
            pts)  # saving grid points into the ProgramItem message
        self.program_vis.set_place_poses(
            poses)  # saving place poses into the ProgramItem message
        self.state_manager.update_program_item(
            self.ph.get_program_id(), self.program_vis.block_id,
            self.program_vis.get_current_item())

    def object_selected(self, id, selected):

        if self.program_vis is None or not self.program_vis.editing_item:
            rospy.logdebug("not in edit mode")
            return False

        rospy.logdebug("attempt to select object id: " + id)
        obj = self.get_object(id)

        msg = self.program_vis.get_current_item()

        if msg is None:
            return False

        if msg.type in [ProgIt.PICK_FROM_FEEDER, ProgIt.PICK_FROM_POLYGON]:

            # this type of object is already set
            if len(msg.object) > 0 and obj.object_type.name == msg.object[0]:
                rospy.logdebug("object type " + obj.object_type.name +
                               " already selected")
                return
            else:
                # TODO remove previously inserted polygon, do not insert new
                # place
                rospy.logdebug("selecting new object type: " +
                               obj.object_type.name)
                pass

        if msg.type == ProgIt.PICK_FROM_FEEDER:

            self.program_vis.set_object(obj.object_type.name)
            self.select_object_type(obj.object_type.name)

        elif msg.type == ProgIt.PICK_OBJECT_ID:

            self.program_vis.set_object(obj.object_id)
            self.select_object(obj.object_id)

        elif msg.type == ProgIt.PICK_FROM_POLYGON:

            if obj.object_type.name not in self.selected_object_types:

                self.remove_scene_items_by_type(PolygonItem)

                poly_points = []

                self.program_vis.set_object(obj.object_type.name)
                self.select_object_type(obj.object_type.name)

                for ob in self.get_scene_items_by_type(ObjectItem):
                    if ob.object_type.name != obj.object_type.name:
                        continue
                    poly_points.append(ob.get_pos())

                self.add_polygon(translate("UICoreRos", "PICK POLYGON"),
                                 poly_points,
                                 polygon_changed=self.polygon_changed)
                self.notif(translate("UICoreRos",
                                     "Check and adjust pick polygon"),
                           temp=True)

        self.state_manager.update_program_item(
            self.ph.get_program_id(), self.program_vis.block_id,
            self.program_vis.get_current_item())
        return True

    def user_status_cb(self, msg):

        self.emit(QtCore.SIGNAL('user_status'), msg)

    def user_status_cb_evt(self, msg):

        if msg.user_state != self.user_state:

            if msg.user_state == UserStatus.USER_NOT_CALIBRATED:

                self.notif(
                    translate("UICoreRos", "Please do a calibration pose"))

            elif msg.user_state == UserStatus.USER_CALIBRATED:

                self.notif(translate("UICoreRos", "Successfully calibrated"))

            elif msg.user_state == UserStatus.NO_USER:

                self.notif(translate("UICoreRos", "Waiting for user..."))

        self.user_state = msg
Example #4
0
def main(args):

    if len(args) < 2:

        print "This script needs program id as argument."
        return

    rospy.init_node('plot_program', anonymous=True)

    art = ArtApiHelper()
    ph = ProgramHelper()

    prog = art.load_program(int(args[1]))

    if prog is None:

        print "Failed to load program"
        return

    if not ph.load(prog):

        print "Faulty program"
        return

    A = pgv.AGraph(label="<<B>Program ID: " + str(prog.header.id) + "<br/>" +
                   escape(prog.header.name) + "</B>>",
                   directed=True,
                   strict=False)
    A.graph_attr['outputorder'] = 'edgesfirst'

    A.add_node("start", label="Program start")
    A.add_node("b0i0", label="Program end")

    A.add_edge("start", get_node_name(*ph.get_first_item_id()))

    for block_id in ph.get_block_ids():

        block_msg = ph.get_block_msg(block_id)
        ids = []

        for item_id in ph.get_items_ids(block_id):

            nn = get_node_name(block_id, item_id)
            item_msg = ph.get_item_msg(block_id, item_id)

            osn = get_node_name(*ph.get_id_on_success(block_id, item_id))
            ofn = get_node_name(*ph.get_id_on_failure(block_id, item_id))

            A.add_edge(nn, osn, color="green", constraint=True)
            A.add_edge(nn, ofn, color="red", constraint=True)

            for ref in item_msg.ref_id:

                A.add_edge(get_node_name(block_id, ref),
                           nn,
                           color="gray",
                           style="dashed",
                           key="ref_" + nn + "_" +
                           get_node_name(block_id, ref))

            A.get_node(nn).attr['label'] = 'Item ID: ' + str(
                item_id) + '\n' + get_type_string(item_msg)
            A.get_node(nn).attr['shape'] = 'box'
            A.get_node(nn).attr['style'] = 'rounded'
            ids.append(nn)

        sg = A.subgraph(name="cluster_" + str(block_id),
                        label="<<b>Group ID: " + str(block_id) + "<br/>" +
                        escape(block_msg.name) + "</b>>",
                        color="gray")
        sg.add_nodes_from(ids)

    A.layout(prog='dot')
    A.draw('output.pdf')
Example #5
0
class UICoreRos(UICore):

    """The class builds on top of UICore and adds ROS-related stuff and application logic.

    Attributes:
        user_status (UserStatus): current user tracking status
        fsm (FSM): state machine maintaining current state of the interface and proper transitions between states
        state_manager (interface_state_manager): synchronization of interfaces within the ARTable system
        scene_pub (rospy.Publisher): publisher for scene images
        last_scene_update (rospy.Time): time when the last scene image was published
        scene_img_deq (Queue.Queue): thread-safe queue for scene images (which are published in separate thread)
        projectors (list): array of ProjectorHelper instances
        art (ArtApiHelper): easy access to ARTable services

    """

    def __init__(self):

        origin = rospy.get_param("~scene_origin", [0, 0])
        size = rospy.get_param("~scene_size", [1.2, 0.75])
        rpm = rospy.get_param("~rpm", 1280)

        super(UICoreRos, self).__init__(origin[0], origin[1], size[0], size[1], rpm)

        QtCore.QObject.connect(self, QtCore.SIGNAL('objects'), self.object_cb_evt)
        QtCore.QObject.connect(self, QtCore.SIGNAL('user_status'), self.user_status_cb_evt)
        QtCore.QObject.connect(self, QtCore.SIGNAL('interface_state'), self.interface_state_evt)
        QtCore.QObject.connect(self, QtCore.SIGNAL('send_scene'), self.send_to_clients_evt)

        QtCore.QObject.connect(self, QtCore.SIGNAL('touch_calibration_points_evt'), self.touch_calibration_points_evt)
        QtCore.QObject.connect(self, QtCore.SIGNAL('touch_detected_evt'), self.touch_detected_evt)

        self.user_status = None

        self.program_vis.active_item_switched = self.active_item_switched
        self.program_vis.program_state_changed = self.program_state_changed

        self.fsm = FSM()

        # TODO do this automatically??
        # map callbacks from FSM to this instance
        self.fsm.cb_start_calibration = self.cb_start_calibration
        self.fsm.cb_waiting_for_user = self.cb_waiting_for_user
        self.fsm.cb_program_selection = self.cb_program_selection
        self.fsm.cb_waiting_for_user_calibration = self.cb_waiting_for_user_calibration
        self.fsm.cb_learning = self.cb_learning
        self.fsm.cb_running = self.cb_running
        self.fsm.is_template = self.is_template

        self.state_manager = InterfaceStateManager("PROJECTED UI", cb=self.interface_state_cb)

        cursors = rospy.get_param("~cursors", [])
        for cur in cursors:
            self.scene_items.append(PoseStampedCursorItem(self.scene, self.rpm, cur))

        self.scene_items.append(TouchTableItem(self.scene,  self.rpm, '/art/interface/touchtable/touch', self.get_scene_items_by_type(PoseStampedCursorItem)))

        self.scene_items.append(ButtonItem(self.scene, self.rpm, 0, 0, "STOP", None, self.stop_btn_clicked, 2.0, QtCore.Qt.red))
        self.scene_items[-1].setPos(self.scene.width() - self.scene_items[-1].w, self.scene.height() - self.scene_items[-1].h - 60)
        self.scene_items[-1].set_enabled(True)

        self.port = rospy.get_param("~scene_server_port", 1234)

        self.tcpServer = QtNetwork.QTcpServer(self)
        if not self.tcpServer.listen(port=self.port):

            rospy.logerr('Failed to start scene TCP server on port ' + str(self.port))

        self.tcpServer.newConnection.connect(self.newConnection)
        self.connections = []

        self.last_scene_update = None
        self.scene.changed.connect(self.scene_changed)

        self.projectors = []

        projs = rospy.get_param("~projectors", [])
        for proj in projs:
            self.add_projector(proj)

        self.art = ArtApiHelper()

    def touch_calibration_points_evt(self,  pts):

        # TODO trigger state change?
        for it in self.scene_items:

            if isinstance(it, LabelItem):
                continue

            it.set_enabled(False, True)

        self.notif(translate("UICoreRos", "Touch table calibration started. Please press the white point."), temp=False)
        self.touch_points = TouchPointsItem(self.scene, self.rpm,  pts)

    def touch_calibration_points_cb(self,  req):

        resp = TouchCalibrationPointsResponse()

        if self.fsm.state not in ['program_selection', 'learning', 'running']:
            resp.success = False
            rospy.logerr('Cannot start touchtable calibration without a user!')
            return resp

        pts = []

        for pt in req.points:

            pts.append((pt.point.x,  pt.point.y))

        self.emit(QtCore.SIGNAL('touch_calibration_points_evt'), pts)
        self.touched_sub = rospy.Subscriber('/art/interface/touchtable/touch_detected',  Empty,  self.touch_detected_cb,  queue_size=10)
        resp.success = True
        return resp

    def touch_detected_evt(self,  msg):

        if self.touch_points is None:
            return

        if not self.touch_points.next():

            self.notif(translate("UICoreRos", "Touch saved."), temp=True)

            for it in self.scene_items:

                if isinstance(it, LabelItem):
                    continue

                it.set_enabled(True, True)

            self.notif(translate("UICoreRos", "Touch table calibration finished."), temp=False)
            self.scene.removeItem(self.touch_points)
            self.touch_points = None
            self.touched_sub.unregister()

        else:

            self.notif(translate("UICoreRos", "Touch saved."), temp=True)
            self.notif(translate("UICoreRos", "Please press the next point."), temp=False)

    def touch_detected_cb(self,  msg):

        self.emit(QtCore.SIGNAL('touch_detected_evt'), msg)

    def newConnection(self):

        rospy.loginfo('Some projector node just connected.')
        self.connections.append(self.tcpServer.nextPendingConnection())
        self.connections[-1].setSocketOption(QtNetwork.QAbstractSocket.LowDelayOption, 1)
        self.emit(QtCore.SIGNAL('send_scene'), self.connections[-1])
        # TODO deal with disconnected clients!
        # self.connections[-1].disconnected.connect(clientConnection.deleteLater)

    def send_to_clients_evt(self, client=None):

        # if all connections are sending scene image, there is no need to render the new one
        if client is None:

            for con in self.connections:

                if con.bytesToWrite() == 0:
                    break

            else:
                return

        # TODO try to use Format_RGB16 - BMP is anyway converted to 32bits (send raw data instead)
        pix = QtGui.QImage(self.scene.width(), self.scene.height(), QtGui.QImage.Format_ARGB32_Premultiplied)
        painter = QtGui.QPainter(pix)
        self.scene.render(painter)
        painter.end()

        block = QtCore.QByteArray()
        out = QtCore.QDataStream(block, QtCore.QIODevice.WriteOnly)
        out.setVersion(QtCore.QDataStream.Qt_4_0)
        out.writeUInt32(0)

        img = QtCore.QByteArray()
        buffer = QtCore.QBuffer(img)
        buffer.open(QtCore.QIODevice.WriteOnly)
        pix.save(buffer, "BMP")
        out << QtCore.qCompress(img, 1)  # this seem to be much faster than using PNG compression

        out.device().seek(0)
        out.writeUInt32(block.size() - 4)

        # print block.size()

        if client is None:

            for con in self.connections:

                if con.bytesToWrite() > 0:
                    return
                con.write(block)

        else:

            client.write(block)

    def start(self):

        rospy.loginfo("Waiting for ART services...")
        self.art.wait_for_api()

        if len(self.projectors) > 0:
            rospy.loginfo("Waiting for projector nodes...")
            for proj in self.projectors:
                proj.wait_until_available()

        rospy.loginfo("Ready! Starting state machine.")

        # TODO move this to ArtApiHelper ??
        self.obj_sub = rospy.Subscriber('/art/object_detector/object_filtered', InstancesArray, self.object_cb, queue_size=1)
        self.user_status_sub = rospy.Subscriber('/art/user/status', UserStatus, self.user_status_cb, queue_size=1)

        self.touch_points = None
        self.touch_calib_srv = rospy.Service('/art/interface/projected_gui/touch_calibration', TouchCalibrationPoints, self.touch_calibration_points_cb)

        self.fsm.tr_start()

    def add_projector(self, proj_id):

        self.projectors.append(ProjectorHelper(proj_id))

    def scene_changed(self, rects):

        if len(rects) == 0:
            return
        # TODO Publish only changes? How to accumulate them (to be able to send it only at certain fps)?

        now = rospy.Time.now()
        if self.last_scene_update is None:
            self.last_scene_update = now
        else:
            if now - self.last_scene_update < rospy.Duration(1.0 / 20):
                return

        # print 1.0/(now - self.last_scene_update).to_sec()
        self.last_scene_update = now

        self.emit(QtCore.SIGNAL('send_scene'))

    def stop_btn_clicked(self):

        # TODO
        self.notif(translate("UICoreRos", "Emergency stop pressed"), temp=True)

    def interface_state_evt(self, our_state, state, flags):

        if state.system_state == InterfaceState.STATE_LEARNING:

            # TODO !!
            pass

        elif state.system_state == InterfaceState.STATE_PROGRAM_RUNNING:

            if self.program_vis.prog is None or self.program_vis.prog.id != state.program_id:

                program = self.art.load_program(state.program_id)
                if program is not None:
                    self.program_vis.set_prog(program, False)
                else:
                    pass  # TODO error

            if self.fsm.state != 'running':

                self.clear_all()
                self.program_vis.set_running()
                self.fsm.tr_running()

            # TODO updatovat program_vis itemem ze zpravy - pokud se lisi (timestamp) ??
            self.program_vis.set_active(inst_id=state.program_current_item.id)
            it = state.program_current_item

            if our_state.program_current_item.id != state.program_current_item:
                self.clear_all()

            if it.type == ProgIt.GET_READY:

                self.notif(translate("UICoreRos", "Robot is getting ready"))

            elif it.type == ProgIt.WAIT:

                if it.spec == ProgIt.WAIT_FOR_USER:
                    self.notif(translate("UICoreRos", "Waiting for user"))
                elif it.spec == ProgIt.WAIT_UNTIL_USER_FINISHES:
                    self.notif(translate("UICoreRos", "Waiting for user to finish"))

            # TODO MANIP_PICK, MANIP_PLACE
            elif it.type == ProgIt.MANIP_PICK_PLACE:

                obj_id = it.object

                if it.spec == ProgIt.MANIP_ID:

                    self.select_object(it.object)

                elif it.spec == ProgIt.MANIP_TYPE:

                    try:
                        obj_id = flags["SELECTED_OBJECT_ID"]
                    except KeyError:
                        rospy.logerr("MANIP_PICK_PLACE/MANIP_TYPE: SELECTED_OBJECT_ID flag not set")
                        pass

                    # TODO how to highlight selected object (by brain) ?
                    self.select_object_type(it.object)
                    self.add_polygon(translate("UICoreRos", "PICK POLYGON"), poly_points=self.program_vis.active_item.get_pick_polygon_points())  # TODO fixed

                self.notif(translate("UICoreRos", "Going to manipulate with object ID=") + obj_id)
                self.add_place(translate("UICoreRos", "PLACE POSE"), it.place_pose.pose.position.x, it.place_pose.pose.position.y, fixed=True)

    def interface_state_cb(self, our_state, state, flags):

        self.emit(QtCore.SIGNAL('interface_state'), our_state, state, flags)

    # callback from ProgramItem (button press)
    def program_state_changed(self, state):

        if state == 'RUNNING':

            prog = self.program_vis.get_prog()
            prog.id = 1

            if not self.art.store_program(prog):
                # TODO what to do?
                self.notif(translate("UICoreRos", "Failed to store program"), temp=True)

            else:

                self.notif(translate("UICoreRos", "Program stored. Starting..."), temp=True)

            # clear all and wait for state update from brain
            self.clear_all()
            self.fsm.tr_program_learned()

            self.art.start_program(prog.id)

        # TODO pause / stop -> fsm
        # elif state == ''

        # callback from ProgramItem
    def active_item_switched(self):

        rospy.logdebug("Program ID:" + str(self.program_vis.prog.id) + ", active item ID: " + str(self.program_vis.active_item.item.id))

        self.clear_all()
        # TODO block_id
        self.state_manager.update_program_item(self.program_vis.prog.id, self.program_vis.prog.blocks[0].id,  self.program_vis.active_item.item)

        if self.program_vis.active_item.item.type in [ProgIt.MANIP_PICK, ProgIt.MANIP_PLACE, ProgIt.MANIP_PICK_PLACE]:

            if self.program_vis.active_item.item_learned():

                self.notif(translate("UICoreRos", "This program item seems to be done"))

            else:

                # TODO vypsat jaky je to task?
                self.notif(translate("UICoreRos", "Program current manipulation task"))

            # TODO loop across program item ids - not indices!!
            idx = self.program_vis.items.index(self.program_vis.active_item)
            if idx > 0:
                for i in range(idx - 1, -1, -1):

                    it = self.program_vis.items[i]

                    if it.item.type in [ProgIt.MANIP_PLACE, ProgIt.MANIP_PICK_PLACE] and it.is_place_pose_set():

                        # TODO add "artif." object instead of place??
                        self.add_place(translate("UICoreRos", "OBJECT FROM STEP") + " " + str(it.item.id), it.item.place_pose.pose.position.x, it.item.place_pose.pose.position.y, fixed=True)
                        break

            if self.program_vis.active_item.item.spec == ProgIt.MANIP_TYPE:

                if not self.program_vis.active_item.is_object_set():

                    self.notif(translate("UICoreRos", "Select object type to be picked up"), temp=True)

                self.select_object_type(self.program_vis.active_item.item.object)

                # if program item already contains polygon - let's display it
                if self.program_vis.active_item.is_pick_polygon_set():

                    self.add_polygon(translate("UICoreRos", "PICK POLYGON"), poly_points=self.program_vis.active_item.get_pick_polygon_points(), polygon_changed=self.polygon_changed, fixed=True)

            else:

                self.notif(translate("UICoreRos", "Select object to be picked up"), temp=True)
                self.select_object(self.program_vis.active_item.item.object)

            if self.program_vis.active_item.is_object_set():

                # TODO kdy misto place pose pouzi place polygon? umoznit zmenit pose na polygon a opacne?

                if self.program_vis.active_item.is_place_pose_set():
                    (x, y) = self.program_vis.active_item.get_place_pose()
                    self.add_place(translate("UICoreRos", "PLACE POSE"), x, y, self.place_pose_changed)
                else:
                    self.notif(translate("UICoreRos", "Set where to place picked object"))
                    self.add_place(translate("UICoreRos", "PLACE POSE"), self.width / 2, self.height / 2, self.place_pose_changed)

    def place_pose_changed(self, pos):

        self.program_vis.set_place_pose(pos[0], pos[1])
        # TODO block_id
        self.state_manager.update_program_item(self.program_vis.prog.id, self.program_vis.prog.blocks[0].id,  self.program_vis.active_item.item)

    def is_template(self):

        return True

    def cb_running(self):

        pass

    def cb_learning(self):

        # TODO zobrazit instrukce k tasku
        pass

    def calib_done_cb(self, proj):

        if proj.is_calibrated():

            self.calib_proj_cnt += 1

            if self.calib_proj_cnt < len(self.projectors):

                self.projectors[self.calib_proj_cnt].calibrate(self.calib_done_cb)
            else:
                rospy.loginfo('Projectors calibrated.')
                self.fsm.tr_calibrated()

        else:

            # calibration failed - let's try again
            rospy.logerr('Calibration failed for projector: ' + proj.proj_id)
            proj.calibrate(self.calib_done_cb)

    def cb_start_calibration(self):

        if len(self.projectors) == 0:

            rospy.loginfo('No projectors to calibrate.')
            self.fsm.tr_calibrated()

        else:

            rospy.loginfo('Starting calibration of ' + str(len(self.projectors)) + ' projector(s)')

            self.calib_proj_cnt = 0

            if not self.projectors[0].calibrate(self.calib_done_cb):
                # TODO what to do?
                rospy.logerr("Failed to start projector calibration")

    def cb_waiting_for_user(self):

        self.notif(translate("UICoreRos", "Waiting for user..."))

    def cb_waiting_for_user_calibration(self):

        self.notif(translate("UICoreRos", "Please do a calibration pose"))

    def cb_program_selection(self):

        self.notif(translate("UICoreRos", "Please select a program"))

        # TODO display list of programs -> let user select -> then load it
        program = self.art.load_program(0)

        if program is not None:

            self.program_vis.set_prog(program, self.is_template())
            self.active_item_switched()
            self.fsm.tr_program_selected()
        else:
            self.notif(translate("UICoreRos", "Loading of requested program failed"), temp=True)

    def object_cb(self, msg):

        self.emit(QtCore.SIGNAL('objects'), msg)

    def object_cb_evt(self, msg):

        for obj_id in msg.lost_objects:

            self.remove_object(obj_id)
            self.notif(translate("UICoreRos", "Object") + " ID=" + str(obj_id) + " " + translate("UICoreRos", "disappeared"), temp=True)

        for inst in msg.instances:

            obj = self.get_object(inst.object_id)

            if obj:
                obj.set_pos(inst.pose.position.x, inst.pose.position.y)
            else:

                # TODO get and display bounding box
                # obj_type = self.art.get_object_type(inst.object_type)
                self.add_object(inst.object_id, inst.object_type, inst.pose.position.x, inst.pose.position.y, self.object_selected)
                self.notif(translate("UICoreRos", "New object") + " ID=" + str(inst.object_id), temp=True)

    def polygon_changed(self, pts):

        self.program_vis.set_polygon(pts)
        # TODO block_id
        self.state_manager.update_program_item(self.program_vis.prog.id, self.program_vis.prog.blocks[0].id, self.program_vis.active_item.item)

    def object_selected(self, id, selected):

        if self.fsm.state != 'learning':
            return False
        # TODO handle un-selected

        print "selected object: " + str(id)

        obj = self.get_object(id)

        # TODO test typu operace

        if self.program_vis.active_item.item.spec == ProgIt.MANIP_TYPE:

            # this type of object is already set
            if obj.object_type == self.program_vis.active_item.item.object:
                return
            else:
                # TODO remove previously inserted polygon, do not insert new place
                pass

            poly_points = []

            self.program_vis.set_object(obj.object_type)
            self.select_object_type(obj.object_type)

            for obj in self.get_scene_items_by_type(ObjectItem):
                poly_points.append(obj.get_pos())

            self.add_polygon(translate("UICoreRos", "PICK POLYGON"), poly_points, polygon_changed=self.polygon_changed)
            self.notif(translate("UICoreRos", "Check and adjust pick polygon"), temp=True)

            self.notif(translate("UICoreRos", "Set where to place picked object"), temp=True)
            self.add_place(translate("UICoreRos", "PLACE POSE"), self.width / 2, self.height / 2, self.place_pose_changed)

        elif self.program_vis.active_item.item.spec == ProgIt.MANIP_ID:

            # TODO
            pass

        # TODO block_id
        self.state_manager.update_program_item(self.program_vis.prog.id, self.program_vis.prog.blocks[0].id, self.program_vis.active_item.item)
        return True

    def user_status_cb(self, msg):

        self.emit(QtCore.SIGNAL('user_status'), msg)

    def user_status_cb_evt(self, msg):

        self.user_status = msg

        try:

            if self.user_status.user_state == UserStatus.USER_NOT_CALIBRATED:

                self.fsm.tr_user_present()

            elif self.user_status.user_state == UserStatus.USER_CALIBRATED:

                self.fsm.tr_user_calibrated()

        except MachineError:
            pass
Example #6
0
class ArtBrain:
    UNKNOWN = -2  # should not happen!
    NOP = -1  # no operation
    GET_READY = 0  # retract arms etc.
    MANIP_PICK = 1
    MANIP_PLACE = 2  # TODO proc nepouzit ProgramItem.MANIP_PLACE?
    MANIP_PICK_PLACE = 3
    WAIT = 4

    INST_OK = 100
    INST_BAD_DATA = 101
    INST_FAILED = 102

    SYSTEM_UNKNOWN = 0  # TODO proc nepouzit SystemState.SYSTEM_UNKNOWN?
    SYSTEM_START = 1
    SYSTEM_CALIBRATING = 2
    SYSTEM_STARTING_PROGRAM_SERVER = 3
    SYSTEM_READY_FOR_PROGRAM_REQUESTS = 4
    SYSTEM_STOPPING_PROGRAM_SERVER = 5

    def __init__(self):
        self.show_marker_service = rospy.get_param('show_marker_service', '/art/interface/projected_gui/show_marker')
        self.hide_marker_service = rospy.get_param('hide_marker_service', '/art/interface/projected_gui/hide_marker')
        self.table_localize_action = rospy.get_param('table_localize_action', '/umf_localizer_node_table/localize')
        self.pr2_localize_action = rospy.get_param('pr2_localize_action', '/umf_localizer_node/localize')

        self.calibrate_pr2 = rospy.get_param('calibrate_pr2', False)
        self.calibrate_table = rospy.get_param('calibrate_table', False)

        self.user_status_sub = rospy.Subscriber("/art/user/status", UserStatus, self.user_status_cb)
        self.user_activity_sub = rospy.Subscriber("/art/user/activity", UserActivity, self.user_activity_cb)

        self.srv_program_start = rospy.Service('/art/brain/program/start', startProgram, self.program_start_cb)
        self.srv_program_stop = rospy.Service('/art/brain/program/stop', Empty, self.program_stop_cb)
        # self.srv_program_pause = rospy.Service(/art/brain/program/pause', Empty, self.program_pause_cb)
        # self.srv_program_resume = rospy.Service(/art/brain/program/resume', Empty, self.program_resume_cb)

        self.state_manager = InterfaceStateManager(InterfaceState.BRAIN_ID)  # TODO callback?
        self.art = ArtApiHelper(brain=True)
        self.ph = ProgramHelper()

        self.user_activity = None

        self.objects_sub = rospy.Subscriber("/art/object_detector/object_filtered", InstancesArray, self.objects_cb)

        # TODO use this topic instead of system_state in InterfaceState (duplication) ??
        # TODO move (pub/sub) to InterfaceStateManager?
        self.state_publisher = rospy.Publisher("/art/brain/system_state", SystemState, queue_size=1)

        self.pp_client = actionlib.SimpleActionClient('/art/pr2/left_arm/pp', pickplaceAction)

        self.state = self.SYSTEM_START
        self.user_id = 0

        self.objects = InstancesArray()
        self.executing_program = False

        self.instruction = None
        self.holding_object = None
        self.stop_server = False
        self.recalibrate = False

        self.quit = False
        self.art.wait_for_api()

    def program_start_cb(self,  req):

        resp = startProgramResponse()

        if self.executing_program:

            resp.success = False
            resp.error = 'Program already running'
            return resp

        rospy.loginfo('Loading program ' + str(req.program_id) + ' from db...')

        program = self.art.load_program(req.program_id)

        if not self.ph.load(program):
            resp.success = False
            resp.error = 'Cannot get program'
            return resp

        rospy.loginfo('Starting program')
        self.executing_program = True
        resp.success = True
        return resp

    def program_stop_cb(self,  req):

        rospy.loginfo('Stopping program ' + str(req.program_id) + '...')
        self.executing_program = False
        return EmptyResponse()

    def instruction_switcher(self):
        instructions = {
            self.NOP: self.nop,
            self.GET_READY: self.get_ready,
            self.MANIP_PICK: self.manip_pick,
            self.MANIP_PLACE: self.manip_place,
            self.MANIP_PICK_PLACE: self.manip_pick_place,
            self.WAIT: self.wait,

        }
        return instructions.get(self.instruction, self.unknown_instruction)

    @staticmethod
    def get_item_by_id(program, item_id):  # TODO item should be specified using both block id and item id?
        print "get_item_by_id: " + str(item_id)
        for bl in program.blocks:
            for it in bl.items:
                if it.id == item_id:
                    return it
        return None

    def get_ready(self, instruction):
        # TODO: call some service to set PR2 to ready position
        self.state_manager.update_program_item(self.ph.get_program_id(),  self.block_id,  instruction)
        return self.INST_OK

    def get_pick_obj_id(self, instruction):
        if instruction.spec == instruction.MANIP_ID:
            obj_id = instruction.object
        elif instruction.spec == instruction.MANIP_TYPE:

            pick_polygon = []
            pol = None

            for point in instruction.pick_polygon.polygon.points:  # TODO check frame_id and transform to table frame?
                pick_polygon.append([point.x,  point.y])
            pick_polygon.append([0,  0])

            if len(pick_polygon) > 0:
                pol = mplPath.Path(np.array(pick_polygon), closed=True)

            # shuffle the array to not get the same object each time
            # random.shuffle(self.objects.instances)

            print self.objects.instances

            for obj in self.objects.instances:

                if pol is None:

                    print "no polygon"

                    # if no pick polygon is specified - let's take the first object of that type
                    if obj.object_type == instruction.object:
                        obj_id = obj.object_id
                        break

                else:

                    print "polygon"

                    # test if some object is in polygon and take the first one
                    if pol.contains_point([obj.pose.position.x,  obj.pose.position.y]):
                        obj_id = obj.object_id
                        rospy.loginfo('Selected object: ' + obj_id)
                        break

            else:
                if pol is not None:
                    rospy.loginfo('No object in the specified polygon')
                    print pol
                return self.INST_BAD_DATA
        else:
            print "strange instruction.spec: " + str(instruction.spec)
            return self.INST_BAD_DATA
        return obj_id

    def get_place_pose(self, instruction):
        # if self.holding_object is None:
        #    return None
        if instruction.spec == instruction.MANIP_ID:
            pose = instruction.place_pose
        elif instruction.spec == instruction.MANIP_TYPE:
            # pose = None
            pose = instruction.place_pose
            # TODO: how to get free position inside polygon? some perception node?
        else:
            return None
        return pose

    def manip_pick(self, instruction):
        """

        :type instruction: ProgramItem
        :return:
        """
        obj_id = self.get_pick_obj_id(instruction)
        self.state_manager.update_program_item(self.ph.get_program_id(),  self.block_id,  instruction,  {"SELECTED_OBJECT_ID": obj_id})

        if obj_id is None:
            return self.INST_BAD_DATA
        if self.pick_object(obj_id):
            self.holding_object = obj_id
            return self.INST_OK
        else:
            return self.INST_FAILED

    def manip_place(self, instruction):
        """

        :type instruction: ProgramItem
        :return:
        """

        pose = self.get_place_pose(instruction)
        self.state_manager.update_program_item(self.ph.get_program_id(),  self.block_id,  instruction)  # TODO place pose

        if pose is None:
            return self.INST_BAD_DATA
        else:
            if self.place_object(self.holding_object, pose):
                self.holding_object = None
                return self.INST_OK
            else:
                return self.INST_FAILED

    def manip_pick_place(self, instruction):

        print "manip_pick_place"
        print self.objects
        obj_id = self.get_pick_obj_id(instruction)
        pose = self.get_place_pose(instruction)

        self.state_manager.update_program_item(self.ph.get_program_id(),  self.block_id,  instruction,  {"SELECTED_OBJECT_ID": obj_id})
        # TODO also update p.i. with selected place pose when not given (place polygon)

        if obj_id is None or pose is None:
            print 'could not get obj_id or pose'
            return self.INST_BAD_DATA
        if self.pick_object(obj_id):  # TODO call pick&place and not pick and then place
            self.holding_object = obj_id
            if self.place_object(obj_id, pose):
                self.holding_object = None
                return self.INST_OK
        return self.INST_FAILED

    def wait(self, instruction):
        """

        :type instruction: ProgramItem
        :return:
        """
        print "waiting"

        # return self.INST_OK
        self.state_manager.update_program_item(self.ph.get_program_id(), self.block_id,  instruction)

        rate = rospy.Rate(10)

        if instruction.spec == instruction.WAIT_FOR_USER:
            while self.user_activity != UserActivity.READY:
                rate.sleep()
        elif instruction.spec == instruction.WAIT_UNTIL_USER_FINISHES:
            while self.user_activity != UserActivity.WORKING:
                rate.sleep()
        else:
            return self.INST_BAD_DATA

        return self.INST_OK

    def unknown_instruction(self, instruction):
        print "F*ck it all, i don't know this instruction!"
        return self.INST_FAILED

    def nop(self, instruction=None):
        return self.INST_OK

    def user_activity_cb(self,  data):

        self.user_activity = data.activity

    def user_status_cb(self, data):
        """

        :type data: UserStatus
        :return:
        """
        self.user_id = data.user_id

        pass

    def objects_cb(self, objects_data):
        """

        :type objects_data: InstancesArray
        :return:
        """
        self.objects = objects_data

    def check_user_active(self):
        return self.user_id != 0

    def calibrate(self, action_name, server="unknown", timeout=5):
        client = actionlib.SimpleActionClient(action_name, LocalizeAgainstUMFAction)
        rospy.logdebug("Waiting for server (" + server + ")")
        client.wait_for_server()
        rospy.logdebug("Server ready (" + server + ")")
        goal = LocalizeAgainstUMFGoal()
        goal.timeout = rospy.Duration(timeout)
        rospy.logdebug("Sending goal to server (" + server + ")")
        client.send_goal(goal)
        rospy.logdebug("Waiting for results  (" + server + ")")
        client.wait_for_result()
        return not client.get_result().result
        pass

    def calibrate_all(self, table_calibration=True, pr2_calibration=True):
        rospy.loginfo("Starting calibrating process")
        rospy.logdebug("Waiting for service " + self.show_marker_service)
        rospy.wait_for_service(self.show_marker_service)
        try:
            show_marker = rospy.ServiceProxy(self.show_marker_service, Empty)
            show_marker()

        except rospy.ServiceException, e:
            rospy.logerr("Service call failed: " + str(e))
            return False

        while table_calibration:
            if self.calibrate(self.table_localize_action, "table", 5):
                table_calibration = False
                rospy.loginfo("Table successfully calibrated")
            else:
                rospy.logwarn("Table calibration failed! Trying every 5 sec")
                time.sleep(5)
        while pr2_calibration:
            if self.calibrate(self.pr2_localize_action, "pr2", 5):
                pr2_calibration = False
                rospy.loginfo("PR2 successfully calibrated")
            else:
                rospy.logwarn("PR2 calibration failed! Trying every 5 sec")
                time.sleep(5)
        rospy.loginfo("Calibration done, hiding umf marker")
        rospy.logdebug("Waiting for service " + self.hide_marker_service)
        rospy.wait_for_service(self.hide_marker_service)
        try:
            hide_marker = rospy.ServiceProxy(self.hide_marker_service, Empty)
            hide_marker()

        except rospy.ServiceException, e:
            rospy.logerr("Service call failed: " + str(e))
            return False