def __init__(self, verbose_debug, ros_pub): ## call base class' __init__ headTiltBaseBehavior.__init__(self, verbose_debug, ros_pub) ## add anything else needed by an instance of this subclass self.DC_helper = dynamixelConversions() if self.makiPP == None: self.makiPP = dict(zip(F_VAL_SEQ, [INVALID_INT] * len(F_VAL_SEQ))) ## Does Maki-ro remain in end position (shift=True) ## or revert to ground position (shift=False) self.shift = False ## default is False self.origin_ep = EP_FRONT ## default is neutral self.origin_et = ET_MIDDLE ## default is neutral ## set random range for eye pan/tilt location centered around neutral ## 30 ticks looks paranoid #self.ep_delta_range = 30 ## ticks #self.et_delta_range = 30 ## ticks ## 10 ticks looks like visual scanning on a focused point self.ep_delta_range = 10 ## ticks self.et_delta_range = 10 #20 #15 #10 ## ticks ## in addition to 100ms command propogation delay, provide option ## additional rest period self.visual_scan_rest_occurence_percent = 35 #25 ## [1,100) self.visual_scan_rest_enabled = True self.visual_scan_rest_min = 400 #200 #250 #100 #50 ## milliseconds self.visual_scan_rest_max = 800 #600 #750 #400 #300 ## milliseconds ## NOTE: Scaz wanted longer rest durations between visual scan movements if selectiveAttention.__is_scanning == None: selectiveAttention.__is_scanning = False self.ALIVE = True return
def __init__(self, verbose_debug, ros_pub): self.count_movements = 0 self.ALIVE = True #self.mTT_INTERRUPT = True ## 2016-06-16, KATE self.mTT_INTERRUPT = False self.VERBOSE_DEBUG = verbose_debug ## default is False self.SWW_WI = ROS_sleepWhileWaiting_withInterrupt() self.DC_helper = dynamixelConversions() ## Does Maki-ro remain in end position (shift=True) ## or revert to ground position (shift=False) self.shift = False ## default is False #print ros_pub if ros_pub == None: self.initROS( self ) else: self.ros_pub = ros_pub ## can we pass a ros publisher??? Apparently so! if baseBehavior.__maki_cmd_msg_format == None: baseBehavior.initPubMAKIFormat( self ) #rospy.logdebug( str(baseBehavior.__maki_cmd_msg_format) ) if baseBehavior.__maki_feedback_format == None: baseBehavior.initSubMAKIFormat( self ) self.initPubMAKI() self.initSubMAKIFeedback() self.makiPP = None self.maki_feedback_values = {} ## empty dictionary
def __init__(self, verbose_debug, ros_pub): ## KATE ## call base class' __init__ lookAt.__init__(self, verbose_debug, ros_pub) #headTiltBaseBehavior.__init__( self, verbose_debug, ros_pub ) #headPanBaseBehavior.__init__( self, verbose_debug, self.ros_pub ) ## add anything else needed by an instance of this subclass self.DC_helper = dynamixelConversions() if lookINSPIRE4Interaction.HP_LEFT_SCREEN == None: lookINSPIRE4Interaction.HP_LEFT_SCREEN = HP_LEFT_SCREEN #650 #620 if lookINSPIRE4Interaction.HT_LEFT_SCREEN == None: lookINSPIRE4Interaction.HT_LEFT_SCREEN = HT_LEFT_SCREEN if lookINSPIRE4Interaction.EP_LEFT_SCREEN_SACCADE == None: lookINSPIRE4Interaction.EP_LEFT_SCREEN_SACCADE = EP_LEFT_SCREEN_SACCADE #EP_RIGHT if lookINSPIRE4Interaction.HP_RIGHT_SCREEN == None: lookINSPIRE4Interaction.HP_RIGHT_SCREEN = HP_RIGHT_SCREEN #404 if lookINSPIRE4Interaction.HT_RIGHT_SCREEN == None: lookINSPIRE4Interaction.HT_RIGHT_SCREEN = lookINSPIRE4Interaction.HT_LEFT_SCREEN if lookINSPIRE4Interaction.EP_RIGHT_SCREEN_SACCADE == None: lookINSPIRE4Interaction.EP_RIGHT_SCREEN_SACCADE = EP_RIGHT_SCREEN_SACCADE #EP_LEFT if lookINSPIRE4Interaction.HP_FACE_INFANT == None: lookINSPIRE4Interaction.HP_FACE_INFANT = HP_FRONT if lookINSPIRE4Interaction.HT_FACE_INFANT == None: lookINSPIRE4Interaction.HT_FACE_INFANT = HT_MIDDLE if lookINSPIRE4Interaction.EP_FACE_INFANT_FROM_LEFT_SCREEN == None: lookINSPIRE4Interaction.EP_FACE_INFANT_FROM_LEFT_SCREEN = EP_LEFT if lookINSPIRE4Interaction.EP_FACE_INFANT_FROM_RIGHT_SCREEN == None: lookINSPIRE4Interaction.EP_FACE_INFANT_FROM_RIGHT_SCREEN = EP_RIGHT if lookINSPIRE4Interaction.FACING_LEFT_SCREEN == None: lookINSPIRE4Interaction.FACING_LEFT_SCREEN = "leftScreen" if lookINSPIRE4Interaction.FACING_RIGHT_SCREEN == None: lookINSPIRE4Interaction.FACING_RIGHT_SCREEN = "rightScreen" if lookINSPIRE4Interaction.FACING_INFANT == None: lookINSPIRE4Interaction.FACING_INFANT = "infant" self.facing = None if self.makiPP == None: self.makiPP = dict(zip(F_VAL_SEQ, [INVALID_INT] * len(F_VAL_SEQ))) self.ipt_turn = 1000 ## ms #self.delta_ht = 10 ## ticks #self.ht_rand_min = lookINSPIRE4Interaction.HT_LEFT_SCREEN - self.delta_ht #self.ht_rand_max = lookINSPIRE4Interaction.HT_LEFT_SCREEN +- self.delta_ht self.delta_ht = 10 ## ticks self.ht_rand_min = lookINSPIRE4Interaction.HT_LEFT_SCREEN - self.delta_ht self.delta_ht = 0 ## ticks self.ht_rand_max = lookINSPIRE4Interaction.HT_LEFT_SCREEN + self.delta_ht self.__use_shift_gaze = True ## CHANGE TO FALSE TO REVERT self.ALIVE = True return
def __init__(self, verbose_debug, ros_pub): ## call base class' __init__ headTiltBaseBehavior.__init__( self, verbose_debug, ros_pub ) ## add anything else needed by an instance of this subclass self.DC_helper = dynamixelConversions() ## different versions of head nodding headShake.v1 = False headShake.v2 = False ## RIGHT --> LEFTMID --> MID headShake.v3 = False ## RIGHT --> MID headShake.v4 = True ## ??LEFT --> MID self.repetition = 1 self.sww_wi = ROS_sleepWhileWaiting_withInterrupt( verbose_debug=self.VERBOSE_DEBUG ) if self.makiPP == None: self.makiPP = dict( zip(F_VAL_SEQ, [ INVALID_INT ] * len(F_VAL_SEQ) ) ) self.ALIVE = True self.headshake_max_min_dist = float( abs( HP_LEFT - HP_RIGHT ) ) self.headshake_front_right_dist = float( abs( HP_FRONT - HP_RIGHT ) ) self.headshake_front_left_dist = float( abs( HP_FRONT - HP_LEFT ) ) ## debugging #print "headshake_max_min_dist = " #print self.headshake_max_min_dist #print "str(" + str(self.headshake_max_min_dist) + ")" ## #print "headshake_front_right_dist = " #print self.headshake_front_right_dist #print "str(" + str(self.headshake_front_right_dist) + ")" ## #print "headshake_front_left_dist = " #print self.headshake_front_left_dist #print "str(" + str(self.headshake_front_left_dist) + ")" ## #print self.headshake_front_right_dist / self.headshake_max_min_dist #print self.headshake_front_left_dist / self.headshake_max_min_dist self.ipt_shake_front_right = float( (self.headshake_front_right_dist / self.headshake_max_min_dist) * IPT_FACE ) self.ipt_shake_front_left = float( (self.headshake_front_left_dist / self.headshake_max_min_dist) * IPT_FACE ) self.ipt_shake_front_right = int(self.ipt_shake_front_right + 0.5) ## implicit rounding self.ipt_shake_front_left = int(self.ipt_shake_front_left + 0.5) ## implicit rounding ## debugging # rospy.logdebug( "(full)\tIPT_NOD = " + str(IPT_FACE) + "ms" ) # rospy.logdebug( "(partial)\t ipt_shake_front_right = " + str(self.ipt_shake_front_right) + "ms" ) # rospy.logdebug( "(partial)\t ipt_shake_front_left = " + str(self.ipt_shake_front_left) + "ms" ) # rospy.logdebug( "(summed partial)\t ipt_nod_middle_* = " + str( self.ipt_shake_front_right + self.ipt_shake_front_left ) + "ms" ) ## maki_command prefix _m_cmd_prefix = "HP" + SC_SET_GP ## shake macros self.shake_left_right = _m_cmd_prefix + str(HP_RIGHT) + SC_SET_IPT + str(IPT_FACE) + TERM_CHAR_SEND self.shake_right_left = _m_cmd_prefix + str(HP_LEFT) + SC_SET_IPT + str(IPT_FACE) + TERM_CHAR_SEND self.shake_front_left = _m_cmd_prefix + str(HP_LEFT) + SC_SET_IPT + str(self.ipt_shake_front_left) + TERM_CHAR_SEND self.shake_front_right = _m_cmd_prefix + str(HP_RIGHT) + SC_SET_IPT + str(self.ipt_shake_front_right) + TERM_CHAR_SEND self.shake_left_front = _m_cmd_prefix + str(HP_FRONT) + SC_SET_IPT + str(self.ipt_shake_front_left) + TERM_CHAR_SEND self.shake_right_front = _m_cmd_prefix + str(HP_FRONT) + SC_SET_IPT + str(self.ipt_shake_front_right) + TERM_CHAR_SEND
def __init__(self, verbose_debug, ros_pub): ## call base class' __init__ eyelidHeadTiltBaseBehavior.__init__(self, verbose_debug, ros_pub) headPanBaseBehavior.__init__(self, verbose_debug, self.ros_pub) ## add anything else needed by an instance of this subclass self.DC_helper = dynamixelConversions() if asleepAwake.__is_asleep == None: asleepAwake.is_asleep = False if asleepAwake.__is_awake == None: asleepAwake.__is_awake = False ## NOTE: should be the same values as look_inspire4_intro.py if asleepAwake.HP_ASLEEP == None: asleepAwake.HP_ASLEEP = HP_LEFT #312 if asleepAwake.HT_ASLEEP == None: ## 2016-06-16, KATE #asleepAwake.HT_ASLEEP = HT_DOWN ## 460 asleepAwake.HT_ASLEEP = HT_MIN ## 445 if asleepAwake.LL_ASLEEP == None: asleepAwake.LL_ASLEEP = LL_CLOSE_MAX if asleepAwake.EP_ASLEEP == None: asleepAwake.EP_ASLEEP = EP_LEFT if asleepAwake.ET_ASLEEP == None: asleepAwake.ET_ASLEEP = ET_DOWN if asleepAwake.HP_AWAKE == None: asleepAwake.HP_AWAKE = HP_FRONT if asleepAwake.HT_AWAKE == None: asleepAwake.HT_AWAKE = HT_MIDDLE if asleepAwake.LL_AWAKE == None: asleepAwake.LL_AWAKE = LL_OPEN_DEFAULT if asleepAwake.EP_AWAKE == None: asleepAwake.EP_AWAKE = EP_FRONT if asleepAwake.ET_AWAKE == None: asleepAwake.ET_AWAKE = ET_MIDDLE if asleepAwake.HP_AWAKE_EXP == None: #asleepAwake.HP_AWAKE_EXP = 620 #HP_RIGHT was too far; experimenter would have had to be standing behind left_screen asleepAwake.HP_AWAKE_EXP = HP_EXPERIMENTER ## ticks if asleepAwake.HT_AWAKE_EXP == None: #asleepAwake.HT_AWAKE_EXP = 565 #562 ## HT_UP is maxed out at 582; NOTE: we still need some room for startle response ==> 20 ticks asleepAwake.HT_AWAKE_EXP = HT_EXPERIMENTER ## ticks if asleepAwake.LL_AWAKE_EXP == None: asleepAwake.LL_AWAKE_EXP = asleepAwake.LL_AWAKE if asleepAwake.EP_AWAKE_EXP == None: asleepAwake.EP_AWAKE_EXP = EP_RIGHT if asleepAwake.ET_AWAKE_EXP == None: asleepAwake.ET_AWAKE_EXP = ET_UP return
def __init__(self, verbose_debug, ros_pub): ## call base class' __init__ headTiltBaseBehavior.__init__(self, verbose_debug, ros_pub) headPanBaseBehavior.__init__(self, verbose_debug, self.ros_pub) ## add anything else needed by an instance of this subclass self.DC_helper = dynamixelConversions() ## DEFAULT MOTOR SPEEDS self.LL_GS_DEFAULT = 100 self.EP_GS_DEFAULT = 100 self.ET_GS_DEFAULT = 200 self.HP_GS_DEFAULT = 15 self.HT_GS_DEFAULT = 51 self.ALIVE = True return
def __init__(self, verbose_debug, ros_pub): ## call base class' __init__ headTiltBaseBehavior.__init__(self, verbose_debug, ros_pub) ## add anything else needed by an instance of this subclass self.DC_helper = dynamixelConversions() self.sww_wi = ROS_sleepWhileWaiting_withInterrupt( verbose_debug=self.VERBOSE_DEBUG) if self.makiPP == None: self.makiPP = dict(zip(F_VAL_SEQ, [INVALID_INT] * len(F_VAL_SEQ))) self.ALIVE = True ipt_turn = 1000 ##ms ## look at and away macros #self.look_at = "HPGP" + str(HP_ALISSA) + "HTGP" + str(HT_ALISSA) + TERM_CHAR_SEND #self.look_neutral = "HPGP" + str(HP_FRONT) + "HTGP" + str(HT_MIDDLE) + TERM_CHAR_SEND #self.look_away = "HPGP" + str(HP_LEFT) + "HTGP" + str(HT_MIDDLE) + TERM_CHAR_SEND self.look_at = "HPGP" + str(HP_EXPERIMENTER) + "HTGP" + str( HT_EXPERIMENTER) + str(SC_SET_IPT) + str(ipt_turn) + TERM_CHAR_SEND self.look_neutral = "HPGP" + str(HP_FRONT) + "HTGP" + str( HT_MIDDLE) + str(SC_SET_IPT) + str(ipt_turn) + TERM_CHAR_SEND self.look_away = "HPGP" + str(HP_RIGHT) + "HTGP" + str( HT_MIDDLE) + str(SC_SET_IPT) + str(ipt_turn) + TERM_CHAR_SEND
## INITIALIZATION ## --------------------------------- ## STEP 0: INIT ROS initROS() ## STEP 1: INIT GLOBAL VARIABLES ALIVE = False INIT = True init_dict = dict( zip(FEEDBACK_SC, [ False ] * len(FEEDBACK_SC) ) ) makiPP = dict( zip(F_VAL_SEQ, [ INVALID_INT ] * len(F_VAL_SEQ) ) ) makiGP = dict( zip(F_VAL_SEQ, [ INVALID_INT ] * len(F_VAL_SEQ) ) ) maki_command = "" mc_count = 0 resetTimer() PIC_INTERRUPT = False DC_helper = dynamixelConversions() SWW_WI = ROS_sleepWhileWaiting_withInterrupt( verbose_debug=VERBOSE_DEBUG ) eyeSaccade = None startle = None headNod = None blink = None asleepAwake = None turnToScreen = None ## STEP 2: SIGNAL HANDLER #to allow closing the program using ctrl+C signal.signal(signal.SIGINT, signal_handler) ## STEP 3: INIT ROBOT STATE ## establish communication with the robot via rostopics initPubMAKI()
def __init__(self, verbose_debug, ros_pub): ## call base class' __init__ timedTest.__init__(self, verbose_debug, ros_pub) ## add anything else needed by an instance of this subclass self.DC_helper = dynamixelConversions()
def __init__(self, verbose_debug, ros_pub): ## call base class' __init__ headTiltBaseBehavior.__init__(self, verbose_debug, ros_pub) ## add anything else needed by an instance of this subclass self.DC_helper = dynamixelConversions() ## different versions of head nodding headNod.v1 = False headNod.v2 = False headNod.v3 = True self.repetition = 1 self.sww_wi = ROS_sleepWhileWaiting_withInterrupt( verbose_debug=self.VERBOSE_DEBUG) if self.makiPP == None: self.makiPP = dict(zip(F_VAL_SEQ, [INVALID_INT] * len(F_VAL_SEQ))) self.ALIVE = True self.headnod_max_min_dist = float(abs(HT_UP - HT_DOWN)) self.headnod_middle_down_dist = float(abs(HT_MIDDLE - HT_DOWN)) self.headnod_middle_up_dist = float(abs(HT_MIDDLE - HT_UP)) ## debugging #print "headnod_max_min_dist = " #print self.headnod_max_min_dist #print "str(" + str(self.headnod_max_min_dist) + ")" ## #print "headnod_middle_down_dist = " #print self.headnod_middle_down_dist #print "str(" + str(self.headnod_middle_down_dist) + ")" ## #print "headnod_middle_up_dist = " #print self.headnod_middle_up_dist #print "str(" + str(self.headnod_middle_up_dist) + ")" ## #print self.headnod_middle_down_dist / self.headnod_max_min_dist #print self.headnod_middle_up_dist / self.headnod_max_min_dist self.ipt_nod_middle_down = float( (self.headnod_middle_down_dist / self.headnod_max_min_dist) * IPT_NOD) self.ipt_nod_middle_up = float( (self.headnod_middle_up_dist / self.headnod_max_min_dist) * IPT_NOD) self.ipt_nod_middle_down = int(self.ipt_nod_middle_down + 0.5) ## implicit rounding self.ipt_nod_middle_up = int(self.ipt_nod_middle_up + 0.5) ## implicit rounding ## debugging #rospy.logdebug( "(full)\tIPT_NOD = " + str(IPT_NOD) + "ms" ) #rospy.logdebug( "(partial)\t ipt_nod_middle_down = " + str(self.ipt_nod_middle_down) + "ms" ) #rospy.logdebug( "(partial)\t ipt_nod_middle_up = " + str(self.ipt_nod_middle_up) + "ms" ) #rospy.logdebug( "(summed partial)\t ipt_nod_middle_* = " + str( self.ipt_nod_middle_down + self.ipt_nod_middle_up ) + "ms" ) ## maki_command prefix _m_cmd_prefix = "HT" + SC_SET_GP ## nod macros self.nod_up_down = _m_cmd_prefix + str(HT_DOWN) + SC_SET_IPT + str( IPT_NOD) + TERM_CHAR_SEND self.nod_down_up = _m_cmd_prefix + str(HT_UP) + SC_SET_IPT + str( IPT_NOD) + TERM_CHAR_SEND self.nod_middle_up = _m_cmd_prefix + str(HT_UP) + SC_SET_IPT + str( self.ipt_nod_middle_up) + TERM_CHAR_SEND self.nod_middle_down = _m_cmd_prefix + str(HT_DOWN) + SC_SET_IPT + str( self.ipt_nod_middle_down) + TERM_CHAR_SEND self.nod_up_middle = _m_cmd_prefix + str(HT_MIDDLE) + SC_SET_IPT + str( self.ipt_nod_middle_up) + TERM_CHAR_SEND self.nod_down_middle = _m_cmd_prefix + str( HT_MIDDLE) + SC_SET_IPT + str( self.ipt_nod_middle_down) + TERM_CHAR_SEND