def stage_pick()

in groups/arm/ggd/stages.py [0:0]
59 lines of code
9 McCabe index (conditional complexity)

    def stage_pick(self, should_run=None,
                   cli=None, previous_results=None, cartesian=True):
        """
        The arm has found an object and will attempt to pick-up that object.

        :param should_run: a `threading.Event` that tells the stage to continue
            if `is_set()` is True
        :param cli: the cli values to use for the pick
        :param previous_results: a dict containing previous stage results
            pertinent to this stage
        :param cartesian: use cartesian (True) or polar coordinates (False) for
            the pick
        :return: a dict containing this stage's results
        """
        log.info("[stage_pick] _begin_")

        stage_results = dict()
        x = y = 0

        # if value is received from the CLI, use that as pickup goal target
        if cli is not None:
            x = cli.x
            y = cli.y
            if cli.polar is not None and cli.polar:
                cartesian = False

        # if value is received from previous results, use as pickup goal target
        if previous_results is not None:
            x = previous_results['x']
            y = previous_results['y']

        if cartesian:
            # use cartesian coordinates to calculate goals
            log.info("[stage_pick] x:{0} y:{1} cartesian pickup".format(x, y))
            base_goal, femur_goal, tibia_goal = cartesian_goals(x, y)
            log.info("[stage_pick] cart base:{0} femur:{1} tibia:{2}".format(
                base_goal, femur_goal, tibia_goal))
        else:
            # use polar coordinates to calculate goals
            log.info("[stage_pick] x:{0} y:{1} polar pickup".format(x, y))
            base_goal, femur_goal, tibia_goal = polar_goals(x, y)
            log.info(
                "[stage_pick] polar base:{0} femur:{1} tibia:{2}".format(
                    base_goal, femur_goal, tibia_goal))

        # ensure there are the expected number of servos in the ServoGroup
        if len(self.sg) is not 5:
            log.error("[stage_pick] Unexpected sg length:{0}".format(
                len(self.sg)))
            return stage_results

        # OPEN EFFECTOR/CLAW
        #####################################################
        self.sg.goal_position([
            HOME_BASE,
            HOME_FEMUR_1,
            HOME_FEMUR_2,
            HOME_TIBIA,
            OPEN_EFFECTOR
        ], block=False, margin=POSITION_MARGIN)
        time.sleep(.5)

        # go to PICK READY location
        ######################################################
        self.sg.goal_position([
            base_goal,
            HOME_FEMUR_1,
            HOME_FEMUR_2,
            HOME_TIBIA,
            OPEN_EFFECTOR
        ], block=False, margin=POSITION_MARGIN)
        time.sleep(.5)

        self.sg.write_values("moving_speed", [140, 140, 140, 140, 140])
        stage_results['slow_down'] = True

        # go to down-most open PICK location
        ######################################################
        self.sg.goal_position([
            base_goal,
            femur_goal,
            femur_goal,
            tibia_goal,
            OPEN_EFFECTOR
        ], block=False, margin=POSITION_MARGIN)
        time.sleep(.5)

        # change effector to the GRAB location
        ######################################################
        self.sg['effector']['goal_position'] = GRAB_EFFECTOR
        time.sleep(.5)

        # TODO: ensure something has been grabbed using torque feedback

        # the pick-up is now complete
        stage_results['pick_complete'] = True
        log.info("[stage_pick] _end_")

        return stage_results