Ejemplo n.º 1
0
    def execute(self, goal):
        rospy.loginfo("Bug algorithm goal: x:{} y:{}".format(goal.target_pose.pose.position.x, goal.target_pose.pose.position.x))

        rate = rospy.Rate(self.controller_freq)
        waypoint = goal.target_pose
    
        dist, yaw_error = self.waypoint_info(waypoint)
    
        # Calculate timeout
        start_time = rospy.Time.now().to_sec()
        timeout = rospy.Duration(dist / 0.3 * 1.2 + 3).to_sec()
        rospy.loginfo("dist: {}, now: {}, timeout: {}, dt: {}".format(dist, start_time, timeout, rospy.Duration(dist / 0.3 * 1.1 + 3).to_sec()))

        while dist > self.distance_tolerance:
            # If preemt
            if self.server.is_preempt_requested():
                rospy.loginfo("PREEMPT bug_algorithm")
                self.server.set_preempted()
                return
            
            # If timeout
            if rospy.Time.now().to_sec() - start_time > timeout:
                rospy.loginfo("ABORTED bug algorithm")
                self.server.set_aborted()
                return

            try:
                # Calculate waypoint data
                dist, yaw_error = self.waypoint_info(waypoint)
                # print("dist: {:.3f} yaw_err:{:.2f}".format(dist, yaw_error))
                
                # Switch states
                if dist < self.distance_tolerance:
                    break  # Waypoint reached
                
                # Sum sonar and bump sensor vectors
                vel_x, ang_z = self.get_range_vector_sum()

                # Return to heading P controller
                ang_z -= clamp(0.8 * yaw_error, -self.max_heading_fix, self.max_heading_fix)

                # Publish cmd_vel
                # self.cmd_vel_msg.linear.x = (vel_x + self.rc_speed) * self.power_coef  # Testing
                self.cmd_vel_msg.linear.x = (vel_x + 0.35) * self.power_coef  # Event
                self.cmd_vel_msg.angular.z = ang_z * self.power_coef

                self.cmd_vel_msg.linear.x = clamp(self.cmd_vel_msg.linear.x, -self.max_vel_x, self.max_vel_x)
                self.cmd_vel_msg.angular.z = clamp(self.cmd_vel_msg.angular.z, -self.max_ang_z, self.max_ang_z)

                self.cmd_vel_pub.publish(self.cmd_vel_msg)
                # print("PUB: {:.3f}, {:.3f}".format(self.cmd_vel_msg.linear.x, self.cmd_vel_msg.angular.z))
            except Exception as e:
                rospy.logerr("HOPE NO CRASH: {}".format(e))

            rate.sleep()

        rospy.loginfo("Done bug algorithm")
        self.server.set_succeeded(result=MoveBaseResult())
Ejemplo n.º 2
0
    def __init__(self):
        # Constants
        self.MAX_SPEED = clamp(rospy.get_param("hoverboard/max_speed", 200), 0,
                               1000)
        self.MAX_STEER = clamp(rospy.get_param("hoverboard/max_steer", 170), 0,
                               1000)
        self.MAX_ACCEL_LIN = max(
            0, rospy.get_param("hoverboard/max_accel_lin", 1000))
        self.MAX_ACCEL_ANG = max(
            0, rospy.get_param("hoverboard/max_accel_ang", 1000))

        self.MAX_AUTON_VEL_LIN = max(
            0, rospy.get_param("hoverboard/max_auton_vel_lin", 0.3))
        self.MAX_AUTON_VEL_ANG = max(
            0, rospy.get_param("hoverboard/max_auton_vel_ang", 0.8))
        self.MAX_AUTON_ACCEL_LIN = max(
            0, rospy.get_param("hoverboard/max_auton_accel_lin", 0.3))
        self.MAX_AUTON_ACCEL_ANG = max(
            0, rospy.get_param("hoverboard/max_auton_accel_ang", 0.8))

        self.MAX_RC_VEL_LIN = max(
            0, rospy.get_param("hoverboard/max_rc_vel_lin", 1.0))
        self.MAX_RC_VEL_ANG = max(
            0, rospy.get_param("hoverboard/max_rc_vel_ang", 1.0))
        self.MAX_RC_ACCEL_LIN = max(
            0, rospy.get_param("hoverboard/max_rc_accel_lin", 5.0))
        self.MAX_RC_ACCEL_ANG = max(
            0, rospy.get_param("hoverboard/max_rc_accel_ang", 5.0))

        self.safe_vel_lin_fwd = self.MAX_RC_VEL_LIN
        self.safe_vel_lin_bwd = self.MAX_RC_VEL_LIN
        self.safe_vel_ang_cw = self.MAX_RC_VEL_ANG
        self.safe_vel_ang_ccw = self.MAX_RC_VEL_ANG

        self.SWITCH_TRIG = 950  # Trigger value for a switch
        self.TELEOP_RATE = 40  # Command sending to motor frequency

        # Variables
        self.allow_rc = False  # When false always ignore RC
        self.armed = False  # When false ignore RC
        self.auton_mode = False  # When True read /cmd_vel and publish it to PID controller
        self.auton_mode_prev = False  # To track change od auton_mode

        # Is fresh variables
        self.fresh_cmd_vel = IsFresh(0.4, "Velocity command")
        self.fresh_rc_teleop = IsFresh(0.15, "Remote teleop")
        self.fresh_odom = IsFresh(0.1, "Odometry")

        # Other
        self.SONAR_COUNT = 10
        self.sonar_hist = [[] for _ in range(self.SONAR_COUNT)]
        self.BUMP_SENSORS_COUNT = 15
        self.bin_data_old = '0' * self.BUMP_SENSORS_COUNT
Ejemplo n.º 3
0
 def layer_idx(self, i):
     warn("Use doc.layer_stack.current_path instead",
          DeprecatedAPIWarning, stacklevel=3)
     i = helpers.clamp(int(i), 0, max(0, len(self._layer_paths)-1))
     path = self._layer_paths[i]
     self._doc.layer_stack.current_path = path
     self._layer_idx = i
Ejemplo n.º 4
0
 def layer_idx(self, i):
     warn("Use doc.layer_stack.current_path instead",
          DeprecatedAPIWarning,
          stacklevel=3)
     i = helpers.clamp(int(i), 0, max(0, len(self._layer_paths) - 1))
     path = self._layer_paths[i]
     self._doc.layer_stack.current_path = path
     self._layer_idx = i
Ejemplo n.º 5
0
    def __init__(self, entity, attr: str, limits_def: Tuple[Union[int, str],
                                                            Union[int, str]],
                 label: str):
        super().__init__(entity, attr)
        self.limits_def = limits_def

        self.label = label
        self.hitboxes = []

        # ensure value is within limits
        self.set_value(clamp(self.get_value(), *self.get_limits()))
    def _get_eye_boxes(self, landmarks):
        """
        Finds the bounding boxes of eyes given the coordinates of facial landmarks.        
        """

        # The landmarks contain a row-vector of 70 floating point values for 35 landmarks'
        # normed coordinates in the form (x0, y0, x1, y1, ..., x34, y34)

        # Left eye
        xmin_l = landmarks[
            0,
            12 * 2]  # p12: starting point of the upper boundary of the eyebrow
        #xmax_l = landmarks[0, 0*2]  # p0: corner of the eye, located on the boundary of the eyeball and the eyelid
        xmax_l = landmarks[
            0,
            14 * 2]  # p14: ending point of the upper boundary of the eyebrow
        ymin_l = landmarks[0, 13 * 2 +
                           1]  # p13: mid-point of the upper arc of the eyebrow
        #ymax_l = ymin_l + abs(xmax_l - xmin_l) # make a square bounding box
        #ymax_l = landmarks[0, 1*2+1] # p1: corner of the eye, located on the boundary of the eyeball and the eyelid
        # p0: corner of the eye, located on the boundary of the eyeball and the eyelid; p14: ending point of the upper boundary of the eyebrow
        ymax_l = helpers.clamp(
            2 * landmarks[0, 0 * 2 + 1] - landmarks[0, 14 * 2 + 1], 0, 1)

        # Right eye
        xmin_r = landmarks[
            0,
            15 * 2]  # p15: starting point of the upper boundary of the eyebrow
        xmax_r = landmarks[
            0,
            17 * 2]  # p17: ending point of the upper boundary of the eyebrow
        ymin_r = landmarks[0, 16 * 2 +
                           1]  # p16: mid-point of the upper arc of the eyebrow
        #ymax_r = ymin_r + abs(xmax_r - xmax_l)  # make a square bounding box
        #ymax_r = landmarks[0, 3*2+1] # p3: corner of the eye, located on the boundary of the eyeball and the eyelid
        # p2: corners of the eye, located on the boundary of the eyeball and the eyelid; p15: starting point of the upper boundary of the eyebrow
        ymax_r = helpers.clamp(
            2 * landmarks[0, 2 * 2 + 1] - landmarks[0, 15 * 2 + 1], 0, 1)

        return (xmin_l, ymin_l, xmax_l, ymax_l), (xmin_r, ymin_r, xmax_r,
                                                  ymax_r)
