def move_torso(self, left_speed, right_speed, milliseconds):
"""
Move the torso by a certain speed for a certain amount of time.
"""
assert -255 <= left_speed <= 255
assert -255 <= right_speed <= 255
assert milliseconds > 0
# Tell the state machine to expect a start in motion.
trigger_up = self.state.set_or_trigger(
torso_arduino_motor_target_a=True,
torso_arduino_motor_target_b=True)
# Tell the state machine to expect a drop in motion.
trigger_down = self.state.set_or_trigger(
torso_arduino_motor_target_a=False,
torso_arduino_motor_target_b=False,
dependencies=[trigger_up])
# publish('/torso_arduino/motor_speed', [64, 64, 500])
publish('/torso_arduino/motor_speed',
[left_speed, right_speed, milliseconds])
rospy.loginfo('Waiting for motors to start...')
trigger_up.wait()
rospy.loginfo('Waiting for motors to stop...')
try:
trigger_down.wait()
except TriggerTimeout:
pass
def rotate_head_relative(self, degrees, threshold=3, timeout=15):
"""
Rotates the head to an angle relative to the current position.
"""
deg0 = self.state.head_arduino_pan_degrees
deg_abs_target = (deg0 + degrees) % 360
for _ in range(timeout):
if self.shutting_down:
return
t0 = time.time()
dist = self.state.head_arduino_pan_degrees - deg0
if degrees < 0:
# Turning CCW.
if dist > 180:
dist -= 360
else:
# Turning CW.
if dist < -180:
dist += 360
change = abs(dist - degrees)
if change <= threshold:
return
publish('/head_arduino/pan_set', deg_abs_target)
# Wait until we get an updated pan position.
self.state.get_since_until('head_arduino_pan_degrees', t=t0)
raise Exception('Unable to rotate head by %s degrees.' % degrees)
def rotate_torso(self, degrees, double_down=False):
"""
Rotates the torso by the given degrees.
double_down := If true, and the motors fail to start, increase the degrees and try again.
"""
degrees = int(degrees)
if not degrees:
rospy.logwarn('Attempting to rotate torso by 0 degrees.')
return
# imu_euler_z0 = self.state.torso_arduino_imu_euler_z
# print('torso.imu_euler_z0:', imu_euler_z0)
trigger_up = None
trigger_down = None
for _ in range(10):
if self.shutting_down:
return
try:
# Tell the state machine to expect a start in motion.
trigger_up = self.state.set_or_trigger(
torso_arduino_motor_target_a=True,
torso_arduino_motor_target_b=True)
# Tell the state machine to expect a drop in motion.
trigger_down = self.state.set_or_trigger(
torso_arduino_motor_target_a=False,
torso_arduino_motor_target_b=False,
dependencies=[trigger_up])
publish('/torso_arduino/motor_rotate', degrees)
rospy.loginfo('Waiting for rotation to start...')
trigger_up.wait()
rospy.loginfo('Rotation started.')
rospy.loginfo('Waiting for rotation to stop...')
try:
trigger_down.wait(timeout=5)
rospy.loginfo('Rotation stopped.')
except TriggerTimeout:
# If we timeout waiting for the motor to stop, that's likely because it already has and even our trigger wasn't fast enough to catch it,
# so ignore the timeout.
rospy.logerr('Rotation stop timed out!')
break
except TriggerTimeout:
# A timeout occurred waiting for the motors to start.
# This either means the degree was too small to cause a change, or the motors have stalled.
if double_down:
degrees *= 2
degrees = max(min(degrees, 359), -359)
else:
raise
except TorsoMotorError as exc:
rospy.logerr('Error rotating torso: %s', exc)
traceback.print_exc()
# Cleanup triggers.
if trigger_up:
trigger_up.destroy()
trigger_up = None
if trigger_down:
trigger_down.destroy()
trigger_down = None
def read_dht():
try:
dht.measure()
except Exception as e:
common.log('Failed to get DHT measurements: %s' % e)
return
common.publish(TEMPTOPIC, "%.2f" % dht.temperature())
common.publish(HUMTOPIC, "%.2f" % dht.humidity())
def setup_conditional(self, step, condition_type, condition, cond_value):
r'that a course has a Conditional conditioned on (?P<condition_type>\w+) (?P<condition>\w+)=(?P<cond_value>\w+)$'
i_am_registered_for_the_course(step, self.COURSE_NUM)
world.scenario_dict['VERTICAL'] = world.ItemFactory(
parent_location=world.scenario_dict['SECTION'].location,
category='vertical',
display_name="Test Vertical",
)
world.scenario_dict['WRAPPER'] = world.ItemFactory(
parent_location=world.scenario_dict['VERTICAL'].location,
category='wrapper',
display_name="Test Poll Wrapper"
)
if condition_type == 'problem':
world.scenario_dict['CONDITION_SOURCE'] = add_problem_to_course(self.COURSE_NUM, 'string')
elif condition_type == 'poll':
world.scenario_dict['CONDITION_SOURCE'] = world.ItemFactory(
parent_location=world.scenario_dict['WRAPPER'].location,
category='poll_question',
display_name='Conditional Poll',
data={
'question': 'Is this a good poll?',
'answers': [
{'id': 'yes', 'text': 'Yes, of course'},
{'id': 'no', 'text': 'Of course not!'}
],
}
)
else:
raise Exception("Unknown condition type: {!r}".format(condition_type))
metadata = {
'xml_attributes': {
condition: cond_value
}
}
world.scenario_dict['CONDITIONAL'] = world.ItemFactory(
parent_location=world.scenario_dict['WRAPPER'].location,
category='conditional',
display_name="Test Conditional",
metadata=metadata,
sources_list=[world.scenario_dict['CONDITION_SOURCE'].location],
)
world.ItemFactory(
parent_location=world.scenario_dict['CONDITIONAL'].location,
category='html',
display_name='Conditional Contents',
data='<html><div class="hidden-contents">Hidden Contents</p></html>'
)
publish(world.scenario_dict['VERTICAL'].location)
def force_sensors(self):
"""
Tells the embedded controllers to give us a full update of all sensor states, so we can update our local cache.
"""
for _ in range(10):
rospy.loginfo('Forcing sensors...')
publish('/torso_arduino/force_sensors')
if self.state.torso_arduino_power_external_0 is not None \
and self.state.torso_arduino_power_external_1 is not None \
and self.state.torso_arduino_imu_euler_z is not None:
rospy.loginfo('Force sensors refreshed everything.')
return
time.sleep(1)
rospy.logwarn('Force sensors did not everything!')
def on_shutdown(self):
"""
Called when the ROS master signals a shutdown, or our parent signals a cancellation of the docking process.
"""
self.shutting_down = True
self.announce_status('Shutting down.')
publish('/head_arduino/ultrabright_set', 0)
self.halt_motors()
rospy.loginfo('Shutting down...')
if self.qr_tracker_process:
self.qr_tracker_process.stop()
if has_camera():
cmd = 'export ROS_MASTER_URI=%s; rosnode kill /raspicam/raspicam_node' % ROS_MASTER_URI
print('cmd:', cmd)
os.system(cmd)
self.set_pan_freeze(0)
rospy.loginfo('Done.')
self.shut_down = True
def rotate_head_absolute(self,
degrees,
threshold=3,
timeout=15,
forgive=False):
"""
Rotates the head to an absolute angle.
"""
for _ in range(timeout):
if self.shutting_down:
return
print('self.state.head_arduino_pan_degrees:',
self.state.head_arduino_pan_degrees)
if abs(
normalize_angle_change(self.state.head_arduino_pan_degrees,
degrees)) <= threshold:
return
publish('/head_arduino/pan_set', degrees)
time.sleep(1)
if not forgive:
raise Exception('Unable to pan head to angle %s.' % degrees)
def set_pan_freeze(self, value):
"""
Ensures the head's pan_freeze flag is set.
rostopic pub --once /head_arduino/pan_freeze_set std_msgs/Bool 1
rostopic echo /head_arduino/pan_freeze_get
"""
if value:
rospy.loginfo('Ensuring pan angle is set.')
publish('/head_arduino/pan_set',
self.state.head_arduino_pan_degrees)
for _ in range(10):
if self.shutting_down:
return
if self.state.head_arduino_pan_freeze_get == value:
return
publish('/head_arduino/pan_freeze_set', value)
rospy.loginfo('Waiting for head_pan_freeze_get to be set to %s.',
value)
time.sleep(0.5)
raise Exception('Unable to set head_pan_freeze_get to %s.' % value)
def define_component(self, step, count):
r"""that a course has an annotatable component with (?P<count>\d+) annotations$"""
count = int(count)
coursenum = 'test_course'
i_am_registered_for_the_course(step, coursenum)
world.scenario_dict['ANNOTATION_VERTICAL'] = world.ItemFactory(
parent_location=world.scenario_dict['SECTION'].location,
category='vertical',
display_name="Test Annotation Vertical"
)
world.scenario_dict['ANNOTATABLE'] = world.ItemFactory(
parent_location=world.scenario_dict['ANNOTATION_VERTICAL'].location,
category='annotatable',
display_name="Test Annotation Module",
data=DATA_TEMPLATE.format("\n".join(ANNOTATION_TEMPLATE.format(i) for i in xrange(count)))
)
publish(world.scenario_dict['ANNOTATION_VERTICAL'].location)
self.annotations_count = count
def add_problems(self, step, count):
r"""the course has (?P<count>\d+) annotatation problems$"""
count = int(count)
for i in xrange(count):
world.scenario_dict.setdefault('PROBLEMS', []).append(
world.ItemFactory(
parent_location=world.scenario_dict['ANNOTATION_VERTICAL'].location,
category='problem',
display_name="Test Annotation Problem {}".format(i),
data=PROBLEM_TEMPLATE.format(
number=i,
options="\n".join(
OPTION_TEMPLATE.format(
number=k,
correctness=_correctness(k, i)
)
for k in xrange(count)
)
)
)
)
publish(world.scenario_dict['ANNOTATION_VERTICAL'].location)
def center_qr_code_using_head(self):
"""
Rotates the head until the QR code is roughly in the center of view.
"""
# Note, our motors don't have good precision, and get stuck at small intervals, so the most we can adjust by are in 10 degree increments.
angle_of_adjustment = 10
# Increase angle => rotate CW
# Decreate angle => rotate CCW
if not self.state.qr_tracker_matches:
rospy.logerr('Cannot center QR code. None found.')
return
for _ in range(10):
qr_matches = self.state.qr_tracker_matches
x = (qr_matches.a[0] + qr_matches.b[0] + qr_matches.c[0] +
qr_matches.d[0]) / 4.
# y = (self.state.qr_matches.a[1] + self.state.qr_matches.b[1] + self.state.qr_matches.c[1] + self.state.qr_matches.d[1])/4.
width = qr_matches.width
# height = qr_matches.height
# Ideally, this should be 0.5, indicating the QR code is exactly in the center of the horizontal view.
# In practice, we'll likely never accomplish this, but we'll try to get it within [0.4:0.6]
position_ratio = x / float(width)
rospy.loginfo('position_ratio: %s', position_ratio)
if 0.4 <= position_ratio <= 0.6:
break
elif position_ratio < 0.4:
# QR code is too far left of screen, so move head counter-clockwise to center it.
new_angle = (self.state.head_arduino_pan_degrees -
angle_of_adjustment) % 360
publish('/head_arduino/pan_set', new_angle)
elif position_ratio > 0.6:
# QR code is too far right of screen, so move head clockwise to center it.
new_angle = (self.state.head_arduino_pan_degrees +
angle_of_adjustment) % 360
publish('/head_arduino/pan_set', new_angle)
rospy.loginfo('new angle: %s', new_angle)
time.sleep(1)
def center_head(self):
"""
Causes the head to pan to its center position.
"""
publish('/head_arduino/tilt_set', 90)
time.sleep(2)
publish('/head_arduino/pan_set', 315)
time.sleep(2)
publish('/head_arduino/pan_set', 356)
time.sleep(2)
def halt_motors(self):
"""
Tells our motors to immediately stop.
"""
publish('/torso_arduino/halt') # std_msgs/Empty
def main():
# Set basic logging configuration
if cfg.settings['debug'] == 1:
logging.basicConfig(level=logging.DEBUG,
format="%(asctime)s - [%(levelname)s] [%(threadName)s] (%(module)s:%(lineno)d) %(message)s",
filename=cfg.logfile)
elif cfg.settings['debug'] == 0:
logging.basicConfig(level=logging.INFO,
format="%(asctime)s - [%(levelname)s] [%(threadName)s] (%(module)s:%(lineno)d) %(message)s",
filename=cfg.logfile)
else:
logging.basicConfig(level=logging.DEBUG,
format="%(asctime)s - [%(levelname)s] (%(module)s:%(lineno)d) %(message)s")
# Before starting everything, make sure that the cache and log directories are available
# since these are critical to operation
if os.path.exists(cfg.logs['logdir']) != True or os.path.exists(cfg.logs['cachedir']) != True:
logging.warn('unable to write to logfile')
return
# Send initial information to the logfile to facilitate
logging.info('=================================================================')
logging.info('Started processing at ' + time.strftime("%Y-%m-%d %H:%M:%S"))
logging.info('Sensor readings collected every ' + str(me['wait']) + ' seconds')
logging.info('Currently configured sensors:')
# Gather information on configured sensors and log
sensors = {
'active': 0,
'total': 0
}
for item_i in cfg.sensors:
set_i = item_i['settings']
attr_i = item_i['attr']
sensors['total'] = sensors['total'] + 1
# Gather the information that is set per sensor, and log the status of the environment to logfile
sens_state = 'ID: %s - "%s": Active: %s, LocalOnly: %s, Cache on Error: %s, Include SysInfo: %s, Clear cache: %s' % \
(item_i['id'],
attr_i['name'],
str(set_i['active']),
str(set_i['localonly']),
str(set_i['cache_on_err']),
str(set_i['sys_info']),
str(set_i['clearcache'])
)
if set_i['active'] == 1:
sensors['active'] = sensors['active'] + 1
logging.info(sens_state)
logging.info('Total Sensors Configured: ' + str(sensors['total']) + ', Active: '+str(sensors['active']))
# Run through the sensor information, and process where configured as "active'
while True:
logging.debug('Running sensor poll')
com.chk_cache()
logging.debug('Checking cache status')
for item in cfg.sensors:
message = {}
set = item['settings']
# If the sensor is configured to clear exsiting cache, check and run
if set['clearcache'] == 1:
logging.debug('Preparing to clear cache files')
com.clear_cache(item['authkey'])
# Check to see if the device is configured to be active - if not, then skip
if set['active'] == 1:
attr = item['attr']
tele = item['tele']
# Get system information (CPU, Ram, etc) if configured
if set['sys_info'] == 1:
sys_info = com.read_sys_stats()
for key, value in sys_info['attr'].items():
attr[key] = value
for key, value in sys_info['tele'].items():
message[key] = value
# Gather sensor data and add to the telemetry data
conditions = com.read_sensor(tele['device'],tele['type'],tele['label'])
# Since not all sensors will not add attributes, if there are none returned, then continue
try:
for key, value in conditions['attr'].items():
attr[key] = value
except:
None
for key, value in conditions['tele'].items():
message[key] = value
pub_status = com.publish(attr,message,item['authkey'],set['cache_on_err'],set['localonly'])
time.sleep(me['sleep_poll'])
logging.debug('Completed sensor poll')
time.sleep(me['wait'])
def set_light(on, value):
light_pwm.duty(on * value)
common.publish(PUBTOPIC_ON, str(on))
common.publish(PUBTOPIC_VALUE, str(value))
def set_pin(pin, state):
common.log('Setting pin to %s' % state)
pin(state)
common.publish(PUBTOPIC, str(pin()))
def undock(self):
publish('/torso_arduino/undock') # std_msgs/Empty
def read_desk():
d.read_height(lambda height: common.publish(PUBTOPIC, str(int(height))))
def set_channel_state(channel, state):
if state:
s.on(channel)
else:
s.off(channel)
common.publish(LAMP_PUBTOPIC+b"/%s" % channel, str(state))
def dock_iter(self):
"""
This is the main docking procedure.
This will stop iterating once the docking procedure has succeeded or reached a failure condition.
"""
t0 = time.time()
try:
self.announce_status('Beginning docking procedure.')
# self.announce_status('Checking current state.')
if self.is_docked():
self.announce_status('We are already docked. Nothing to do.')
return
# if self.is_dead_docked():
# self.announce_status('We sense the docking magnet, but no power, possibly from a previous failed docking attempt.')
# self.announce_status('Undocking to abort last attempt and retry.')
# self.undock()
# yield
# time.sleep(5)
# yield
# Turn on ultrabrights to help us see the QR code.
publish('/head_arduino/ultrabright_set', 254)
time.sleep(2)
# for _ in range(10):
# print('rotate:', _)
# self.rotate_torso(degrees=2, double_down=True)
# time.sleep(3)
# return
if not self.qr_code_found():
raise Terminate('QR code not found.') #TODO:remove
# self.find_qr_code()
# Ensure head is centered. Since there's too much friction to move it by only a couple degrees when we're already close to 0,
# ensure it's centered by wiggling it back and forth.
self.rotate_head_absolute(20, forgive=True)
self.rotate_head_absolute(360 - 20, forgive=True)
self.rotate_head_absolute(0)
self.center_qr_code_using_torso()
self.move_forward_until_docked()
return
# self.announce_status('Centering QR code in view.')
# self.center_qr_code_using_head()
# yield
# self.announce_status('View centered.')
# self.announce_status('Freezing head angle.')
# self.set_pan_freeze(1)
# yield
# self.announce_status('Angling body to be perpendicular to QR code.')
# # Calculate torso angle rotation needed so that the front faces inward by 90 degrees to the head position.
# # Note, a positive degree proceeds CW when looking down.
# # e.g. if we're facing -10 degrees to the left == 350 degrees, then we need to rotate the torso by -10 degrees to center,
# # and then another -90 degrees to be perpendicular.
# #TODO:estimate angle of offset of QR code
# qr_tracker_matches = None
# while 1:
# if self.state.qr_tracker_matches:
# qr_tracker_matches = self.state.qr_tracker_matches
# break
# rospy.loginfo('Waiting for QR match...')
# time.sleep(1)
# # The head's pan angle is the measure of deflection between the head's center and the torso's center.
# theta1 = self.state.head_arduino_pan_degrees
# # Get the deflection angle between the QR codes orthogonal line and our line of sight. Also known as theta1.
# deflection_angle_degrees = qr_tracker_matches.deflection_angle * 180. / pi
# print('deflection_angle_degrees:', deflection_angle_degrees)
# # Estimate the angle between our head's angle and the angle to turn our head completely perpendicular to the QR code. Also known as theta2.
# # Note that theta2 + theta3 + 90 for all the angle in a right triangle between the camera, the QR code and our ideal docking start position.
# theta2 = 180 - (90 + deflection_angle_degrees)
# print('theta1 (head pan_degrees):', theta1)
# print('theta2 (angle between head and docking start point):', theta2)
# if theta1 > 180:
# # Need to rotate CCW.
# torso_rotation = (theta1 - 360) - theta2
# print('torso_rotation.a:', torso_rotation)
# else:
# # Need to rotate CW.
# torso_rotation = theta1 - theta2
# print('torso_rotation.b:', torso_rotation)
# rospy.loginfo('torso_rotation: %s', torso_rotation)
# self.announce_status('Rotating torso by %s.' % int(torso_rotation))
# self.rotate_torso(torso_rotation)
# self.announce_status('Body angled.')
# self.announce_status('Moving body to be infront of QR code.')
# self.move_forward_until_aligned()
#TODO
# self.announce_status('Freezing head angle.')
# self.set_pan_freeze(1)
# self.announce_status('Rotating torso to face dock.')
# self.center_torso()
# rospy.loginfo('Approaching dock.')
# elif self.state.index == self.CENTERING:
# #rotate body to parallel dock
# #move sideways until perpendicular to dock
# pass
# elif self.state.index == self.APPROACHING:
# #rotate body, keeping head pointed at dock, until body straight inline with dock
# #move forward slowly in bursts until EP1 and EP2 register power connected
# #disable forward motors in arduino when EP1 and EP2 set, timeout after 3 seconds
# pass
# elif self.state.index == self.RETREATING:
# pass
# time.sleep(1)
except KeyboardInterrupt:
self.announce_status('Emergency interrupt received.')
except Terminate as exc:
self.announce_status('Docking procedure terminated. %s' % exc)
traceback.print_exc()
except Exception as exc: # pylint: disable=broad-except
self.announce_status('An unexpected error occurred. %s' % exc)
traceback.print_exc()
else:
self.announce_status('Docking procedure complete.')
finally:
self.on_shutdown()
def handle_button(pin):
common.log('Button was pressed')
new_state = not relay()
set_pin(relay, new_state)
if new_state == False:
common.publish(EMERGENCY_STOP_PUBTOPIC, "stop")
