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")