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