Ejemplo n.º 7
0
    def go_to_point(self, point, pos_x_goal, yaw_goal):
        """
            Simple P controller with extra if statements
            :param point: shapely Point
            :param pos_x_goal: goal position x (forward)
            :param yaw_goal: goal yaw angle
            :return: True if goal is withing all tolerances
        """

        pos_error = point.x - pos_x_goal
        yaw_error = math.atan2(point.y, point.x) - yaw_goal
        # print("yaw_err: {:.2f}, pos_err: {:.2f}".format(math.degrees(yaw_error), pos_error))

        # P controller with clampping
        vel = 0
        rot = 0
        is_in_yaw_tol = abs(yaw_error) < self.yaw_tolerance
        is_in_pos_tol = abs(pos_error) < self.pos_tolerance
        is_far = pos_error > self.pos_far

        # Align angle if not in tolerance
        if not is_in_yaw_tol:
            rot = yaw_error * self.yaw_p
            rot = sign(rot) * clamp(abs(rot), self.yaw_min, self.yaw_max)

        # Align position (when yaw in tolerance or really far away)
        if (not is_in_pos_tol and is_in_yaw_tol) or is_far:
            vel = pos_error * self.pos_p
            vel = sign(vel) * clamp(abs(vel), self.pos_min, self.pos_max)

        if is_far:  # Speed up
            vel *= 1.5
            rot *= 1.5

        self.drive(vel, rot)

        if is_in_yaw_tol is True and is_in_pos_tol:
            return True
        return False
