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())
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
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
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
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)
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
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)
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
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
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))
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()
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()
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()
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)
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()
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()
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()
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)
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)
def get_wire_width(self):
w = round(DEFAULT_WIRE_WIDTH * self.zoom_level)
return clamp(w, MIN_GRID_LINE_WIDTH, MAX_GRID_LINE_WIDTH)
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)
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
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
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()