def _setup_switch_sources(self, robot):
#for id in ['FRONT_L', 'FRONT_R', 'BACK_L', 'BACK_R']:
# RuggeduinoController(robot, type='bump_src', id=id).write(False)
RuggeduinoController(robot, type='sensor_src',
id='TOKEN_L').write(False)
RuggeduinoController(robot, type='sensor_src',
id='TOKEN_R').write(False)
def _setup_controllers(self, robot):
self.log.debug("Setup controllers")
self.left_wheel = MotorController(robot, type='wheel', id='LEFT')
self.right_wheel = MotorController(robot, type='wheel', id='RIGHT')
self.camera = VisionController(robot)
self.grabber = ServoController(robot, type='grabber')
self.arm = ServoController(robot, type='arm')
self._setup_switch_sources(robot)
#self.bump_FrL = RuggeduinoController(robot, type='bump', id='FRONT_L')
#self.bump_FrR = RuggeduinoController(robot, type='bump', id='FRONT_R')
#self.bump_BkL = RuggeduinoController(robot, type='bump', id='BACK_L')
#self.bump_BkR = RuggeduinoController(robot, type='bump', id='BACK_R')
self.token_sensL = RuggeduinoController(robot, type='sensor', id='TOKEN_L')
self.token_sensR = RuggeduinoController(robot, type='sensor', id='TOKEN_R')
def _setup_controllers(self, robot):
self.log.debug("Setup controllers")
self.left_wheel = MotorController(robot, type='wheel', id='LEFT')
self.right_wheel = MotorController(robot, type='wheel', id='RIGHT')
self.camera = VisionController(robot)
self.grabber = ServoController(robot, type='grabber')
self.arm = ServoController(robot, type='arm')
self._setup_switch_sources(robot)
#self.bump_FrL = RuggeduinoController(robot, type='bump', id='FRONT_L')
#self.bump_FrR = RuggeduinoController(robot, type='bump', id='FRONT_R')
#self.bump_BkL = RuggeduinoController(robot, type='bump', id='BACK_L')
#self.bump_BkR = RuggeduinoController(robot, type='bump', id='BACK_R')
self.token_sensL = RuggeduinoController(robot,
type='sensor',
id='TOKEN_L')
self.token_sensR = RuggeduinoController(robot,
type='sensor',
id='TOKEN_R')
class IOInterface(object):
def __init__(self, Robot):
self.log = logging.getLogger('Robot.IO')
self.log.info("Initializing")
# Aliases for English readability
self.drive = self.turn = self._move
try:
self._setup_controllers(Robot)
except:
self.log.critical("Could not set-up hardware controller")
raise
try:
self.running = True
tr = ThreadRegister(self)
#self.bump_thread = BumpThread()
self._bump_callback = lambda s: None
self._marker_callback = lambda m: None
#tr.add_thread(self.bump_thread)
#self.bump_thread.start()
except Exception as e:
self.log.critical("Unable to start threads")
self.log.exception(e)
raise
self.log.info("Done")
def _setup_controllers(self, robot):
self.log.debug("Setup controllers")
self.left_wheel = MotorController(robot, type='wheel', id='LEFT')
self.right_wheel = MotorController(robot, type='wheel', id='RIGHT')
self.camera = VisionController(robot)
self.grabber = ServoController(robot, type='grabber')
self.arm = ServoController(robot, type='arm')
self._setup_switch_sources(robot)
#self.bump_FrL = RuggeduinoController(robot, type='bump', id='FRONT_L')
#self.bump_FrR = RuggeduinoController(robot, type='bump', id='FRONT_R')
#self.bump_BkL = RuggeduinoController(robot, type='bump', id='BACK_L')
#self.bump_BkR = RuggeduinoController(robot, type='bump', id='BACK_R')
self.token_sensL = RuggeduinoController(robot, type='sensor', id='TOKEN_L')
self.token_sensR = RuggeduinoController(robot, type='sensor', id='TOKEN_R')
def _setup_switch_sources(self, robot):
#for id in ['FRONT_L', 'FRONT_R', 'BACK_L', 'BACK_R']:
# RuggeduinoController(robot, type='bump_src', id=id).write(False)
RuggeduinoController(robot, type='sensor_src', id='TOKEN_L').write(False)
RuggeduinoController(robot, type='sensor_src', id='TOKEN_R').write(False)
def _move(self, instruction):
if not isinstance(instruction, MoveInstruction):
raise TypeError("Invalid movement instruction")
self.log.debug("Do movement %s", instruction)
instruction.action(self)
def goto_marker(self, marker, speed, comparator=None, offset=0):
self.log.debug("goto marker %s %.1f", marker, speed)
if comparator is None or not hasattr(comparator, '__call__'):
comparator = lambda a, b: a.info.code == b.info.code
reached = False
blind_spot = 0.66
while True: # Alternatively, on each new find, call goto_marker within
horiz_dist = Marker(marker).horizontal_dist - offset
travel_dist = horiz_dist / 2 # set dist to travel half the measured
if marker.rot_y < 0:
Dir = Left # negative rot_y indicates a left turn
else:
Dir = Right # positive rot_y means right turn
if not reached: # if reached flag is true then should not rotate
degree = marker.rot_y
if degree < 1: degree += 0.7 # rotation < 1 is too subtle
if degree < 2: degree *= 2 # amplify rotation
self.turn(Dir(degree, Speed(speed / 3))) # face marker
self.drive(Distance(travel_dist, speed)) # drive half way
if reached: # set below
self.log.info("Reached marker")
break
potential_markers = [] # new markers of the same type stored here
for new_marker in self.get_markers():
if comparator(new_marker, marker):
potential_markers.append(new_marker) # append based on comparator
closest = self.get_closest_marker(potential_markers) # closest new
if closest is None: # there are no markers found
if travel_dist < blind_spot: # we have traveled beyond blind_spot
self.log.info("Distance to go in blind_spot (%.4f)",
travel_dist)
reached = True # break on next loop
else:
self.log.warning("Lost the marker")
break # lost the marker
else:
self.log.debug("Re-found marker")
c = Marker(closest)
expected = horiz_dist - min(0.1, travel_dist / 3.0)
if c.horizontal_dist >= expected:
self.log.warning("Did not travel far enough (%f >= %f) (%f, %f)" % (
c.horizontal_dist, expected, closest.dist, marker.dist))
return 0
marker = closest # the new marker to target
if not reached:
if marker.info.marker_type in MarkerFilter.include("TOKENS"):
reached = self.is_holding_token()
self.log.debug("Not reached, holding:%s", reached)
if reached: break
return reached
def face_marker(self, marker, speed):
self.log.debug("Going to face marker %s", marker)
m_orient = abs(marker.orientation.rot_y)
if m_orient < 0:
Dir1, Dir2 = Left, Right
self.log.debug("Going left then right")
else:
Dir1, Dir2 = Right, Left
self.log.debug("Going right then left")
self.turn(Dir1(abs(marker.rot_y), Speed(speed)))
self.drive(Distance((0.5 * marker.dist) /
abs(math.cos(math.radians(m_orient))), speed))
self.turn(Dir2(3 * m_orient, Speed(speed)))
def face_marker_2(self, marker, speed):
theta = abs(marker.orientation.rot_y)
length = marker.dist
if theta < 0:
Dir1, Dir2 = Left, Right
self.log.debug("Going left then right")
else:
Dir1, Dir2 = Right, Left
self.log.debug("Going right then left")
self.turn(Dir1(2 * theta, Speed(speed)))
x = (length / 2) * math.cos(math.radians(theta))
self.drive(Distance(x, speed))
angle = 2 * theta
self.turn(Dir2(angle, Speed(speed)))
self.drive(Distance(x, speed))
def face_marker_3(self, marker, speed):
self.log.debug("Going to face marker %s", marker)
m_orient = abs(marker.orientation.rot_y)
if m_orient < 0:
Dir1, Dir2 = Left, Right
self.log.debug("Going left then right")
else:
Dir1, Dir2 = Right, Left
self.log.debug("Going right then left")
self.turn(Dir1(abs(marker.rot_y), Speed(speed)))
self.drive(Distance((0.7 * marker.dist) /
abs(math.cos(math.radians(m_orient))), speed))
self.turn(Dir2(2 * m_orient, Speed(speed)))
def navigate_to_marker(self, marker, speed, comparator=None):
self.log.debug("navigating to marker")
self.face_marker_3(marker, speed) # face the marker
markers = self.get_markers() # rescan for the marker
old_marker = marker
for new_marker in markers:
if new_marker.info.code == marker.info.code:
marker = new_marker
self.log.debug("Found marker, continuing")
break
if marker is old_marker: # the marker got lost
self.log.warning("Could not find marker")
return False
if self.goto_marker(marker, speed, comparator):
return True
markers = self.get_markers() # rescan for the marker
for new_marker in markers:
if new_marker.info.code == marker.info.code:
marker = new_marker
self.log.debug("Found marker again, continuing")
break
if marker is old_marker: # the marker got lost
self.log.warning("Could not find marker")
return False
self.navigate_to_marker(marker, speed, comparator)
def is_holding_token(self):
return self.token_sensL.read() or self.token_sensR.read()
def move_arm(self, direction, delay=False):
if direction not in [UP, DOWN, MIDDLE]:
raise TypeError("Invalid arm movement")
if direction == UP:
angle = self.arm.MIN
elif direction == DOWN:
angle = self.arm.MAX
elif direction == MIDDLE:
angle = self.arm.MAX / 2
self.log.debug("Move arm %s (angle = %d)", direction, angle)
self.arm.set_angle(angle)
if delay:
self.wait(0.5)
def open_grabber(self, delay=False):
self.log.debug("OPEN grabber (angle = %d)", self.grabber.MIN)
self.grabber.set_angle(self.grabber.MIN)
if delay:
self.wait(0.7)
def close_grabber(self, delay=False):
self.log.debug("CLOSE grabber (angle = %d)", self.grabber.MAX)
self.grabber.set_angle(self.grabber.MAX)
if delay:
self.wait(0.7)
def get_markers(self, _filter=None):
markers = self.camera.find_markers()
self._marker_callback(markers)
self.log.debug("get_markers %s", str(self.camera.fmt_markers(markers)))
if _filter is not None:
markers = filter(lambda m: m.info.marker_type in _filter, markers)
return markers
def get_closest_marker(self, markers):
self.log.debug("Get closest marker to robot")
return self.camera.get_closest(markers)
def get_closest_marker_rotation(self, markers, degree=0):
self.log.debug("Get closest marker from degree (%f)", degree)
return self.camera.get_rotation_nearest(markers, degree)
def _stop_operation(self, filter=None):
self.log.info("Stopping current action")
try:
self.right_wheel.stop()
self.left_wheel.stop()
except Exception as e:
self.log.exception(e)
raise StateInterrupt('stop', 'operation.stop')
def wait(self, seconds):
self.log.debug("Wait %.4f seconds", seconds)
time.sleep(seconds)
def set_bump_handler(self, callback):
self._bump_callback = callback
def set_marker_handler(self, callback):
self._marker_callback = callback
def bumped(self, sensor):
if sensor is self.bump_FrL:
self._bump_callback('Front.Left')
elif sensor is self.bump_FrR:
self._bump_callback('Front.Right')
elif sensor is self.bump_BkL:
self._bump_callback('Back.Left')
elif sensor is self.bump_BkR:
self._bump_callback('Back.Right')
def create_token_obj(self, markers):
return Token(markers)
def stop(self):
self.log.info("Stopping all communications")
self.running = False
class IOInterface(object):
def __init__(self, Robot):
self.log = logging.getLogger('Robot.IO')
self.log.info("Initializing")
# Aliases for English readability
self.drive = self.turn = self._move
try:
self._setup_controllers(Robot)
except:
self.log.critical("Could not set-up hardware controller")
raise
try:
self.running = True
tr = ThreadRegister(self)
#self.bump_thread = BumpThread()
self._bump_callback = lambda s: None
self._marker_callback = lambda m: None
#tr.add_thread(self.bump_thread)
#self.bump_thread.start()
except Exception as e:
self.log.critical("Unable to start threads")
self.log.exception(e)
raise
self.log.info("Done")
def _setup_controllers(self, robot):
self.log.debug("Setup controllers")
self.left_wheel = MotorController(robot, type='wheel', id='LEFT')
self.right_wheel = MotorController(robot, type='wheel', id='RIGHT')
self.camera = VisionController(robot)
self.grabber = ServoController(robot, type='grabber')
self.arm = ServoController(robot, type='arm')
self._setup_switch_sources(robot)
#self.bump_FrL = RuggeduinoController(robot, type='bump', id='FRONT_L')
#self.bump_FrR = RuggeduinoController(robot, type='bump', id='FRONT_R')
#self.bump_BkL = RuggeduinoController(robot, type='bump', id='BACK_L')
#self.bump_BkR = RuggeduinoController(robot, type='bump', id='BACK_R')
self.token_sensL = RuggeduinoController(robot,
type='sensor',
id='TOKEN_L')
self.token_sensR = RuggeduinoController(robot,
type='sensor',
id='TOKEN_R')
def _setup_switch_sources(self, robot):
#for id in ['FRONT_L', 'FRONT_R', 'BACK_L', 'BACK_R']:
# RuggeduinoController(robot, type='bump_src', id=id).write(False)
RuggeduinoController(robot, type='sensor_src',
id='TOKEN_L').write(False)
RuggeduinoController(robot, type='sensor_src',
id='TOKEN_R').write(False)
def _move(self, instruction):
if not isinstance(instruction, MoveInstruction):
raise TypeError("Invalid movement instruction")
self.log.debug("Do movement %s", instruction)
instruction.action(self)
def goto_marker(self, marker, speed, comparator=None, offset=0):
self.log.debug("goto marker %s %.1f", marker, speed)
if comparator is None or not hasattr(comparator, '__call__'):
comparator = lambda a, b: a.info.code == b.info.code
reached = False
blind_spot = 0.66
while True: # Alternatively, on each new find, call goto_marker within
horiz_dist = Marker(marker).horizontal_dist - offset
travel_dist = horiz_dist / 2 # set dist to travel half the measured
if marker.rot_y < 0:
Dir = Left # negative rot_y indicates a left turn
else:
Dir = Right # positive rot_y means right turn
if not reached: # if reached flag is true then should not rotate
degree = marker.rot_y
if degree < 1: degree += 0.7 # rotation < 1 is too subtle
if degree < 2: degree *= 2 # amplify rotation
self.turn(Dir(degree, Speed(speed / 3))) # face marker
self.drive(Distance(travel_dist, speed)) # drive half way
if reached: # set below
self.log.info("Reached marker")
break
potential_markers = [] # new markers of the same type stored here
for new_marker in self.get_markers():
if comparator(new_marker, marker):
potential_markers.append(
new_marker) # append based on comparator
closest = self.get_closest_marker(potential_markers) # closest new
if closest is None: # there are no markers found
if travel_dist < blind_spot: # we have traveled beyond blind_spot
self.log.info("Distance to go in blind_spot (%.4f)",
travel_dist)
reached = True # break on next loop
else:
self.log.warning("Lost the marker")
break # lost the marker
else:
self.log.debug("Re-found marker")
c = Marker(closest)
expected = horiz_dist - min(0.1, travel_dist / 3.0)
if c.horizontal_dist >= expected:
self.log.warning(
"Did not travel far enough (%f >= %f) (%f, %f)" %
(c.horizontal_dist, expected, closest.dist,
marker.dist))
return 0
marker = closest # the new marker to target
if not reached:
if marker.info.marker_type in MarkerFilter.include("TOKENS"):
reached = self.is_holding_token()
self.log.debug("Not reached, holding:%s", reached)
if reached: break
return reached
def face_marker(self, marker, speed):
self.log.debug("Going to face marker %s", marker)
m_orient = abs(marker.orientation.rot_y)
if m_orient < 0:
Dir1, Dir2 = Left, Right
self.log.debug("Going left then right")
else:
Dir1, Dir2 = Right, Left
self.log.debug("Going right then left")
self.turn(Dir1(abs(marker.rot_y), Speed(speed)))
self.drive(
Distance(
(0.5 * marker.dist) / abs(math.cos(math.radians(m_orient))),
speed))
self.turn(Dir2(3 * m_orient, Speed(speed)))
def face_marker_2(self, marker, speed):
theta = abs(marker.orientation.rot_y)
length = marker.dist
if theta < 0:
Dir1, Dir2 = Left, Right
self.log.debug("Going left then right")
else:
Dir1, Dir2 = Right, Left
self.log.debug("Going right then left")
self.turn(Dir1(2 * theta, Speed(speed)))
x = (length / 2) * math.cos(math.radians(theta))
self.drive(Distance(x, speed))
angle = 2 * theta
self.turn(Dir2(angle, Speed(speed)))
self.drive(Distance(x, speed))
def face_marker_3(self, marker, speed):
self.log.debug("Going to face marker %s", marker)
m_orient = abs(marker.orientation.rot_y)
if m_orient < 0:
Dir1, Dir2 = Left, Right
self.log.debug("Going left then right")
else:
Dir1, Dir2 = Right, Left
self.log.debug("Going right then left")
self.turn(Dir1(abs(marker.rot_y), Speed(speed)))
self.drive(
Distance(
(0.7 * marker.dist) / abs(math.cos(math.radians(m_orient))),
speed))
self.turn(Dir2(2 * m_orient, Speed(speed)))
def navigate_to_marker(self, marker, speed, comparator=None):
self.log.debug("navigating to marker")
self.face_marker_3(marker, speed) # face the marker
markers = self.get_markers() # rescan for the marker
old_marker = marker
for new_marker in markers:
if new_marker.info.code == marker.info.code:
marker = new_marker
self.log.debug("Found marker, continuing")
break
if marker is old_marker: # the marker got lost
self.log.warning("Could not find marker")
return False
if self.goto_marker(marker, speed, comparator):
return True
markers = self.get_markers() # rescan for the marker
for new_marker in markers:
if new_marker.info.code == marker.info.code:
marker = new_marker
self.log.debug("Found marker again, continuing")
break
if marker is old_marker: # the marker got lost
self.log.warning("Could not find marker")
return False
self.navigate_to_marker(marker, speed, comparator)
def is_holding_token(self):
return self.token_sensL.read() or self.token_sensR.read()
def move_arm(self, direction, delay=False):
if direction not in [UP, DOWN, MIDDLE]:
raise TypeError("Invalid arm movement")
if direction == UP:
angle = self.arm.MIN
elif direction == DOWN:
angle = self.arm.MAX
elif direction == MIDDLE:
angle = self.arm.MAX / 2
self.log.debug("Move arm %s (angle = %d)", direction, angle)
self.arm.set_angle(angle)
if delay:
self.wait(0.5)
def open_grabber(self, delay=False):
self.log.debug("OPEN grabber (angle = %d)", self.grabber.MIN)
self.grabber.set_angle(self.grabber.MIN)
if delay:
self.wait(0.7)
def close_grabber(self, delay=False):
self.log.debug("CLOSE grabber (angle = %d)", self.grabber.MAX)
self.grabber.set_angle(self.grabber.MAX)
if delay:
self.wait(0.7)
def get_markers(self, _filter=None):
markers = self.camera.find_markers()
self._marker_callback(markers)
self.log.debug("get_markers %s", str(self.camera.fmt_markers(markers)))
if _filter is not None:
markers = filter(lambda m: m.info.marker_type in _filter, markers)
return markers
def get_closest_marker(self, markers):
self.log.debug("Get closest marker to robot")
return self.camera.get_closest(markers)
def get_closest_marker_rotation(self, markers, degree=0):
self.log.debug("Get closest marker from degree (%f)", degree)
return self.camera.get_rotation_nearest(markers, degree)
def _stop_operation(self, filter=None):
self.log.info("Stopping current action")
try:
self.right_wheel.stop()
self.left_wheel.stop()
except Exception as e:
self.log.exception(e)
raise StateInterrupt('stop', 'operation.stop')
def wait(self, seconds):
self.log.debug("Wait %.4f seconds", seconds)
time.sleep(seconds)
def set_bump_handler(self, callback):
self._bump_callback = callback
def set_marker_handler(self, callback):
self._marker_callback = callback
def bumped(self, sensor):
if sensor is self.bump_FrL:
self._bump_callback('Front.Left')
elif sensor is self.bump_FrR:
self._bump_callback('Front.Right')
elif sensor is self.bump_BkL:
self._bump_callback('Back.Left')
elif sensor is self.bump_BkR:
self._bump_callback('Back.Right')
def create_token_obj(self, markers):
return Token(markers)
def stop(self):
self.log.info("Stopping all communications")
self.running = False