Ejemplo n.º 8
0
def enhance(enhancer,
            upscaler,
            image: Image,
            device,
            factor: int = 5,
            half_precision: bool = False) -> Image:

    x = to_tensor(image).to(device).unsqueeze(0)
    h, w = x.shape[-2:]  # original sizes
    x_size = (h, w)
    hp, wp = compute_padding(h, factor), compute_padding(w, factor)
    padding = (0, wp, 0, hp)  # pading: left, right, top, bottom
    x_prime = F.pad(x, padding, mode="replicate")
    h_prime, w_prime = x_prime.shape[-2:]

    x_prime_prime_size = (h_prime // factor, w_prime // factor)
    x_prime_n = normalize(x_prime)
    x_prime_prime_n = F.interpolate(x_prime_n,
                                    size=x_prime_prime_size,
                                    mode=MODE,
                                    align_corners=True)
    print(
        f"Running inference at {x_prime_prime_size[1]}x{x_prime_prime_size[0]} pixels"
    )
    # enhance the input, output is normalized
    with torch.no_grad():
        if half_precision:
            y_hat_prime_prime_n = enhancer(x_prime_prime_n.half())
            guide = x_prime_n.half()
            y_hat_prime_n = F.interpolate(y_hat_prime_prime_n,
                                          guide.shape[-2:],
                                          mode='nearest')
            y_hat_prime_n = upscaler(torch.cat([guide, y_hat_prime_n],
                                               dim=1)).float()
        else:
            y_hat_prime_prime_n = enhancer(x_prime_prime_n)
            guide = x_prime_n
            y_hat_prime_n = F.interpolate(y_hat_prime_prime_n,
                                          guide.shape[-2:],
                                          mode='nearest')
            y_hat_prime_n = upscaler(torch.cat([guide, y_hat_prime_n], dim=1))

    y_hat_prime = denormalize(y_hat_prime_n)
    y_hat = remove_padding(x_size, y_hat_prime)
    result = clamp(y_hat)

    return output_transforms(result)
Ejemplo n.º 9
0
 def __unicode__(self):
     result = u"GIMP Palette\n"
     if self._name is not None:
         result += u"Name: %s\n" % self._name
     if self._columns > 0:
         result += u"Columns: %d\n" % self._columns
     result += u"#\n"
     for col in self._colors:
         if col is self._EMPTY_SLOT_ITEM:
             col_name = self._EMPTY_SLOT_NAME
             r = g = b = 0
         else:
             col_name = col.__name
             r, g, b = [clamp(int(c*0xff), 0, 0xff) for c in col.get_rgb()]
         if col_name is None:
             result += u"%d %d %d\n" % (r, g, b)
         else:
             result += u"%d %d %d    %s\n" % (r, g, b, col_name)
     return result
Ejemplo n.º 10
0
 def __unicode__(self):
     result = u"GIMP Palette\n"
     if self._name is not None:
         result += u"Name: %s\n" % self._name
     if self._columns > 0:
         result += u"Columns: %d\n" % self._columns
     result += u"#\n"
     for col in self._colors:
         if col is self._EMPTY_SLOT_ITEM:
             col_name = self._EMPTY_SLOT_NAME
             r = g = b = 0
         else:
             col_name = col.__name
             r, g, b = [clamp(int(c*0xff), 0, 0xff) for c in col.get_rgb()]
         if col_name is None:
             result += u"%d %d %d\n" % (r, g, b)
         else:
             result += u"%d %d %d    %s\n" % (r, g, b, col_name)
     return result
Ejemplo n.º 11
0
    def draw_onto(self, surf: pg.Surface, rect: pg.Rect, **kwargs):
        super().draw_onto(surf, rect)

        self.set_value(clamp(self.get_value(), *self.get_limits()))

        render_text_left_justified(
            self.label, (0, 0, 0), surf,
            V2(rect.left + rect.width * 0.03, rect.centery), FONT_SIZE)
        # TODO: draw number selector boxes
        box_width = int(rect.height * 0.75)
        box_height = int(rect.height * 0.75)
        box_thickness = 2
        self.hitboxes.clear()
        low, high = self.get_limits()
        for i, n in enumerate(range(low, high + 1)):  # inclusive
            box = pg.Rect(
                rect.left + rect.width * 0.375 +
                (box_width - box_thickness // 2) * i,
                rect.centery - box_height / 2, box_width, box_height)
            color = (255, 255, 255) if n == self.get_value() else (0, 0, 0)
            draw_rectangle(surf, box, (0, 0, 0), thickness=box_thickness)
            render_text_centered_xy(str(n), color, surf, box.center, FONT_SIZE)
            self.hitboxes.append((n, box))
Ejemplo n.º 12
0
    def load_ora(self, filename, feedback_cb=None):
        """Loads from an OpenRaster file"""
        print 'load_ora:'
        t0 = time.time()
        z = zipfile.ZipFile(filename)
        print 'mimetype:', z.read('mimetype').strip()
        xml = z.read('stack.xml')
        image = ET.fromstring(xml)
        stack = image.find('stack')

        w = int(image.attrib['w'])
        h = int(image.attrib['h'])

        def round_up_to_n(value, n):
            assert value >= 0, "function undefined for negative numbers"

            residual = value % n
            if residual:
                value = value - residual + n
            return int(value)

        def get_pixbuf(filename):
            t1 = time.time()

            try:
                fp = z.open(filename, mode='r')
            except KeyError:
                # support for bad zip files (saved by old versions of the GIMP ORA plugin)
                fp = z.open(filename.encode('utf-8'), mode='r')
                print 'WARNING: bad OpenRaster ZIP file. There is an utf-8 encoded filename that does not have the utf-8 flag set:', repr(filename)

            res = self._pixbuf_from_stream(fp, feedback_cb)
            fp.close()
            print '  %.3fs loading %s' % (time.time() - t1, filename)
            return res

        def get_layers_list(root, x=0,y=0):
            res = []
            for item in root:
                if item.tag == 'layer':
                    if 'x' in item.attrib:
                        item.attrib['x'] = int(item.attrib['x']) + x
                    if 'y' in item.attrib:
                        item.attrib['y'] = int(item.attrib['y']) + y
                    res.append(item)
                elif item.tag == 'stack':
                    stack_x = int( item.attrib.get('x', 0) )
                    stack_y = int( item.attrib.get('y', 0) )
                    res += get_layers_list(item, stack_x, stack_y)
                else:
                    print 'Warning: ignoring unsupported tag:', item.tag
            return res

        self.clear() # this leaves one empty layer
        no_background = True
        # FIXME: don't require tile alignment for frame
        self.set_frame(width=round_up_to_n(w, N), height=round_up_to_n(h, N))

        for layer in get_layers_list(stack):
            a = layer.attrib

            if 'background_tile' in a:
                assert no_background
                try:
                    print a['background_tile']
                    self.set_background(get_pixbuf(a['background_tile']))
                    no_background = False
                    continue
                except backgroundsurface.BackgroundError, e:
                    print 'ORA background tile not usable:', e

            src = a.get('src', '')
            if not src.lower().endswith('.png'):
                print 'Warning: ignoring non-png layer'
                continue
            pixbuf = get_pixbuf(src)
            name = a.get('name', '')
            x = int(a.get('x', '0'))
            y = int(a.get('y', '0'))
            opac = float(a.get('opacity', '1.0'))
            visible = not 'hidden' in a.get('visibility', 'visible')
            self.add_layer(insert_idx=0, name=name)
            last_pixbuf = pixbuf
            t1 = time.time()
            self.load_layer_from_pixbuf(pixbuf, x, y)
            layer = self.layers[0]

            self.set_layer_opacity(helpers.clamp(opac, 0.0, 1.0), layer)
            self.set_layer_visibility(visible, layer)
            print '  %.3fs converting pixbuf to layer format' % (time.time() - t1)
            # strokemap
            fname = a.get('mypaint_strokemap_v2', None)
            if fname:
                if x % N or y % N:
                    print 'Warning: dropping non-aligned strokemap'
                else:
                    sio = StringIO(z.read(fname))
                    layer.load_strokemap_from_file(sio, x, y)
                    sio.close()
Ejemplo n.º 13
0
    def load_ora(self, filename, feedback_cb=None):
        """Loads from an OpenRaster file"""
        print 'load_ora:'
        t0 = time.time()
        z = zipfile.ZipFile(filename)
        print 'mimetype:', z.read('mimetype').strip()
        xml = z.read('stack.xml')
        image = ET.fromstring(xml)
        stack = image.find('stack')

        w = int(image.attrib['w'])
        h = int(image.attrib['h'])

        def round_up_to_n(value, n):
            assert value >= 0, "function undefined for negative numbers"

            residual = value % n
            if residual:
                value = value - residual + n
            return int(value)

        def get_pixbuf(filename):
            t1 = time.time()

            try:
                fp = z.open(filename, mode='r')
            except KeyError:
                # support for bad zip files (saved by old versions of the GIMP ORA plugin)
                fp = z.open(filename.encode('utf-8'), mode='r')
                print 'WARNING: bad OpenRaster ZIP file. There is an utf-8 encoded filename that does not have the utf-8 flag set:', repr(filename)

            res = self._pixbuf_from_stream(fp, feedback_cb)
            fp.close()
            print '  %.3fs loading %s' % (time.time() - t1, filename)
            return res

        def get_layers_list(root, x=0,y=0):
            res = []
            for item in root:
                if item.tag == 'layer':
                    if 'x' in item.attrib:
                        item.attrib['x'] = int(item.attrib['x']) + x
                    if 'y' in item.attrib:
                        item.attrib['y'] = int(item.attrib['y']) + y
                    res.append(item)
                elif item.tag == 'stack':
                    stack_x = int( item.attrib.get('x', 0) )
                    stack_y = int( item.attrib.get('y', 0) )
                    res += get_layers_list(item, stack_x, stack_y)
                else:
                    print 'Warning: ignoring unsupported tag:', item.tag
            return res

        self.clear() # this leaves one empty layer
        no_background = True
        # FIXME: don't require tile alignment for frame
        self.set_frame(width=round_up_to_n(w, N), height=round_up_to_n(h, N))

        for layer in get_layers_list(stack):
            a = layer.attrib

            if 'background_tile' in a:
                assert no_background
                try:
                    print a['background_tile']
                    self.set_background(get_pixbuf(a['background_tile']))
                    no_background = False
                    continue
                except backgroundsurface.BackgroundError, e:
                    print 'ORA background tile not usable:', e

            src = a.get('src', '')
            if not src.lower().endswith('.png'):
                print 'Warning: ignoring non-png layer'
                continue
            pixbuf = get_pixbuf(src)
            name = a.get('name', '')
            x = int(a.get('x', '0'))
            y = int(a.get('y', '0'))
            opac = float(a.get('opacity', '1.0'))
            compositeop = str(a.get('composite-op', DEFAULT_COMPOSITE_OP))
            if compositeop not in VALID_COMPOSITE_OPS:
                compositeop = DEFAULT_COMPOSITE_OP

            visible = not 'hidden' in a.get('visibility', 'visible')
            self.add_layer(insert_idx=0, name=name)
            last_pixbuf = pixbuf
            t1 = time.time()
            self.load_layer_from_pixbuf(pixbuf, x, y)
            layer = self.layers[0]

            self.set_layer_opacity(helpers.clamp(opac, 0.0, 1.0), layer)
            self.set_layer_compositeop(compositeop, layer)
            self.set_layer_visibility(visible, layer)
            print '  %.3fs converting pixbuf to layer format' % (time.time() - t1)
            # strokemap
            fname = a.get('mypaint_strokemap_v2', None)
            if fname:
                if x % N or y % N:
                    print 'Warning: dropping non-aligned strokemap'
                else:
                    sio = StringIO(z.read(fname))
                    layer.load_strokemap_from_file(sio, x, y)
                    sio.close()
Ejemplo n.º 14
0
    def load(self, filehandle, silent=False):
        """Load contents from a file handle containing a GIMP palette.

        :param filehandle: File-like object (.readline, line iteration)
        :param bool silent: If true, don't emit any events.

        >>> pal = Palette()
        >>> with open("palettes/MyPaint_Default.gpl", "r") as fp:
        ...     pal.load(fp)
        >>> len(pal) > 1
        True

        If the file format is incorrect, a RuntimeError will be raised.

        """
        comment_line_re = re.compile(r'^#')
        field_line_re = re.compile(r'^(\w+)\s*:\s*(.*)$')
        color_line_re = re.compile(r'^(\d+)\s+(\d+)\s+(\d+)\s*(?:\b(.*))$')
        fp = filehandle
        self.clear(silent=True)   # method fires events itself
        line = fp.readline()
        if line.strip() != "GIMP Palette":
            raise RuntimeError("Not a valid GIMP Palette")
        header_done = False
        line_num = 0
        for line in fp:
            line = line.strip()
            line_num += 1
            if line == '':
                continue
            if comment_line_re.match(line):
                continue
            if not header_done:
                match = field_line_re.match(line)
                if match:
                    key, value = match.groups()
                    key = key.lower()
                    if key == 'name':
                        self._name = value.strip()
                    elif key == 'columns':
                        self._columns = int(value)
                    else:
                        logger.warning("Unknown 'key:value' pair %r", line)
                    continue
                else:
                    header_done = True
            match = color_line_re.match(line)
            if not match:
                logger.warning("Expected 'R G B [Name]', not %r", line)
                continue
            r, g, b, col_name = match.groups()
            col_name = col_name.strip()
            r = float(clamp(int(r), 0, 0xff))/0xff
            g = float(clamp(int(g), 0, 0xff))/0xff
            b = float(clamp(int(b), 0, 0xff))/0xff
            if r == g == b == 0 and col_name == self._EMPTY_SLOT_NAME:
                self.append(None)
            else:
                col = RGBColor(r, g, b)
                col.__name = col_name
                self._colors.append(col)
        if not silent:
            self.info_changed()
            self.sequence_changed()
            self.match_changed()
Ejemplo n.º 15
0
def flood_fill(src, x, y, color, bbox, tolerance, dst):
    """Fills connected areas of one surface into another

    :param src: Source surface-like object
    :type src: Anything supporting readonly tile_request()
    :param x: Starting point X coordinate
    :param y: Starting point Y coordinate
    :param color: an RGB color
    :type color: tuple
    :param bbox: Bounding box: limits the fill
    :type bbox: lib.helpers.Rect or equivalent 4-tuple
    :param tolerance: how much filled pixels are permitted to vary
    :type tolerance: float [0.0, 1.0]
    :param dst: Target surface
    :type dst: lib.tiledsurface.MyPaintSurface

    See also `lib.layer.Layer.flood_fill()`.
    """
    # Color to fill with
    fill_r, fill_g, fill_b = color

    # Limits
    tolerance = helpers.clamp(tolerance, 0.0, 1.0)

    # Maximum area to fill: tile and in-tile pixel extents
    bbx, bby, bbw, bbh = bbox
    if bbh <= 0 or bbw <= 0:
        return
    bbbrx = bbx + bbw - 1
    bbbry = bby + bbh - 1
    min_tx = int(bbx // N)
    min_ty = int(bby // N)
    max_tx = int(bbbrx // N)
    max_ty = int(bbbry // N)
    min_px = int(bbx % N)
    min_py = int(bby % N)
    max_px = int(bbbrx % N)
    max_py = int(bbbry % N)

    # Tile and pixel addressing for the seed point
    tx, ty = int(x // N), int(y // N)
    px, py = int(x % N), int(y % N)

    # Sample the pixel color there to obtain the target color
    with src.tile_request(tx, ty, readonly=True) as start:
        targ_r, targ_g, targ_b, targ_a = [int(c) for c in start[py][px]]
    if targ_a == 0:
        targ_r = 0
        targ_g = 0
        targ_b = 0
        targ_a = 0

    # Flood-fill loop
    filled = {}
    tileq = [
        ((tx, ty),
         [(px, py)])
    ]
    while len(tileq) > 0:
        (tx, ty), seeds = tileq.pop(0)
        # Bbox-derived limits
        if tx > max_tx or ty > max_ty:
            continue
        if tx < min_tx or ty < min_ty:
            continue
        # Pixel limits within this tile...
        min_x = 0
        min_y = 0
        max_x = N-1
        max_y = N-1
        # ... vary at the edges
        if tx == min_tx:
            min_x = min_px
        if ty == min_ty:
            min_y = min_py
        if tx == max_tx:
            max_x = max_px
        if ty == max_ty:
            max_y = max_py
        # Flood-fill one tile
        with src.tile_request(tx, ty, readonly=True) as src_tile:
            dst_tile = filled.get((tx, ty), None)
            if dst_tile is None:
                dst_tile = numpy.zeros((N, N, 4), 'uint16')
                filled[(tx, ty)] = dst_tile
            overflows = mypaintlib.tile_flood_fill(
                src_tile, dst_tile, seeds,
                targ_r, targ_g, targ_b, targ_a,
                fill_r, fill_g, fill_b,
                min_x, min_y, max_x, max_y,
                tolerance
            )
            seeds_n, seeds_e, seeds_s, seeds_w = overflows
        # Enqueue overflows in each cardinal direction
        if seeds_n and ty > min_ty:
            tpos = (tx, ty-1)
            tileq.append((tpos, seeds_n))
        if seeds_w and tx > min_tx:
            tpos = (tx-1, ty)
            tileq.append((tpos, seeds_w))
        if seeds_s and ty < max_ty:
            tpos = (tx, ty+1)
            tileq.append((tpos, seeds_s))
        if seeds_e and tx < max_tx:
            tpos = (tx+1, ty)
            tileq.append((tpos, seeds_e))

    # Composite filled tiles into the destination surface
    mode = mypaintlib.CombineNormal
    for (tx, ty), src_tile in filled.iteritems():
        with dst.tile_request(tx, ty, readonly=False) as dst_tile:
            mypaintlib.tile_combine(mode, src_tile, dst_tile, True, 1.0)
        dst._mark_mipmap_dirty(tx, ty)
    bbox = lib.surface.get_tiles_bbox(filled)
    dst.notify_observers(*bbox)
Ejemplo n.º 16
0
    def load(self, filehandle, silent=False):
        """Load contents from a file handle containing a GIMP palette.

        :param filehandle: File-like object (.readline, line iteration)
        :param bool silent: If true, don't emit any events.

        >>> pal = Palette()
        >>> with open("palettes/MyPaint_Default.gpl", "r") as fp:
        ...     pal.load(fp)
        >>> len(pal) > 1
        True

        If the file format is incorrect, a RuntimeError will be raised.

        """
        comment_line_re = re.compile(r'^#')
        field_line_re = re.compile(r'^(\w+)\s*:\s*(.*)$')
        color_line_re = re.compile(r'^(\d+)\s+(\d+)\s+(\d+)\s*(?:\b(.*))$')
        fp = filehandle
        self.clear(silent=True)   # method fires events itself
        line = fp.readline()
        if line.strip() != "GIMP Palette":
            raise RuntimeError("Not a valid GIMP Palette")
        header_done = False
        line_num = 0
        for line in fp:
            line = line.strip()
            line_num += 1
            if line == '':
                continue
            if comment_line_re.match(line):
                continue
            if not header_done:
                match = field_line_re.match(line)
                if match:
                    key, value = match.groups()
                    key = key.lower()
                    if key == 'name':
                        self._name = value.strip()
                    elif key == 'columns':
                        self._columns = int(value)
                    else:
                        logger.warning("Unknown 'key:value' pair %r", line)
                    continue
                else:
                    header_done = True
            match = color_line_re.match(line)
            if not match:
                logger.warning("Expected 'R G B [Name]', not %r", line)
                continue
            r, g, b, col_name = match.groups()
            col_name = col_name.strip()
            r = clamp(int(r), 0, 0xff) / 0xff
            g = clamp(int(g), 0, 0xff) / 0xff
            b = clamp(int(b), 0, 0xff) / 0xff
            if r == g == b == 0 and col_name == self._EMPTY_SLOT_NAME:
                self.append(None)
            else:
                col = RGBColor(r, g, b)
                col.__name = col_name
                self._colors.append(col)
        if not silent:
            self.info_changed()
            self.sequence_changed()
            self.match_changed()
Ejemplo n.º 17
0
    def load_ora(self, filename, feedback_cb=None):
        """Loads from an OpenRaster file"""
        print 'load_ora:'
        t0 = time.time()
        tempdir = tempfile.mkdtemp('mypaint')
        if not isinstance(tempdir, unicode):
            tempdir = tempdir.decode(sys.getfilesystemencoding())
        z = zipfile.ZipFile(filename)
        print 'mimetype:', z.read('mimetype').strip()
        xml = z.read('stack.xml')
        image = ET.fromstring(xml)
        stack = image.find('stack')

        w = int(image.attrib['w'])
        h = int(image.attrib['h'])

        def get_pixbuf(filename):
            t1 = time.time()

            try:
                fp = z.open(filename, mode='r')
            except KeyError:
                # support for bad zip files (saved by old versions of the GIMP ORA plugin)
                fp = z.open(filename.encode('utf-8'), mode='r')
                print 'WARNING: bad OpenRaster ZIP file. There is an utf-8 encoded filename that does not have the utf-8 flag set:', repr(filename)

            res = self._pixbuf_from_stream(fp, feedback_cb)
            fp.close()
            print '  %.3fs loading %s' % (time.time() - t1, filename)
            return res

        def get_layers_list(root, x=0,y=0):
            res = []
            for item in root:
                if item.tag == 'layer':
                    if 'x' in item.attrib:
                        item.attrib['x'] = int(item.attrib['x']) + x
                    if 'y' in item.attrib:
                        item.attrib['y'] = int(item.attrib['y']) + y
                    res.append(item)
                elif item.tag == 'stack':
                    stack_x = int( item.attrib.get('x', 0) )
                    stack_y = int( item.attrib.get('y', 0) )
                    res += get_layers_list(item, stack_x, stack_y)
                else:
                    print 'Warning: ignoring unsupported tag:', item.tag
            return res

        self.clear() # this leaves one empty layer
        no_background = True
        self.set_frame(width=w, height=h)

        selected_layer = None
        for layer in get_layers_list(stack):
            a = layer.attrib

            if 'background_tile' in a:
                assert no_background
                try:
                    print a['background_tile']
                    self.set_background(get_pixbuf(a['background_tile']))
                    no_background = False
                    continue
                except tiledsurface.BackgroundError, e:
                    print 'ORA background tile not usable:', e

            src = a.get('src', '')
            if not src.lower().endswith('.png'):
                print 'Warning: ignoring non-png layer'
                continue
            name = a.get('name', '')
            x = int(a.get('x', '0'))
            y = int(a.get('y', '0'))
            opac = float(a.get('opacity', '1.0'))
            compositeop = str(a.get('composite-op', DEFAULT_COMPOSITE_OP))
            if compositeop not in VALID_COMPOSITE_OPS:
                compositeop = DEFAULT_COMPOSITE_OP
            selected = self.__xsd2bool(a.get("selected", 'false'))
            locked = self.__xsd2bool(a.get("edit-locked", 'false'))

            visible = not 'hidden' in a.get('visibility', 'visible')
            self.add_layer(insert_idx=0, name=name)
            t1 = time.time()

            # extract the png form the zip into a file first
            # the overhead for doing so seems to be neglegible (around 5%)
            z.extract(src, tempdir)
            tmp_filename = join(tempdir, src)
            self.load_layer_from_png(tmp_filename, x, y, feedback_cb)
            os.remove(tmp_filename)

            layer = self.layers[0]

            self.set_layer_opacity(helpers.clamp(opac, 0.0, 1.0), layer)
            self.set_layer_compositeop(compositeop, layer)
            self.set_layer_visibility(visible, layer)
            self.set_layer_locked(locked, layer)
            if selected:
                selected_layer = layer
            print '  %.3fs loading and converting layer png' % (time.time() - t1)
            # strokemap
            fname = a.get('mypaint_strokemap_v2', None)
            if fname:
                if x % N or y % N:
                    print 'Warning: dropping non-aligned strokemap'
                else:
                    sio = StringIO(z.read(fname))
                    layer.load_strokemap_from_file(sio, x, y)
                    sio.close()
Ejemplo n.º 18
0
    def load_ora(self, filename, feedback_cb=None):
        """Loads from an OpenRaster file"""
        logger.info('load_ora: %r', filename)
        t0 = time.time()
        tempdir = tempfile.mkdtemp('mypaint')
        if not isinstance(tempdir, unicode):
            tempdir = tempdir.decode(sys.getfilesystemencoding())
        z = zipfile.ZipFile(filename)
        logger.debug('mimetype: %r', z.read('mimetype').strip())
        xml = z.read('stack.xml')
        image = ET.fromstring(xml)
        stack = image.find('stack')

        image_w = int(image.attrib['w'])
        image_h = int(image.attrib['h'])

        def get_pixbuf(filename):
            t1 = time.time()

            try:
                fp = z.open(filename, mode='r')
            except KeyError:
                # support for bad zip files (saved by old versions of the GIMP ORA plugin)
                fp = z.open(filename.encode('utf-8'), mode='r')
                logger.warning(
                    'Bad OpenRaster ZIP file. There is an utf-8 '
                    'encoded filename that does not have the '
                    'utf-8 flag set: %r', filename)

            res = self._pixbuf_from_stream(fp, feedback_cb)
            fp.close()
            logger.debug('%.3fs loading pixbuf %s', time.time() - t1, filename)
            return res

        def get_layers_list(root, x=0, y=0):
            res = []
            for item in root:
                if item.tag == 'layer':
                    if 'x' in item.attrib:
                        item.attrib['x'] = int(item.attrib['x']) + x
                    if 'y' in item.attrib:
                        item.attrib['y'] = int(item.attrib['y']) + y
                    res.append(item)
                elif item.tag == 'stack':
                    stack_x = int(item.attrib.get('x', 0))
                    stack_y = int(item.attrib.get('y', 0))
                    res += get_layers_list(item, stack_x, stack_y)
                else:
                    logger.warning('ignoring unsupported tag %r', item.tag)
            return res

        self.clear()  # this leaves one empty layer
        no_background = True

        selected_layer = None
        for layer in get_layers_list(stack):
            a = layer.attrib

            if 'background_tile' in a:
                assert no_background
                try:
                    logger.debug("background tile: %r", a['background_tile'])
                    self.set_background(get_pixbuf(a['background_tile']))
                    no_background = False
                    continue
                except tiledsurface.BackgroundError, e:
                    logger.warning('ORA background tile not usable: %r', e)

            src = a.get('src', '')
            if not src.lower().endswith('.png'):
                logger.warning('Ignoring non-png layer %r', src)
                continue
            name = a.get('name', '')
            x = int(a.get('x', '0'))
            y = int(a.get('y', '0'))
            opac = float(a.get('opacity', '1.0'))
            compositeop = str(a.get('composite-op', DEFAULT_COMPOSITE_OP))
            if compositeop not in VALID_COMPOSITE_OPS:
                compositeop = DEFAULT_COMPOSITE_OP
            selected = self.__xsd2bool(a.get("selected", 'false'))
            locked = self.__xsd2bool(a.get("edit-locked", 'false'))

            visible = not 'hidden' in a.get('visibility', 'visible')
            self.add_layer(insert_idx=0, name=name)
            t1 = time.time()

            # extract the png form the zip into a file first
            # the overhead for doing so seems to be neglegible (around 5%)
            z.extract(src, tempdir)
            tmp_filename = join(tempdir, src)
            self.load_layer_from_png(tmp_filename, x, y, feedback_cb)
            os.remove(tmp_filename)

            layer = self.layers[0]

            self.set_layer_opacity(helpers.clamp(opac, 0.0, 1.0), layer)
            self.set_layer_compositeop(compositeop, layer)
            self.set_layer_visibility(visible, layer)
            self.set_layer_locked(locked, layer)
            if selected:
                selected_layer = layer
            logger.debug('%.3fs loading and converting layer png',
                         time.time() - t1)
            # strokemap
            fname = a.get('mypaint_strokemap_v2', None)
            if fname:
                sio = StringIO(z.read(fname))
                layer.load_strokemap_from_file(sio, x, y)
                sio.close()
Ejemplo n.º 19
0
def flood_fill(src, x, y, color, bbox, tolerance, dst):
    """Fills connected areas of one surface into another

    :param src: Source surface-like object
    :type src: Anything supporting readonly tile_request()
    :param x: Starting point X coordinate
    :param y: Starting point Y coordinate
    :param color: an RGB color
    :type color: tuple
    :param bbox: Bounding box: limits the fill
    :type bbox: lib.helpers.Rect or equivalent 4-tuple
    :param tolerance: how much filled pixels are permitted to vary
    :type tolerance: float [0.0, 1.0]
    :param dst: Target surface
    :type dst: lib.tiledsurface.MyPaintSurface

    See also `lib.layer.Layer.flood_fill()`.
    """
    # Color to fill with
    fill_r, fill_g, fill_b = color

    # Limits
    tolerance = helpers.clamp(tolerance, 0.0, 1.0)

    # Maximum area to fill: tile and in-tile pixel extents
    bbx, bby, bbw, bbh = bbox
    if bbh <= 0 or bbw <= 0:
        return
    bbbrx = bbx + bbw - 1
    bbbry = bby + bbh - 1
    min_tx = int(bbx // N)
    min_ty = int(bby // N)
    max_tx = int(bbbrx // N)
    max_ty = int(bbbry // N)
    min_px = int(bbx % N)
    min_py = int(bby % N)
    max_px = int(bbbrx % N)
    max_py = int(bbbry % N)

    # Tile and pixel addressing for the seed point
    tx, ty = int(x // N), int(y // N)
    px, py = int(x % N), int(y % N)

    # Sample the pixel color there to obtain the target color
    with src.tile_request(tx, ty, readonly=True) as start:
        targ_r, targ_g, targ_b, targ_a = [int(c) for c in start[py][px]]
    if targ_a == 0:
        targ_r = 0
        targ_g = 0
        targ_b = 0
        targ_a = 0

    # Flood-fill loop
    filled = {}
    tileq = [((tx, ty), [(px, py)])]
    while len(tileq) > 0:
        (tx, ty), seeds = tileq.pop(0)
        # Bbox-derived limits
        if tx > max_tx or ty > max_ty:
            continue
        if tx < min_tx or ty < min_ty:
            continue
        # Pixel limits within this tile...
        min_x = 0
        min_y = 0
        max_x = N - 1
        max_y = N - 1
        # ... vary at the edges
        if tx == min_tx:
            min_x = min_px
        if ty == min_ty:
            min_y = min_py
        if tx == max_tx:
            max_x = max_px
        if ty == max_ty:
            max_y = max_py
        # Flood-fill one tile
        with src.tile_request(tx, ty, readonly=True) as src_tile:
            dst_tile = filled.get((tx, ty), None)
            if dst_tile is None:
                dst_tile = numpy.zeros((N, N, 4), 'uint16')
                filled[(tx, ty)] = dst_tile
            overflows = mypaintlib.tile_flood_fill(src_tile, dst_tile, seeds,
                                                   targ_r, targ_g, targ_b,
                                                   targ_a, fill_r, fill_g,
                                                   fill_b, min_x, min_y, max_x,
                                                   max_y, tolerance)
            seeds_n, seeds_e, seeds_s, seeds_w = overflows
        # Enqueue overflows in each cardinal direction
        if seeds_n and ty > min_ty:
            tpos = (tx, ty - 1)
            tileq.append((tpos, seeds_n))
        if seeds_w and tx > min_tx:
            tpos = (tx - 1, ty)
            tileq.append((tpos, seeds_w))
        if seeds_s and ty < max_ty:
            tpos = (tx, ty + 1)
            tileq.append((tpos, seeds_s))
        if seeds_e and tx < max_tx:
            tpos = (tx + 1, ty)
            tileq.append((tpos, seeds_e))

    # Composite filled tiles into the destination surface
    mode = mypaintlib.CombineNormal
    for (tx, ty), src_tile in filled.iteritems():
        with dst.tile_request(tx, ty, readonly=False) as dst_tile:
            mypaintlib.tile_combine(mode, src_tile, dst_tile, True, 1.0)
        dst._mark_mipmap_dirty(tx, ty)
    bbox = lib.surface.get_tiles_bbox(filled)
    dst.notify_observers(*bbox)
Ejemplo n.º 20
0
    def handle_mousebuttondown(self, button):
        mouse_over_shelf = self.mouse_pos.y >= self.screen_height - self.shelf_height_onscreen
        mouse_over_editor = self.mouse_pos.x >= self.screen_width - self.editor_width_onscreen \
                            and self.mouse_pos.y < self.screen_height - SHELF_HEIGHT

        # scroll wheel
        if button in (4, 5):
            if button == 4:
                scroll_direction = 1
            elif button == 5:
                scroll_direction = -1
            
            # handle editor scrolling
            editor_scrolled = False
            if mouse_over_editor:
                old_scroll_amt = self.editor_scroll_amt
                max_scroll_amt = self.editor_content_height - self.editor_surf.get_height()
                if max_scroll_amt < 0: max_scroll_amt = 0
                self.editor_scroll_amt = clamp(
                    self.editor_scroll_amt + scroll_direction * EDITOR_SCROLL_SPEED, 
                    -max_scroll_amt, 0
                )
                editor_scrolled = old_scroll_amt != self.editor_scroll_amt
                if editor_scrolled:
                    self.editor_changed = True
            
            # # if unable to scroll editor, then zoom the camera
            # if not editor_scrolled:
            # if not over editor editor, then zoom the camera
            if not mouse_over_editor:
                zoom_direction = 0
                if button == 4:    # zoom in
                    zoom_direction = 1
                elif button == 5:   # zoom out
                    zoom_direction = -1
                
                if zoom_direction != 0:
                    pivot = self.camera.get_world_coords(self.mouse_pos, self.screen_width, self.screen_height)
                    self.camera.zoom(zoom_direction, pivot)
                    self.viewport_changed = True
        
        # left click
        if button == 1:
            # handle shelf icons
            for rect, icon in self.shelf_icon_rects:
                if rect.collidepoint(*self.mouse_pos):
                    self.pressed_icon = icon
                    self.reblit_needed = True
                    return
            
            # if not self.edit_mode:
            #     return

            entity_clicked = None
            clicked_wire_widget = False

            # handle entity holding (left click)
            if mouse_over_shelf:
                adjusted_pos = self.mouse_pos - V2(0, self.screen_height - self.shelf_height_onscreen)
                for rect, e_prototype in self.palette_rects:
                    if rect.collidepoint(*adjusted_pos):
                        # pick up entity (from palette)
                        new = e_prototype.get_instance()  # create new entity
                        self.held_entity = new
                        self.hold_point = V2(0.5, 0.5)
                        self.level.palette.remove(e_prototype)
                        self.deselect_entity()
                        self.select_entity(new)
                        self.shelf_changed = True
                        break
            elif mouse_over_editor:
                if self.edit_mode:      # cannot interact with editor if not in edit mode
                    adjusted_pos = self.mouse_pos - V2(self.screen_width - self.editor_width_onscreen, 0)
                    for hitbox, widget in self.widget_rects:
                        if hitbox.collidepoint(*adjusted_pos):
                            clicked_wire_widget = widget.handle_click(adjusted_pos)
                            if clicked_wire_widget:
                                self.finish_wiring(None)    # deselect previously selected wire widget
                                self.wiring_widget = clicked_wire_widget
                            self.editor_changed = True      # just redraw every time (easier)
                            self.viewport_changed = True    # ^^^
                            break
                else:   # if not in edit mode, trigger read-only indicator to blink
                    if self.read_only_indicator_blink_frames == 0:                      # if not currently blinking
                        self.read_only_indicator_blink_frames = 2 * BLINK_DURATION      # blink twice
            else:   # mouse is over board
                pos_float = self.camera.get_world_coords(self.mouse_pos, self.screen_width, self.screen_height)
                pos = pos_float.floor()
                cell = self.level.board.get(*pos)
                if cell:
                    entity_clicked = cell[-1]    # click last element; TODO: figure out if this is a problem lol
            
                if self.wiring_widget is None:  # don't change selection if in wiring mode
                    # deselect current selection if clicking on something else
                    if self.selected_entity is not entity_clicked:
                        self.deselect_entity()
                    if entity_clicked is not None and not entity_clicked.locked:
                        # pick up entity (from board)
                        if self.edit_mode:
                            self.held_entity = entity_clicked
                            self.hold_point = pos_float.fmod(1)
                            self.level.board.remove(*pos, self.held_entity)
                            self.viewport_changed = True
                        if entity_clicked.editable:
                            self.select_entity(entity_clicked)
                    else:
                        self.deselect_entity()

            if not clicked_wire_widget:
                self.finish_wiring(entity_clicked)
Ejemplo n.º 21
0
 def get_wire_width(self):
     w = round(DEFAULT_WIRE_WIDTH * self.zoom_level)
     return clamp(w, MIN_GRID_LINE_WIDTH, MAX_GRID_LINE_WIDTH)
Ejemplo n.º 22
0
 def __init__(self, center: V2, zoom_level: float):
     self.center = center
     self.zoom_level = clamp(zoom_level, self.min_zoom_level,
                             self.max_zoom_level)
Ejemplo n.º 23
0
    def app_control(self, loopStart):
        """

        loopStart: float
            Current clock time in microseconds
        """

        # Stop IK and g8s from coinciding... make Numa stop in place.
        #if self.walk == True:
        #    self.g8countdown = g8Stand(self.axbus, self.leg_ids)

        # -------- Start Switch/Button-------
        # Switch/Button - see switch.
        # To test if it is pressed then
        #    if button.pressed():
        #        # Triggers gun test           #Want to run motors at 7.2V, so do PWM:
        #        act_setSpeed(&LeftGun, -70)   #NOTE: (7.2V / 12.6V) * 127 = 72.5714286
        #
        #        # pressed
        #        # We use the light variable to toggle stuff.
        #        if (light == True):
        #            LED_on(&statusLED)
        #            light = False
        #            #print("on!")
        #
        #        else:
        #            LED_off(&statusLED)
        #            light = True
        #            #print("off")

        # TODO: Consider whether I can justify disabling guns when self.cmdrAlive=0
        # Check whether to stop firing guns
        if self.guns_firing and loopStart > self.guns_firing_end_time:
            self.guns_firing = False
            self.gunMotor.direct_set_speed(GUN_SPEED_OFF)
            self.guns_firing_end_time = loopStart

        #/////////////////////////////////////////

        # Get current commander command
        self.CmdrReadMsgs()
        self.cmdrAlive -= 1
        self.cmdrAlive = clamp(self.cmdrAlive, 0, CMDR_ALIVE_CNT)

        #
        adcval_effort = self.ammoMotor.cs_adc.read()
        adcval_loaded = self.bb_detect_adc.read()
        self.bb_loader_service(loopStart, adcval_effort, adcval_loaded)

        # Get current time in ms
        ms = (loopStart) / 1000

        # We always move the turret to the position specified by the Commander.
        self.axbus.sync_write(self.turret_ids, ax.GOAL_POSITION, [
            struct.pack('<H', self.pan_pos),
            struct.pack('<H', self.tilt_pos)
        ])

        # dump temperatures
        self.read_write_s2_temps(ms)

        # If a gait has recently set the g8countdown, we won't process any leg movement code
        if self.g8countdown:
            #
            pass

        # TODO Test this!
        # Guessing: We go into the g8Crouch pose, then we reenable the torque to the 2nd servo in each leg afterwards
        if self.crouchbutton:
            self.crouchCnt += 1

            # If we're ready to crouch and aren't already
            if self.crouchCnt >= LOOPS_B4_CROUCH and not self.crouch:
                self.g8countdown = g8Crouch(
                    self.gaits, self.axbus, self.leg_ids
                )  # This disables torque to second servo in each leg
                self.crouch = True
                print("Howdydoo?", self.crouchCnt)
                self.crouchCnt = 0
                # After doing safety-critical things, we'll end the loop early
            # Already crouched
            elif self.crouchCnt >= LOOPS_B4_CROUCH:
                print("Hurray, not spasming now?", self.crouchCnt)
                pass

            #self.crouchbutton = False
            # Limit to one increment of crouchCnt per button press
            #sleep_ms(10)

        # If crouch no longer pressed and were in crouch mode, Exit crouch/panic, enable standing, and re-enable torque to 2nd servo of each leg.
        elif self.crouch:
            self.crouch = False
            self.standing = 0
            # Will subsequently automatically stand

            # Enable torque second servo of each leg
            self.axbus.sync_write(self.leg_ids[4:8], ax.TORQUE_ENABLE,
                                  [bytearray([1]) for _ in range(4)])

        #elif self.crouchCnt:

        #FIRE THE GUNS!!!!!
        #TODO
        # NOTE could also make this conditional on loader_timeout_mode == "running"
        # But (hypothesis) I'm not going to be continually firing for more than a second at a time, so I should
        # have sufficient BBs to feed in via gravity while unjamming occurs.
        if self.gunbutton:
            print("bang!!!")
            self.guns_firing = True
            self.gunMotor.direct_set_speed(GUN_SPEED_ON)
            self.guns_firing_end_time = ticks_us() + GUNS_FIRING_DURATION
            self.loader_timeout_end = loopStart + LOADER_TIMEOUT_DURATION  # microseconds

        # We check "panic" before anything else that might move the legs
        if self.crouch:
            self.zero_all_actions()
            return PROG_LOOP_TIME  # microseconds

        # Decrement turn_loops continuously
        if self.turn_loops > 0:
            self.turn_loops -= 1

        # If commander says to turn
        if self.turnleft or self.turnright:
            #if PRINT_DEBUG: print("Turn!  %u\t%u", self.turnright, self.turnleft)

            # If not currently turning, setup turning timing
            if self.turn_loops <= 0:
                self.loopLength = self.loopLengthList[THE_TURN_SPEED - 1]
                self.half_loopLength = self.loopLength / 2
                self.spdChngOffset = 0
                # Two parts: TODO 4-28 re-evaluate this
                # 1) how far 'ms' is from the beginning of a turn animation
                # 2) how far from the beginning of a turn animation we want to start at
                fractional_offset = 0.5  # used 0.2 for Numa V1
                self.turnTimeOffset = (ms % self.loopLength) - (
                    fractional_offset * self.loopLength)
                print(self.get_now(ms))

            # Make sure we continue turning for at least... 30 loops
            self.turn = True  # TODO redundant
            self.turn_loops = 30
            self.turn_dir = 1
            if self.turnright:
                self.turn_dir = -1  # Reverse turn dir here

            self.standing = 0
            self.turnright = False
            self.turnleft = False
        # Continue turning if we're already turning, until we exceed turn_loops
        elif self.turn_loops > 0:
            pass
        # Else, walking, possibly
        else:
            self.turnTimeOffset = 0

            # New walk direction; Walking forward = 0; TODO am I actually generating clockwise angles instead of cc?
            walkDIR = atan2(self.walkH, self.walkV)  # -pi to pi

            # walkSPD is an integer value; 0 or positive
            walkSPD = sqrt(self.walkV * self.walkV + self.walkH * self.walkH)
            walkSPD = int(0 + (6 - 0) * (walkSPD - 0) /
                          (102 - 0))  # interpolate(walkSPD, 0,102, 0,6)
            #print("WalkDIR:", walkDIR, "WalkSPD:", walkSPD)

            # Not walking, and not turning, so stand! We send this pose 5 times to ensure we reach the position.
            if walkSPD == 0 and self.turn_loops == 0:
                self.g8countdown = g8Stand(self.gaits, self.axbus,
                                           self.leg_ids)
                self.walk = False
                if self.standing < 6:
                    self.standing += 1
            # else: already standing, don't re-send the pose

            # Walking
            elif walkSPD > 0:
                if PRINT_DEBUG:
                    print("walk! %f ", (walkDIR * 180.0 / pi))

                # Disable turning when walking joystick is moved.
                self.turn_loops = 0  #REDUNDANT
                # Disable standing g8
                self.standing = 0

                # With this logic, we're effectively using 3 speeds
                if walkSPD > self.MAX_WALK_SPD:
                    walkSPD = self.MAX_WALK_SPD
                elif walkSPD == 1:  # or walkSPD == 2:
                    walkSPD = 2
                    #walkSPD = 3

                if USE_ONE_SPEED:
                    walkSPD = THE_ONE_SPEED

                newLoopLength = self.loopLengthList[walkSPD - 1]
                if newLoopLength != self.loopLength:  # So we can check for change
                    print("Changed from", self.loopLength, "to", newLoopLength)
                    # Note speedPhaseFix both incrs and decrs our time tracking variable, `ms`
                    self.spdChngOffset = speedPhaseFix(self.spdChngOffset, ms,
                                                       self.loopLength,
                                                       newLoopLength)
                    self.loopLength = newLoopLength
                self.walk = True
                self.set_new_heading(int(walkDIR * 180.0 / pi))
        #////////////////////////////////////////

        self.half_loopLength = self.loopLength / 2
        # These don't change currently...
        #self.travRate = 30 - 10 # this was redundant
        #self.double_travRate = 2 * self.travRate

        #//////////////////////////
        # -------- Start Leg stuff-------

        # the 'now' variables are sawtooth waves (or triangle waves???).
        now1, now2, now3, now4 = self.get_now(ms + self.spdChngOffset)

        # Above is where the commander input is interpretted.
        #
        # The next few blocks are where we determine what gait to use.

        # WALKING WITH IK via walk_code()
        #if 0:  #Disables walking
        if self.walk == True and self.turn_loops == 0:
            self.gaits.walk_code(self.loopLength, self.half_loopLength,
                                 self.travRate, self.double_travRate, now1,
                                 now2, now3, now4, self.ang_dir,
                                 self.curve_dir)
        #   #Do this in the middle of the calculations to give guns a better firing time accuracy
        #if self.guns_firing and loopStart > self.guns_firing_end_time:
        #    self.guns_firing = False
        #    self.gunMotor.direct_set_speed(GUN_SPEED_OFF)
        #    self.guns_firing_end_time = loopStart

        # Turning with IK
        elif self.turn_loops > 0 and self.walk == False:
            self.gaits.turn_code(self.turn_dir, self.loopLength,
                                 self.half_loopLength, now1, now2, now3, now4)

        elif self.standing > 0 and self.standing <= 5:
            # or g8Stand?
            self.g8countdown = g8FeetDown(self.gaits, self.axbus, self.leg_ids)

        elif self.turn_loops > 0 and self.walk == True:
            # or g8Stand?
            self.g8countdown = g8FeetDown(self.gaits, self.axbus, self.leg_ids)

        # Move all servos
        try:
            if self.walk == True or self.turn_loops > 0:
                self.axbus.sync_write(self.leg_ids, ax.GOAL_POSITION, [
                    struct.pack('<H', int(pos))
                    for pos in (self.gaits.s11pos, self.gaits.s21pos,
                                self.gaits.s31pos, self.gaits.s41pos,
                                self.gaits.s12pos, self.gaits.s22pos,
                                self.gaits.s32pos, self.gaits.s42pos,
                                self.gaits.s13pos, self.gaits.s23pos,
                                self.gaits.s33pos, self.gaits.s43pos,
                                self.gaits.s14pos, self.gaits.s24pos,
                                self.gaits.s34pos, self.gaits.s44pos)
                ])
                #print("Coax positions:",self.gaits.s11pos, self.gaits.s21pos, self.gaits.s31pos, self.gaits.s41pos)
                #print("Femur positions:",self.gaits.s12pos, self.gaits.s22pos, self.gaits.s32pos, self.gaits.s42pos)
                #print("Tibia positions:",self.gaits.s13pos, self.gaits.s23pos, self.gaits.s33pos, self.gaits.s43pos)
        except Exception:
            self.print_gait_positions()

        # TODO
        #if PRINT_IR_RANGE:
        #    IRcnt += 1
        #    if IRcnt >= 8:
        #        distanceRead(distance)
        #        print("L")
        #        distanceDump(distance)
        #        print("\t")
        #        distanceRead(distance2)
        #        print("R")
        #        distanceDump(distance2)
        #        printCRLF()
        #    IRcnt = 0


#        if PRINT_DEBUG and walk == True:
#            print("")
# elif PRINT_DEBUG_IK == True and turn == True: print("")
#elif PRINT_DEBUG_IK == True and self.turn_loops > 0:
#    print("")

        return PROG_LOOP_TIME  #45000 # microseconds
Ejemplo n.º 24
0
    def CmdrReadMsgs(self):
        while self.cmdrbus.any(
        ):  # avoid waiting for timeout when nothing to read
            byte = self.cmdrbus.read(1)
            # TODO mocking uses serial.Serial.read() which blocks... won't ever be None currently
            if byte is None:  # emptied buffer; conditional probably not needed
                break
            # process_byte will update crx with latest values from a complete packet; no need to do anything with it here
            if self.crx.process_byte(ord(byte)) == CommanderRx.SUCCESS:
                self.cmdrAlive = CMDR_ALIVE_CNT  # reset keepalive
                self.cmdrbus.read(self.cmdrbus.any())  # empty buffer
                break

        # Update variables:
        out = ""
        buttonval = self.crx.button
        if buttonval & BUT_L6:
            self.gunbutton = True
            if PRINT_DEBUG_COMMANDER: out += "guns\t"
        else: self.gunbutton = False

        if buttonval & BUT_R3:
            #self.crouchbutton = True
            #if PRINT_DEBUG_COMMANDER: out += "crouch\t"
            self.turn_mode = True
        else:
            #if PRINT_DEBUG_COMMANDER: out += "!panic\t"
            #self.crouchbutton = False
            self.turn_mode = False

        if buttonval & BUT_L4:
            self.slowturret = True
            if PRINT_DEBUG_COMMANDER: out += "fastpan\t"
            if self.cmdrAlive:
                self.laserGPIO.value(1)
            else:
                self.laserGPIO.value(0)
        else:
            self.slowturret = False
            self.laserGPIO.value(0)

        if buttonval & BUT_R2:
            self.pan_pos = PAN_CENTER
            self.tilt_pos = TILT_CENTER
            if PRINT_DEBUG_COMMANDER: out += "lookcenter\t"

        if buttonval & BUT_R1:
            self.pan_pos = PAN_CENTER
            if PRINT_DEBUG_COMMANDER: out += "lookfront\t"
        else:
            pass

        # If button is pressed (or switch is "on") disable BB Loader
        if buttonval & BUT_L5:  # and self.cmdrAlive:
            self.enable_bb_loader = False
        else:
            self.enable_bb_loader = True

        # Defaults that might change below
        dowalking = True
        self.curve_dir = 0
        self.walkV = 0
        self.walkH = 0

        if self.turn_mode:
            turnH = self.crx.walkh  #v
            dowalking = False
            if turnH < -80:  # LEFT buttonval & BUT_LT:
                self.turnleft = True
                self.turnright = False
            elif turnH > 80:  # RIGHT buttonval & BUT_RT:
                self.turnright = True
                self.turnleft = False
            elif abs(turnH) > 10:  # Curved walking
                # Walk curving from straight
                self.walkH = 0
                # Conditionals for four regions of joystick location
                if turnH > 5:
                    if self.crx.walkv > 0:
                        self.walkV = min(self.crx.walkv, 80)
                    else:
                        self.walkV = max(self.crx.walkv, -80)
                    self.curve_dir = 1
                elif turnH < -5:
                    if self.crx.walkv > 0:
                        self.walkV = min(self.crx.walkv, 80)
                    else:
                        self.walkV = max(self.crx.walkv, -80)
                    self.curve_dir = -1
            else:  # Do nothing
                self.turnright = False
                self.turnleft = False
                self.turn = False

        # Old code for turning; holding trigger buttons was unreliable in noisey environ
        #if buttonval & BUT_LT:
        #    if PRINT_DEBUG_COMMANDER: out += "tlft\t"
        #    self.turnleft = True
        #    self.turnright = False
        #    dowalking = False
        #elif buttonval & BUT_RT:
        #    if PRINT_DEBUG_COMMANDER: out += "trgt\t"
        #    self.turnright = True
        #    self.turnleft = False
        #    dowalking = False
        #else: # Do nothing
        #    self.turnright = False
        #    self.turnleft = False
        #    self.turn = False

        if dowalking:
            # Walk joystick is left joystick
            # Default handling in original Commander.c - sets to range of -127 to 127 or so...
            # vals - 128 gives look a value in the range from -128 to 127?
            self.walkV = self.crx.walkv
            self.walkH = self.crx.walkh  #v
        #else:
        #    self.walkV = 0
        #    self.walkH = 0

        # Look joystick is right joystick
        if self.slowturret:
            pan_add = int(-self.crx.lookh /
                          40)  #17) # 17 is old value from Numa1
        else:
            pan_add = int(-self.crx.lookh / 10)
        tilt_add = int(-self.crx.lookv / 25)

        self.pan_pos = clamp(self.pan_pos + pan_add, self.servo51Min,
                             self.servo51Max)
        self.tilt_pos = clamp(self.tilt_pos + tilt_add, self.servo52Min,
                              self.servo52Max)

        if out:
            print("Output:", out)
        return
Ejemplo n.º 25
0
    def run(self):
        global running
        motors = {}
        for motor_key in robot_config['motors']:
            motor = robot_config['motors'][motor_key]
            if motor['port'] == "A":
                motors["A"] = Motor(OUTPUT_A)
            if motor['port'] == "B":
                motors["B"] = Motor(OUTPUT_B)
            if motor['port'] == "C":
                motors["C"] = Motor(OUTPUT_C)
            if motor['port'] == "D":
                motors["D"] = Motor(OUTPUT_D)
            motors[motor['port']].position = 0
        motorloop = Throttler(MOTOR_CMD_RATE)

        while running:
            # run if there is client, stop otherwise.
            if len(connection_list) > 1:
                for m_key in robot_config['motors']:
                    motor = robot_config['motors'][m_key]

                    if motor['type'] == 'servo':
                        # Act like a servo, move towards the target as
                        # fast and precise as possible.
                        # the movement speed is based on the error (err)
                        # between current and target positions
                        target = scaled_gamepad_input(motor['control'],
                                                      motor['range'])
                        err = motors[motor['port']].position - target

                        # Calibrate the servo
                        if 'trim_up' in motor:
                            if all_buttons_pressed(motor['trim_up']):
                                motors[motor['port']].position += motor[
                                    'trim_step']

                        if 'trim_down' in motor:
                            if all_buttons_pressed(motor['trim_down']):
                                motors[motor['port']].position -= motor[
                                    'trim_step']

                        # if 'co_rotate' in motor:
                        #     # when the head turns horizontally, the vertical axis has turn to match
                        #     # the rotation, because the axles are concentric.
                        #     co_motor = robot_config['motors'][motor['co_rotate']]
                        #     co_position = BrickPi.Encoder[co_motor['port']] - motorPIDs[motor['co_rotate']].zero  # get rotation of co-rotational motor
                        #     co_rotation_speed = BrickPi.Encoder[co_motor['port']] - scale(gp_state[co_motor['control']],
                        #                                                                 STICK_RANGE, co_motor['range'])
                        #     err += co_position * motor['co_rotate_pos'] + co_rotation_speed * motor[
                        #         'co_rotate_speed']  # offset motor target with this number to make it move along

                        pwr = clamp(err * -1.5, (-100, 100))
                        motors[motor['port']].run_direct(duty_cycle_sp=pwr)

                    if motor['type'] == 'dc':
                        target_speed = clamp(
                            scaled_gamepad_input(motor['control'],
                                                 motor['range']), (-100, 100))
                        motors[motor['port']].run_direct(
                            duty_cycle_sp=target_speed)

                # Don't overload the brickpi too much,
                # wait a bit before next loop
                motorloop.throttle()