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 macroAsleep(self):
_sww_wi = ROS_sleepWhileWaiting_withInterrupt(
verbose_debug=self.VERBOSE_DEBUG)
self.ALIVE = True
## this is a nested while loop
_print_once = True
while self.ALIVE:
if not rospy.is_shutdown():
pass
else:
break ## break out of outer while loop
if _print_once:
rospy.logdebug("Entering macroAsleep outer while loop")
_print_once = False
if self.mTT_INTERRUPT:
#print "start sleep 5"
_sww_wi.sleepWhileWaiting(5)
#print "end sleep 5"
continue ## begin loop again from the beginning skipping below
#print "shouldn't get here"
rospy.logdebug("Entering macroAsleep inner while loop")
_asleep_count = 0
while not self.mTT_INTERRUPT:
_asleep_count = asleepAwakeTest.helper_macroAsleep(
self, _asleep_count)
## try to nicely end testing
if self.mTT_INTERRUPT:
_sww_wi.sleepWhileWaiting(
1
) ## make sure to wait for message to reach Dynamixel servo
timedTest.pubTo_maki_command(self, "reset")
_sww_wi.sleepWhileWaiting(
1
) ## make sure to wait for message to reach Dynamixel servo
else:
pass
# end while not self.mTT_INTERRUPT
# end while self.ALIVE
timedTest.pubTo_maki_command(self, "reset")
_sww_wi.sleepWhileWaiting(resetting_time)
headTiltTimedTest.disableHT()
timedTest.pubTo_maki_command(
self,
"HT" + str(SC_SET_TL) + str(ht_tl_disable) + str(TERM_CHAR_SEND))
rospy.loginfo("END: After outer while loop in macroAsleepTest()")
def __init__(self, verbose_debug, ros_pub):
self.ALIVE = True
self.mTT_INTERRUPT = True
self.VERBOSE_DEBUG = verbose_debug ## default is False
self.SWW_WI = ROS_sleepWhileWaiting_withInterrupt()
if ros_pub == None:
self.initROS(self)
else:
self.ros_pub = ros_pub ## can we pass a ros publisher??? Apparently so!
if timedTest.__maki_msg_format == None:
timedTest.initPubMAKIFormat(self)
#rospy.logdebug( str(timedTest.__maki_msg_format) )
self.makiPP = None
def macroBlink(self):
_sww_wi = ROS_sleepWhileWaiting_withInterrupt()
## this is a nested while loop
_print_once = True
while self.ALIVE:
if not rospy.is_shutdown():
pass
else:
break ## break out of outer while loop
if _print_once:
rospy.logdebug("Entering macroBlink outer while loop")
_print_once = False
if self.mTT_INTERRUPT:
#print "start sleep 5"
_sww_wi.sleepWhileWaiting(5) # 5 seconds
#print "end sleep 5"
continue ## begin loop again from the beginning skipping below
#print "shouldn't get here"
rospy.logdebug("Entering macroBlink inner while loop")
_blink_count = 0
while not self.mTT_INTERRUPT:
for _blink_rep in range(1, 2): #range(1,6): ## [1,6)
#_blink_count = blinkTest.helper_macroBlink( self, _blink_count, True, _blink_rep, read_time=2.0 )
#_blink_count = blinkTest.helper_macroBlink( self, _blink_count, False, _blink_rep, read_time=2.0 )
#_blink_count = blinkTest.helper_macroBlink( self, _blink_count, True, 3, read_time=0.5 )
_blink_count = blinkTest.helper_macroBlink(self,
_blink_count,
False,
3,
read_time=0.5)
_sww_wi.sleepWhileWaiting(2)
rospy.logdebug("***************")
## try to nicely end testing the eye blink
if self.mTT_INTERRUPT:
_sww_wi.sleepWhileWaiting(
1
) ## make sure to wait for message to reach Dynamixel servo
timedTest.pubTo_maki_command(self, "reset")
_sww_wi.sleepWhileWaiting(
1
) ## make sure to wait for message to reach Dynamixel servo
else:
pass
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__
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
def macroStartle( self ):
_sww_wi = ROS_sleepWhileWaiting_withInterrupt( verbose_debug=self.VERBOSE_DEBUG )
self.ALIVE = True
## this is a nested while loop
_print_once = True
while self.ALIVE:
if not rospy.is_shutdown():
pass
else:
break ## break out of outer while loop
if _print_once:
rospy.logdebug("Entering macroStartle outer while loop")
_print_once = False
if self.mTT_ITERRUPT:
#print "start sleep 5"
self.SWW_WI.sleepWhileWaiting( 5 )
#print "end sleep 5"
continue ## begin loop again from the beginning skipping below
#print "shouldn't get here"
rospy.logdebug("Entering macroStartle inner while loop")
_startle_count = 0
while not self.mTT_ITERRUPT:
_startle_count = startleTest.helper_macroStartle( self, _startle_count )
## try to nicely end testing
if self.mTT_ITERRUPT:
_sww_wi.sleepWhileWaiting( 1 ) ## make sure to wait for message to reach Dynamixel servo
timedTest.pubTo_maki_command( self, "reset" )
_sww_wi.sleepWhileWaiting( 1 ) ## make sure to wait for message to reach Dynamixel servo
else:
pass
# end while not self.mTT_ITERRUPT
# end while self.ALIVE
rospy.loginfo("END: After outer while loop in macroStartleTest()")
def helper_macroEmptyTest( self, pass_count, read_time=1.0 ):
_sww_wi = ROS_sleepWhileWaiting_withInterrupt( verbose_debug=self.VERBOSE_DEBUG )
_start_pass_time = None
_finish_pass_time = None
_total_pass_time = 0
## maki_command
_m_cmd = "reset"
rospy.loginfo("-----------------")
for _pass_time in range(0,3): ## 0, 1, 2
_start_pass_time = rospy.get_time()
rospy.logdebug( "resetting..." )
timedTest.pubTo_maki_command( self, str(_m_cmd) )
_sww_wi.sleepWhileWaitingMS( (_pass_time+1)*1000, 0.01 )
_finish_pass_time = rospy.get_time()
_total_pass_time = abs(_finish_pass_time - _start_pass_time)
pass_count += 1
#rospy.loginfo( "Completed " + str(pass_count) + " passes" )
rospy.loginfo( "Pass #" + str(pass_count) + ": full pass time = "
+ str( _total_pass_time ) )
rospy.loginfo("-----------------")
## make it easier to read
_sww_wi.sleepWhileWaiting( read_time, 0.5 )
## try to nicely end test
if self.mTT_ITERRUPT:
break ## break out of for loop
else:
## reset timers
_start_pass_time = None
_finish_pass_time = None
_total_pass_time = 0
# end for _pass_time in range(0,3):
return pass_count
class timedTest(object):
## all instances of this class share the same value
## variables private to this class
__maki_msg_format = None
def __init__(self, verbose_debug, ros_pub):
self.ALIVE = True
self.mTT_INTERRUPT = True
self.VERBOSE_DEBUG = verbose_debug ## default is False
self.SWW_WI = ROS_sleepWhileWaiting_withInterrupt()
if ros_pub == None:
self.initROS(self)
else:
self.ros_pub = ros_pub ## can we pass a ros publisher??? Apparently so!
if timedTest.__maki_msg_format == None:
timedTest.initPubMAKIFormat(self)
#rospy.logdebug( str(timedTest.__maki_msg_format) )
self.makiPP = None
def startTimedTest(self, makiPP):
self.ALIVE = True
self.mTT_INTERRUPT = False
self.makiPP = makiPP
def stopTimedTest(self):
self.mTT_INTERRUPT = True
def exitTimedTest(self):
self.ALIVE = False
self.mTT_INTERRUPT = True
def update(self, makiPP):
self.makiPP = makiPP
###############################
## Example contents of timed test
###############################
def macroEmptyTest(self):
## this is a nested while loop
_print_once = True
#while self.ALIVE and not rospy.is_shutdown():
while self.ALIVE:
if not rospy.is_shutdown():
pass
else:
break ## break out of outer while loop
if _print_once:
rospy.logdebug("Entering macroEmptyTest outer while loop")
_print_once = False
if self.mTT_INTERRUPT:
print "start sleep 5"
self.SWW_WI.sleepWhileWaiting(5)
print "end sleep 5"
continue ## begin loop again from the beginning skipping below
#print "shouldn't get here"
rospy.logdebug("Entering macroEmptyTest inner while loop")
_pass_count = 0
while not self.mTT_INTERRUPT:
_pass_count = timedTest.helper_macroEmptyTest(
self, _pass_count)
## try to nicely end testing
if self.mTT_INTERRUPT:
self.SWW_WI.sleepWhileWaiting(
1
) ## make sure to wait for message to reach Dynamixel servo
timedTest.pubTo_maki_command(self, "reset")
self.SWW_WI.sleepWhileWaiting(
1
) ## make sure to wait for message to reach Dynamixel servo
else:
pass
# end while not mTT_INTERRUPT
# end while self.ALIVE and not rospy.is_shutdown():
rospy.loginfo("END: After outer while loop in macroEmptyTest()")
def helper_macroEmptyTest(self, pass_count, read_time=1.0):
_start_pass_time = None
_finish_pass_time = None
_total_pass_time = 0
## maki_command
_m_cmd = "reset"
rospy.loginfo("-----------------")
for _pass_time in range(0, 3): ## 0, 1, 2
_start_pass_time = rospy.get_time()
rospy.logdebug("resetting...")
timedTest.pubTo_maki_command(self, str(_m_cmd))
self.SWW_WI.sleepWhileWaitingMS((_pass_time + 1) * 1000, 0.01)
_finish_pass_time = rospy.get_time()
_total_pass_time = abs(_finish_pass_time - _start_pass_time)
pass_count += 1
#rospy.loginfo( "Completed " + str(pass_count) + " passes" )
rospy.loginfo("Pass #" + str(pass_count) + ": full pass time = " +
str(_total_pass_time))
rospy.loginfo("-----------------")
## make it easier to read
self.SWW_WI.sleepWhileWaiting(read_time, 0.5)
## try to nicely end test
if self.mTT_INTERRUPT:
break ## break out of for loop
else:
## reset timers
_start_pass_time = None
_finish_pass_time = None
_total_pass_time = 0
# end for _pass_time in range(0,3):
return pass_count
#####################
## THESE ARE COMMON FOR ALL TESTS
#####################
def pubTo_maki_command(self, commandOut):
_pub_flag = False
## make sure that commandOut ends in only one TERM_CHAR_SEND
_tmp = re.search(timedTest.__maki_msg_format, commandOut)
if _tmp != None:
## Yes, commandOut ends in only one TERM_CHAR_SEND
_pub_flag = True
#if self.VERBOSE_DEBUG: rospy.logdebug( str(commandOut) + " matched maki_msg_format" )
elif (commandOut == "reset"):
## special case handled by MAKI-Arbotix-Interface.py driver
_pub_flag = True
elif not commandOut.endswith(str(TERM_CHAR_SEND)):
## append the missing TERM_CHAR_SEND
commandOut += str(TERM_CHAR_SEND)
_pub_flag = True
if self.VERBOSE_DEBUG:
rospy.logdebug(str(commandOut) + " added TERM_CHAR_SEND")
else:
rospy.logerr("Incorrect message format" + str(commandOut))
if self.VERBOSE_DEBUG: rospy.logdebug(str(commandOut))
if _pub_flag and not rospy.is_shutdown():
self.ros_pub.publish(commandOut)
#####################
## Initialize ROS node
#####################
def initROS(self, nodename="anon"):
## get function name for logging purposes
_fname = sys._getframe(
).f_code.co_name ## see http://stackoverflow.com/questions/5067604/determine-function-name-from-within-that-function-without-using-tracebacko
print str(_fname) + ": BEGIN" ## THIS IS BEFORE ROSNODE INIT
_anon_rosnode = False
if nodename == "anon": _anon_rosnode = True
# see http://wiki.ros.org/rospy/Overview/Logging
if self.VERBOSE_DEBUG:
rospy.init_node(nodename,
anonymous=_anon_rosnode,
log_level=rospy.DEBUG)
rospy.logdebug("log_level=rospy.DEBUG")
else:
rospy.init_node(
nodename,
anonymous=_anon_rosnode) ## defaults to log_level=rospy.INFO
rospy.logdebug("anonymous=" + str(_anon_rosnode))
rospy.loginfo(str(_fname) + ": END")
return
#####################
## Set up publisher to /maki_command
#####################
def initPubMAKI(self):
## get function name for logging purposes
_fname = sys._getframe(
).f_code.co_name ## see http://stackoverflow.com/questions/5067604/determine-function-name-from-within-that-function-without-using-tracebacko
rospy.logdebug(str(_fname) + ": BEGIN")
# Setup publisher
self.ros_pub = rospy.Publisher("maki_command", String, queue_size=10)
rospy.loginfo(str(_fname) + ": END")
return
#####################
## Set up regex format for publishing on /maki_command
#####################
def initPubMAKIFormat(self):
## get function name for logging purposes
_fname = sys._getframe(
).f_code.co_name ## see http://stackoverflow.com/questions/5067604/determine-function-name-from-within-that-function-without-using-tracebacko
rospy.logdebug(str(_fname) + ": BEGIN")
## make sure that commandOut ends in only one TERM_CHAR_SEND
#self.__maki_msg_format = "\A[a-yA-Y]+[a-yA-Y0-9]*"
#self.__maki_msg_format += str(TERM_CHAR_SEND)
#self.__maki_msg_format += "{1}$"
timedTest.__maki_msg_format = "\A[a-yA-Y]+[a-yA-Y0-9]*"
timedTest.__maki_msg_format += str(TERM_CHAR_SEND)
timedTest.__maki_msg_format += "{1}$"
rospy.loginfo(str(_fname) + ": END")
return
def helper_macroEyeSaccade(self, saccade_count, read_time=1.0):
_sww_wi = ROS_sleepWhileWaiting_withInterrupt(
verbose_debug=self.VERBOSE_DEBUG)
_start_saccade_time = None
_start_saccade_side_time = None
_finish_saccade_side_time = None
_start_saccade_front_time = None
_finish_saccade_front_time = None
_finish_saccade_time = None
_total_saccade_time = 0
## maki_command prefix
_m_cmd_prefix = "EP" + SC_SET_GP
rospy.loginfo("-----------------")
for _eye_saccade_time in eye_saccade_time:
## maki_command suffix
_m_cmd_suffix = SC_SET_IPT + str(
_eye_saccade_time) + TERM_CHAR_SEND
_saccade_front_right = _m_cmd_prefix + str(
EP_RIGHT) + _m_cmd_suffix
_saccade_front_left = _m_cmd_prefix + str(EP_LEFT) + _m_cmd_suffix
_saccade_front = _m_cmd_prefix + str(EP_FRONT) + _m_cmd_suffix
## used for choosing direction of eye saccade
## 0 = left, 1 = right
_rand_saccade_right = random.randint(0, 1)
_start_saccade_time = rospy.get_time()
_start_saccade_side_time = rospy.get_time()
if _rand_saccade_right == 1:
rospy.logdebug("saccade RIGHT")
timedTest.pubTo_maki_command(self, str(_saccade_front_right))
else:
rospy.logdebug("saccade LEFT")
timedTest.pubTo_maki_command(self, str(_saccade_front_left))
_sww_wi.sleepWhileWaitingMS(_eye_saccade_time, 0.01)
_finish_saccade_side_time = rospy.get_time()
_finish_saccade_time = rospy.get_time()
_total_saccade_time = abs(_finish_saccade_time -
_start_saccade_time)
## make it easier to read
_sww_wi.sleepWhileWaiting(read_time, 0.5)
_start_saccade_time = rospy.get_time()
## return to eye pan looking front
_start_saccade_front_time = rospy.get_time()
timedTest.pubTo_maki_command(self, str(_saccade_front))
_sww_wi.sleepWhileWaitingMS(_eye_saccade_time, 0.01)
_finish_saccade_front_time = rospy.get_time()
_finish_saccade_time = rospy.get_time()
_total_saccade_time += abs(_finish_saccade_time -
_start_saccade_time)
## make it easier to read
_sww_wi.sleepWhileWaiting(read_time, .5)
saccade_count += 1
#rospy.loginfo( "Completed " + str(saccade_count) + " full eye saccades" )
rospy.loginfo(
"Eye saccade #" + str(saccade_count) +
": full eye saccade = " + str(_total_saccade_time) +
"; eye saccade front->side = " +
str(abs(_finish_saccade_side_time -
_start_saccade_side_time)) +
"; eye saccade side->front = " +
str(abs(_finish_saccade_front_time -
_start_saccade_front_time)))
rospy.loginfo("-----------------")
## try to nicely end testing the eye saccade
if self.mTT_INTERRUPT:
break ## break out of for loop
else:
## reset timers
_start_saccade_time = None
_start_saccade_side_time = None
_finish_saccade_side_time = None
_start_saccade_front_time = None
_finish_saccade_front_time = None
_finish_saccade_time = None
_total_saccade_time = 0
# end for _eye_saccade_time in eye_saccade_time:
return saccade_count
def macroEyeSaccade(self):
_sww_wi = ROS_sleepWhileWaiting_withInterrupt(
verbose_debug=self.VERBOSE_DEBUG)
## this is a nested while loop
_print_once = True
while self.ALIVE:
if rospy.is_shutdown():
break ## break out of macroEyeSaccade out while loop
if _print_once:
rospy.logdebug("Entering macroEyeSaccade outer while loop")
_print_once = False
if self.mTT_INTERRUPT:
#print "start sleep 5"
_sww_wi.sleepWhileWaiting(5) ## 5 seconds
#print "end sleep 5"
continue ## begin loop again from the beginning skipping below
#print "shouldn't get here"
## where is MAKI's EP closest to currently? EP_LEFT, EP_FRONT, EP_RIGHT
_ep_pp = self.makiPP["EP"]
_delta_ep_pp = abs(_ep_pp - EP_FRONT)
_ep_macro_pose = EP_FRONT
if (abs(_ep_pp - EP_LEFT) < _delta_ep_pp):
_delta_ep_pp = abs(_ep_pp - EP_LEFT)
_ep_macro_pose = EP_LEFT
if (abs(_ep_pp - EP_RIGHT) < _delta_ep_pp):
_delta_ep_pp = abs(_ep_pp - EP_RIGHT)
_ep_macro_pose = EP_RIGHT
## publish GP of _ep_macro_pose in case in between poses
timedTest.pubTo_maki_command(
self, "EP" + SC_SET_GP + str(_ep_macro_pose) + TERM_CHAR_SEND)
## raise LL so that it doesn't impede saccade
timedTest.pubTo_maki_command(
self, "LL" + SC_SET_GP + str(LL_OPEN_MAX) + TERM_CHAR_SEND)
rospy.logdebug("Entering macroEyeSaccade inner while loop")
_saccade_count = 0
while not self.mTT_INTERRUPT:
_saccade_count = saccadeTest.helper_macroEyeSaccade(
self, _saccade_count)
## try to nicely end testing the eye saccade
if self.mTT_INTERRUPT:
_sww_wi.sleepWhileWaiting(
1
) ## make sure to wait for message to reach Dynamixel servo
timedTest.pubTo_maki_command(self, "reset")
_sww_wi.sleepWhileWaiting(
1
) ## make sure to wait for message to reach Dynamixel servo
else:
pass
# end while not self.mTT_INTERRUPT
# end while self.ALIVE
rospy.loginfo("END: After outer while loop in macroEyeSaccade()")
def helper_macroStartle( self, startle_count, read_time=1.0 ):
_sww_wi = ROS_sleepWhileWaiting_withInterrupt( verbose_debug=self.VERBOSE_DEBUG )
_start_lid_open_time = None
_finish_lid_open_time = None
_total_lid_open_time = 0
## neutral to LL_OPEN_MAX
NEUTRAL_LL = self.makiPP["LL"]
_delta_ticks_LL = abs( NEUTRAL_LL - LL_OPEN_MAX )
## neutral to HT_STARTLE
#NEUTRAL_HT = self.makiPP["HT"]
NEUTRAL_HT = HT_MIDDLE
rospy.logerr("HT_STARTLE=" + str(HT_STARTLE))
_delta_ticks_HT = abs( NEUTRAL_HT - HT_STARTLE )
## neutral to ET_UP
NEUTRAL_ET = ET_MIDDLE
_delta_ticks_ET = abs( NEUTRAL_ET - ET_UP )
## maki_cmd
_m_cmd_prefix_LL = "LL" + str(SC_SET_GS)
_m_cmd_lid_neutral_open = "LL" + str(SC_SET_GP) + str(LL_OPEN_MAX) #+ str(TERM_CHAR_SEND)
_m_cmd_lid_open_neutral = "LL" + str(SC_SET_GP) + str(NEUTRAL_LL) #+ str(TERM_CHAR_SEND)
_m_cmd_prefix_HT = "HT" + str(SC_SET_GS)
_m_cmd_head_neutral_up = "HT" + str(SC_SET_GP) + str(HT_STARTLE) #+ str(TERM_CHAR_SEND)
_m_cmd_head_up_neutral = "HT" + str(SC_SET_GP) + str(NEUTRAL_HT) #+ str(TERM_CHAR_SEND)
_m_cmd_prefix_ET = "ET" + str(SC_SET_GS)
_m_cmd_eye_neutral_up = "ET" + str(SC_SET_GP) + str(ET_UP) #+ str(TERM_CHAR_SEND)
_m_cmd_eye_up_neutral = "ET" + str(SC_SET_GP) + str(NEUTRAL_ET) #+ str(TERM_CHAR_SEND)
headTiltTimedTest.enableHT( self )
rospy.loginfo("-----------------")
for _ms_duration in range(300, 100, -10):
## calculate GoalSpeed
_gs_LL = int( self.DC_helper.getGoalSpeed_ticks_durationMS(_delta_ticks_LL, _ms_duration) ) + 1
_gs_HT = int( self.DC_helper.getGoalSpeed_ticks_durationMS(_delta_ticks_HT, _ms_duration) ) + 1
_gs_ET = int( self.DC_helper.getGoalSpeed_ticks_durationMS(_delta_ticks_ET, _ms_duration) ) + 1
## preset eyelid to open GoalSpeed
_pub_cmd = _m_cmd_prefix_LL + str(_gs_LL)
_pub_cmd += _m_cmd_prefix_HT + str(_gs_HT)
_pub_cmd += _m_cmd_prefix_ET + str(_gs_ET)
_pub_cmd += str(TERM_CHAR_SEND)
timedTest.pubTo_maki_command( self, str(_pub_cmd) )
_sww_wi.sleepWhileWaitingMS( 100, 0.05 )
## publish eyelid neutral to LL_OPEN_MAX GoalPosition
timedTest.pubTo_maki_command( self, str(_m_cmd_lid_neutral_open + _m_cmd_head_neutral_up + _m_cmd_eye_neutral_up + TERM_CHAR_SEND) )
_start_lid_open_time = rospy.get_time()
_sww_wi.sleepWhileWaitingMS( _ms_duration, 0.01 )
_finish_lid_open_time = rospy.get_time()
_sww_wi.sleepWhileWaitingMS( 500, 0.01 )
## preset eyelid to neutral GoalSpeed
## return to neutral slowly
_pub_cmd = _m_cmd_prefix_LL + str(31) ## used spreadsheet to calculate these values
_pub_cmd += _m_cmd_prefix_HT + str(26)
_pub_cmd += _m_cmd_prefix_ET + str(100)
_pub_cmd += str(TERM_CHAR_SEND)
timedTest.pubTo_maki_command( self, str(_pub_cmd) )
_sww_wi.sleepWhileWaitingMS( 100, 0.05 )
## publish eyelid open to neutral GoalPosition
timedTest.pubTo_maki_command( self, str(_m_cmd_lid_open_neutral + _m_cmd_head_up_neutral + _m_cmd_eye_up_neutral + TERM_CHAR_SEND) )
_sww_wi.sleepWhileWaiting( 1 )
_total_lid_open_time = abs( _finish_lid_open_time - _start_lid_open_time )
startle_count += 1
rospy.loginfo( "Startle #" + str(startle_count)
+ ": GS_LL = " + str(_gs_LL)
+ ", GS_HT = " + str(_gs_HT)
+ ", GS_ET = " + str(_gs_ET)
+ "; expected time = " + str( _ms_duration )
+ "ms; actual lid_open time = " + str( _total_lid_open_time )
+ " seconds" )
rospy.loginfo("-----------------")
## make it easier to read
_sww_wi.sleepWhileWaiting( read_time, 0.5 )
## try to nicely end test
if self.mTT_ITERRUPT:
break ## break out of for loop
else:
## reset timers
_start_lid_open_time = None
_finish_lid_open_time = None
_total_lid_open_time = 0
# end for _ms_duration
headTiltTimedTest.disableHT( self )
return startle_count
class baseBehavior(object):
## all instances of this class share the same value
## variables private to this class
__maki_cmd_msg_format = None
__maki_feedback_format = None
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 start(self, makiPP=None):
_invalid_entry = dict( zip(F_VAL_SEQ, [ INVALID_INT ] * len(F_VAL_SEQ) ) )
## request a feedback message
baseBehavior.requestFeedback( self, str(SC_GET_PP) )
## check to see if there is an entry with key "PP"
#while (not rospy.is_shutdown() and self.mTT_INTERRUPT):
## 1026-06-12, KATE
## while ros is running and no interruptions have occurred
while (not rospy.is_shutdown() and not self.mTT_INTERRUPT):
## LEGACY CHECK
## if we were passed a valid makiPP
if (makiPP != None) and (makiPP != _invalid_entry):
self.makiPP = makiPP
break ## break the while loop
## if we got a message on /maki_feedback_pres_pos
if (self.makiPP != None) and (self.makiPP != _invalid_entry):
break ## break the while loop
#else:
# rospy.loginfo("Waiting for a message on /maki_feedback_pres_pos...")
# ## request a feedback message
# baseBehavior.requestFeedback( self, str(SC_GET_PP) )
#self.SWW_WI.sleepWhileWaitingMS( 250, end_early=False ) ## 0.25 second
rospy.loginfo("Waiting for a message on /maki_feedback_pres_pos...")
baseBehavior.requestFeedback( self, str(SC_GET_PP), time_ms=500 )
#end while (not rospy.is_shutdown() and self.mTT_INTERRUPT):
## 2016-06-16, KATE
#self.ALIVE = True
self.mTT_INTERRUPT = False
return
#######################
# stop at a planned break point
#######################
def stop(self):
self.mTT_INTERRUPT = True
#######################
# stop immediately
#######################
def abort(self):
self.ALIVE = False
self.mTT_INTERRUPT = True
def update( self, makiPP ):
self.makiPP = makiPP
def requestFeedback( self, feedback_type, cmd_prop=True, time_ms=100, time_inc=0.01 ):
## check /maki_feedback_*
## request a feedback message
#baseBehavior.pubTo_maki_command( self, str(SC_FEEDBACK) + str(feedback_type) + str(TERM_CHAR_SEND), cmd_prop, time_ms, time_inc )
baseBehavior.pubTo_maki_command( self, str(SC_FEEDBACK) + str(feedback_type) + str(TERM_CHAR_SEND), cmd_prop=cmd_prop, time_ms=time_ms, time_inc=time_inc )
return
#####################
## THESE ARE COMMON FOR ALL BEHAVIORS
#####################
def pubTo_maki_command( self, commandOut, cmd_prop=True, time_ms=100, time_inc=0.01 ):
_pub_flag = False
## make sure that commandOut ends in only one TERM_CHAR_SEND
_tmp = re.search( baseBehavior.__maki_cmd_msg_format, commandOut )
if _tmp != None:
## Yes, commandOut ends in only one TERM_CHAR_SEND
_pub_flag = True
#if self.VERBOSE_DEBUG: rospy.logdebug( str(commandOut) + " matched maki_msg_format" )
elif (commandOut == "reset"):
## special case handled by MAKI-Arbotix-Interface.py driver
_pub_flag = True
elif not commandOut.endswith( str(TERM_CHAR_SEND) ):
## append the missing TERM_CHAR_SEND
commandOut += str(TERM_CHAR_SEND)
_pub_flag = True
# if self.VERBOSE_DEBUG: rospy.logdebug( str(commandOut) + " added TERM_CHAR_SEND" )
rospy.logdebug( str(commandOut) + " added TERM_CHAR_SEND" )
else:
rospy.logerr( "Incorrect message format" + str(commandOut) )
# if self.VERBOSE_DEBUG: rospy.logdebug( str(commandOut) )
rospy.logdebug( str(commandOut) )
if _pub_flag and not rospy.is_shutdown():
self.ros_pub.publish( commandOut )
if (cmd_prop): self.SWW_WI.sleepWhileWaitingMS( time_ms, time_inc, end_early=False ) ## make sure command propogates
return
#####################
## Initialize ROS node
#####################
def initROS( self, nodename="anon" ):
## get function name for logging purposes
_fname = sys._getframe().f_code.co_name ## see http://stackoverflow.com/questions/5067604/determine-function-name-from-within-that-function-without-using-tracebacko
print str(_fname) + ": BEGIN" ## THIS IS BEFORE ROSNODE INIT
_anon_rosnode = False
if nodename == "anon":
_anon_rosnode = True
else:
## Parse nodename
## Examples passed: <__main__.headNod object at 0x7f24b3e07150>
## <__main__.lookAlissa object at 0xb6c91bac>
#print str(nodename)
_tmp_nodename = str(nodename)
_object_name_format = "^\<\_\_main\_\_\.(\w+)"
#print _object_name_format
_tmp = re.search( _object_name_format, _tmp_nodename )
if _tmp != None:
nodename = _tmp.group(1)
#print nodename
else:
## Actually... this is already an initialized rosnode, so quick, exit!
self.ros_pub = nodename
#print nodename
return
# see http://wiki.ros.org/rospy/Overview/Logging
# if self.VERBOSE_DEBUG:
# self.ros_pub = rospy.init_node(str(nodename), anonymous=_anon_rosnode, log_level=rospy.DEBUG)
# rospy.logdebug("log_level=rospy.DEBUG")
# else:
self.ros_pub = rospy.init_node(nodename, anonymous=_anon_rosnode) ## defaults to log_level=rospy.INFO
rospy.logdebug("anonymous=" + str(_anon_rosnode))
rospy.loginfo( str(_fname) + ": END")
return
#####################
## Set up publisher to /maki_command
#####################
def initPubMAKI(self):
## get function name for logging purposes
_fname = sys._getframe().f_code.co_name ## see http://stackoverflow.com/questions/5067604/determine-function-name-from-within-that-function-without-using-tracebacko
rospy.logdebug( str(_fname) + ": BEGIN")
# Setup publisher
self.ros_pub = rospy.Publisher("maki_command", String, queue_size = 10)
rospy.logdebug( str(_fname) + ": END")
return
#####################
## Set up regex format for publishing on /maki_command
#####################
def initPubMAKIFormat(self):
## get function name for logging purposes
_fname = sys._getframe().f_code.co_name ## see http://stackoverflow.com/questions/5067604/determine-function-name-from-within-that-function-without-using-tracebacko
rospy.logdebug( str(_fname) + ": BEGIN")
## make sure that commandOut ends in only one TERM_CHAR_SEND
baseBehavior.__maki_cmd_msg_format = "\A[a-yA-Y]+[a-yA-Y0-9]*"
baseBehavior.__maki_cmd_msg_format += str(TERM_CHAR_SEND)
baseBehavior.__maki_cmd_msg_format += "{1}$"
rospy.loginfo( str(_fname) + ": END")
return
def initSubMAKIFormat( self ):
## Setup regex template for expected feedback syntax
_feedback_msg_format = "\A([A-Z]{2})" ## 2 alphabetic char prefix
_feedback_msg_format += "(([0-9]+" + DELIMITER_RECV + "){" + str(SERVOCOUNT-1) + "}[0-9]+)"
_feedback_msg_format += TERM_CHAR_RECV + "\Z"
#print _feedback_msg_format
baseBehavior.__maki_feedback_format = re.compile(_feedback_msg_format)
return baseBehavior.__maki_feedback_format
def initROSSub( self, feedback ):
#print feedback
# Setup subscribers
for _prefix, _sub_params in feedback.iteritems():
_topic = _sub_params[0]
_type = _sub_params[1]
_callback = _sub_params[2]
rospy.Subscriber( _topic, _type, _callback )
rospy.logdebug( "now subscribed to " + str(_topic) )
return
def initSubMAKIFeedback( self ):
_maki_feedback_sub = {} ## init as empty dictionary
_maki_feedback_sub[ str(SC_GET_PP) ] = ("maki_feedback_pres_pos", String, self.parseMAKIFeedbackMsg)
_maki_feedback_sub[ str(SC_GET_ER) ] = ("maki_feedback_error", String, self.parseMAKIFeedbackMsg)
baseBehavior.initROSSub( self, _maki_feedback_sub )
return
def parseMAKIFeedbackMsg ( self, recv_msg ):
#if self.VERBOSE_DEBUG:
# #rospy.logdebug( "parseMAKIFeedbackMsg: BEGIN" )
# rospy.logdebug( "Received: " + str(recv_msg.data) )
_tmp = baseBehavior.__maki_feedback_format.search( recv_msg.data )
if _tmp != None:
_prefix = _tmp.group(1)
_feedback_values = _tmp.group(2)
#print "Validated: prefix='" + _prefix + "' and feedback_values='" + _feedback_values + "'"
else:
rospy.logerr( "Received with ERROR! Invalid message format: " + str(recv_msg) )
return ## return without an expression argument returns None. Falling off the end of a function also returns None
_values = re.findall("([0-9]+)", _feedback_values) ## this is a list of strings
## need to conver to int (see http://stackoverflow.com/questions/22672598/converting-lists-of-digits-stored-as-strings-into-integers-python-2-7)
_tmp_dict = dict( zip(F_VAL_SEQ, map(int, _values)) )
if (len(self.maki_feedback_values) == 0) or not ( str(_prefix) in self.maki_feedback_values ):
## if no _prefix entry exists in the dictionary, add new
self.maki_feedback_values[ str(_prefix) ] = _tmp_dict
rospy.logdebug( "New entry added to self.maki_feedback_values" + str(recv_msg) )
elif not (_tmp_dict == self.maki_feedback_values[ str(_prefix) ]):
## if _prefix entry exists, update
self.maki_feedback_values[ str(_prefix) ].update( _tmp_dict )
#rospy.loginfo( "Updated entry in self.maki_feedback_values" + str(recv_msg) )
rospy.logdebug( "Updated entry in self.maki_feedback_values" + str(recv_msg) )
else:
pass
## update
if str(_prefix) == str(SC_GET_PP):
self.makiPP = _tmp_dict
#print "parseMAKIFeedbackMsg: END"
return
#####################
##
## This is blocking
##
## Raises rospy.exceptions.ROSException when breaks on stall
##
## TODO:
## * Estimate duration based on the largest difference
## between current and goal positions
#####################
def monitorMoveToGP( self, gp_cmd, hp_gp=None, ht_gp=None, ll_gp=None, lr_gp=None, ep_gp=None, et_gp=None, delta_pp=DELTA_PP, cmd_prop=True, msg_id=None):
### SEND THE COMMAND
baseBehavior.pubTo_maki_command( self, gp_cmd, cmd_prop=cmd_prop )
# This publishes the status of the behavior. We will send this to the VH
#pub_behavior = rospy.Publisher("behavior_status",String,queue_size=10)
rospy.logdebug("monitorMoveToGP(): delta_pp=" + str(delta_pp) + "; gp_cmd=" + gp_cmd)
_moving_flag = dict( zip(F_VAL_SEQ, [None]*len(F_VAL_SEQ)) )
_count_moving_flags = 0
if (hp_gp != None) and (hp_gp != INVALID_INT):
_moving_flag["HP"] = True
_count_moving_flags = _count_moving_flags +1
if (ht_gp != None) and (ht_gp != INVALID_INT):
_moving_flag["HT"] = True
_count_moving_flags = _count_moving_flags +1
if (ll_gp != None) and (ll_gp != INVALID_INT):
_moving_flag["LL"] = True
_count_moving_flags = _count_moving_flags +1
if (lr_gp != None) and (lr_gp != INVALID_INT):
_moving_flag["LR"] = True
_count_moving_flags = _count_moving_flags +1
if (ep_gp != None) and (ep_gp != INVALID_INT):
_moving_flag["EP"] = True
_count_moving_flags = _count_moving_flags +1
if (et_gp != None) and (et_gp != INVALID_INT):
_moving_flag["ET"] = True
_count_moving_flags = _count_moving_flags +1
if _count_moving_flags == 0: return
## NOTE: COMMENTED OUT... Let the behaviors themselves increment
## Otherwise, duplicative increment is likely
#self.count_movements = self.count_movements +1
_stall_count = 0
_loop_count = 0
_old_makiPP = self.makiPP
_start_time = rospy.get_time()
### REPEAT SENDING THE COMMAND
baseBehavior.pubTo_maki_command( self, gp_cmd, cmd_prop=False )
#while not rospy.is_shutdown():
## 2016-06-16, KATE
while ((not rospy.is_shutdown()) and (not self.mTT_INTERRUPT)):
_loop_count += 1
## There is an implicit sleep in requestFeedback of 100ms (default)
baseBehavior.requestFeedback( self, SC_GET_PP )
### TODO: THIS WOULD BE A NICE OPTIMIZATION
### Check to see if the goal position command has propogated
### If so, there should be changes in PP
### Otherwise, catch a breath... continue
### instead of inflating _stall_count
#if (_old_makiPP == self.makiPP):
# rospy.logwarn("baseBehavior.monitorMoveToGP(): CONTINUE!!!")
# continue ## jump back to the top of the loop
if _moving_flag["HP"] and (abs(self.makiPP["HP"] - hp_gp) <= delta_pp):
rospy.logdebug("HP done moving")
_moving_flag["HP"] = False
_count_moving_flags = _count_moving_flags -1
if _moving_flag["HT"] and (abs(self.makiPP["HT"] - ht_gp) <= delta_pp):
rospy.logdebug("HT done moving")
_moving_flag["HT"] = False
_count_moving_flags = _count_moving_flags -1
if _moving_flag["EP"] and (abs(self.makiPP["EP"] - ep_gp) <= delta_pp):
rospy.logdebug("EP done moving")
_moving_flag["EP"] = False
_count_moving_flags = _count_moving_flags -1
if _moving_flag["ET"] and (abs(self.makiPP["ET"] - et_gp) <= delta_pp):
rospy.logdebug("ET done moving")
_moving_flag["ET"] = False
_count_moving_flags = _count_moving_flags -1
if _moving_flag["LL"] and (abs(self.makiPP["LL"] - ll_gp) <= delta_pp):
rospy.logdebug("LL done moving")
_moving_flag["LL"] = False
_count_moving_flags = _count_moving_flags -1
if _moving_flag["LR"] and (abs(self.makiPP["LR"] - lr_gp) <= delta_pp):
rospy.logdebug("LR done moving")
_moving_flag["LR"] = False
_count_moving_flags = _count_moving_flags -1
if _count_moving_flags == 0: break
if (_old_makiPP == self.makiPP):
_stall_count += 1
rospy.logdebug("... _stall_count = " + str(_stall_count) )
## 2016-07-21 ktsui: comment out
#baseBehavior.requestFeedback( self, SC_GET_PP )
#baseBehavior.requestFeedback( self, SC_GET_PP, time_ms=250 )
## KATE
if _stall_count != _loop_count:
rospy.logdebug("potential STALL RECOVERY?? _loop_count=" + str(_loop_count) + ", _stall_count=" + str(_stall_count))
if (_stall_count == 10):
#rospy.logerr("STALLED!!!")
raise rospy.exceptions.ROSException("STALLED!!!! self.makiPP hasn't changed... is the motor at its limit?")
# pub_behavior.publish("{} STALLED".format(msg_id))
break
_old_makiPP = self.makiPP
_duration = abs(rospy.get_time() - _start_time)
rospy.logdebug("monitorMoveToGP() **** DONE! movement took " + str(_duration) + " seconds")
#pub_behavior.publish("{} COMPLETED".format(msg_id))
return
## Funtion: Verifes if the current pose matches the specified pose (within tolerance delta_pp)
## Returns: True or False
def verifyPose( self, hp=None, ht=None, ll=None, lr=None, ep=None, et=None, delta_pp=DELTA_PP ):
_ret = False
_request_verify_flag = dict( zip(F_VAL_SEQ, [None]*len(F_VAL_SEQ)) )
_count_request_verify_flags = 0
if (hp != None) and (hp != INVALID_INT):
_request_verify_flag["HP"] = True
_count_request_verify_flags = _count_request_verify_flags +1
if (ht != None) and (ht != INVALID_INT):
_request_verify_flag["HT"] = True
_count_request_verify_flags = _count_request_verify_flags +1
if (ll != None) and (ll != INVALID_INT):
_request_verify_flag["LL"] = True
_count_request_verify_flags = _count_request_verify_flags +1
if (lr != None) and (lr != INVALID_INT):
_request_verify_flag["LR"] = True
_count_request_verify_flags = _count_request_verify_flags +1
if (ep != None) and (ep != INVALID_INT):
_request_verify_flag["EP"] = True
_count_request_verify_flags = _count_request_verify_flags +1
if (et != None) and (et != INVALID_INT):
_request_verify_flag["ET"] = True
_count_request_verify_flags = _count_request_verify_flags +1
if _count_request_verify_flags == 0:
rospy.logdebug("verifyPose(): INVALID goal poses")
return _ret #False
## request feedback of present positions
baseBehavior.requestFeedback( self, SC_GET_PP )
_makiPP = self.makiPP
## build dictionary to verify
_verify_dict = {}
_verify_dict["HP"] = hp
_verify_dict["HT"] = ht
_verify_dict["EP"] = ep
_verify_dict["ET"] = et
_verify_dict["LL"] = ll
_verify_dict["LR"] = lr
for _motor in F_VAL_SEQ:
if _request_verify_flag[_motor]:
if (abs(_makiPP[_motor] - _verify_dict[_motor]) < delta_pp):
rospy.logdebug( _motor + " present position matches goal position")
_request_verify_flag[_motor] = False
_count_request_verify_flags = _count_request_verify_flags -1
else:
_ret = False
if self.VERBOSE_DEBUG:
rospy.logwarn("verifyPose(): " + _motor + " present position (" + str(_makiPP[_motor]) + ") DOES NOT matches goal position (" + str(_verify_dict[_motor]) + ")" )
else:
break ## break the for loop; no sense in performing unnecessary calculations
#end for _motor in F_VAL_SEQ:
if _count_request_verify_flags == 0: _ret = True
return _ret
# blocks until movement is stable for 200ms or until 4 seconds have passed.
def waitForMovementToComplete(self):
lastPP = self.makiPP
equalTimes = 0
totalTimes = 0
maxTimes = 80
while not rospy.is_shutdown() and equalTimes < 4 and totalTimes < maxTimes:
# sleeps 50ms as well as requests.
baseBehavior.requestFeedback( self, SC_GET_PP, time_ms=50 )
if lastPP == self.makiPP:
equalTimes += 1
totalTimes += 1
else:
equalTimes = 0
lastPP = self.makiPP
if totalTimes >= maxTimes:
rospy.logerror('Waiting for movement to complete for more than ' + str(maxTimes * 50) + 'ms. Is something wrong?')
def helper_macroTurnToScreen( self, turn_count, read_time=1.0 ):
_sww_wi = ROS_sleepWhileWaiting_withInterrupt( verbose_debug=self.VERBOSE_DEBUG )
_start_turn_time = None
_finish_turn_time = None
_total_turn_time = 0
## neutral to HT_DOWN
NEUTRAL_HT = HT_MIDDLE
#_delta_ticks_HT = abs( NEUTRAL_HT - HT_DOWN )
_delta_ticks_HT = 10
## neutral to HP_LEFT (same delta as HP_RIGHT)
NEUTRAL_HP = HP_FRONT
_delta_ticks_HP = abs( NEUTRAL_HP - HP_LEFT )
## neutral to ET_DOWN
NEUTRAL_ET = ET_MIDDLE
#_delta_ticks_ET = abs( NEUTRAL_ET - ET_DOWN )
_delta_ticks_ET = _delta_ticks_HT
## neutral to EP_LEFT (same delta as EP_RIGHT)
NEUTRAL_EP = EP_FRONT
_delta_ticks_EP = abs( NEUTRAL_EP - EP_LEFT )
rospy.logerr("delta_ticks[LL, HT, HP, ET, EP] = "
#+ str(_delta_ticks_LL)
+ ", " + str(_delta_ticks_HT)
+ ", " + str(_delta_ticks_HP)
+ ", " + str(_delta_ticks_ET)
+ ", " + str(_delta_ticks_EP) )
## maki_cmd
_m_cmd_prefix_HP = "HP" + str(SC_SET_GP)
_m_cmd_head_neutral_left = _m_cmd_prefix_HP + str(HP_LEFT)
_m_cmd_head_neutral_right = _m_cmd_prefix_HP + str(HP_RIGHT)
_m_cmd_head_lr_neutral = _m_cmd_prefix_HP + str(NEUTRAL_HP)
_m_cmd_prefix_HP = "HP" + str(SC_SET_GS)
_m_cmd_prefix_HT = "HT" + str(SC_SET_GP)
#_m_cmd_head_neutral_offlevel = _m_cmd_prefix_HT + str(HT_DOWN)
_m_cmd_head_neutral_offlevel = _m_cmd_prefix_HT
_m_cmd_head_offlevel_neutral = _m_cmd_prefix_HT + str(NEUTRAL_HT)
_m_cmd_prefix_HT = "HT" + str(SC_SET_GS)
_m_cmd_prefix_ET = "ET" + str(SC_SET_GP)
#_m_cmd_eye_neutral_offlevel = _m_cmd_prefix_ET + str(ET_DOWN)
_m_cmd_eye_neutral_offlevel = _m_cmd_prefix_ET
_m_cmd_eye_offlevel_neutral = _m_cmd_prefix_ET + str(NEUTRAL_ET)
_m_cmd_prefix_ET = "ET" + str(SC_SET_GS)
_m_cmd_prefix_EP = "EP" + str(SC_SET_GP)
_m_cmd_eye_neutral_left = _m_cmd_prefix_EP + str(EP_LEFT)
_m_cmd_eye_neutral_right = _m_cmd_prefix_EP + str(EP_RIGHT)
_m_cmd_eye_lr_neutral = _m_cmd_prefix_EP + str(NEUTRAL_EP)
_m_cmd_prefix_EP = "EP" + str(SC_SET_GS) ## fixed typo
headTiltTimedTest.enableHT( self )
_sww_wi.sleepWhileWaitingMS(1000, 0.05)
rospy.loginfo("-----------------")
for _ms_duration in range(2000, 500, -250): ## [2000, 750] ms
## generate some offlevel variation
_delta_ticks_offlevel = random.randint(-20,20)
rospy.loginfo("delta_ticks_offlevel = " + str(_delta_ticks_offlevel))
_m_cmd_head_neutral_offlevel = "HT" + str(SC_SET_GP) + str(NEUTRAL_HT + _delta_ticks_offlevel)
_m_cmd_eye_neutral_offlevel = "ET" + str(SC_SET_GP) + str(NEUTRAL_ET + _delta_ticks_offlevel)
_delta_ticks_HT = _delta_ticks_offlevel
_delta_ticks_ET = _delta_ticks_offlevel
headTiltTimedTest.enableHT( self )
## calculate GoalSpeed
_gs_HT = abs( int( self.DC_helper.getGoalSpeed_ticks_durationMS(_delta_ticks_HT, _ms_duration) ) + 1 )
_gs_HP = abs( int( self.DC_helper.getGoalSpeed_ticks_durationMS(_delta_ticks_HP, _ms_duration) ) + 1 )
## eyes should finish first
#_ms_duration_eyes = int( (float(_ms_duration) * 0.3) + 0.5 )
_ms_duration_eyes = _ms_duration
_gs_ET = abs( int( self.DC_helper.getGoalSpeed_ticks_durationMS(_delta_ticks_ET, _ms_duration_eyes) ) + 1 )
#_gs_EP = int( self.DC_helper.getGoalSpeed_ticks_durationMS(_delta_ticks_EP, _ms_duration_eyes) ) + 1
rospy.logerr("ms_duration_eyes = " + str(_ms_duration_eyes) + "ms")
### preset eyelid to open GoalSpeed
_pub_cmd = _m_cmd_prefix_HT + str(_gs_HT)
_pub_cmd += _m_cmd_prefix_HP + str(_gs_HP)
_pub_cmd += _m_cmd_prefix_ET + str(_gs_ET)
#_pub_cmd += _m_cmd_prefix_EP + str(_gs_EP)
_pub_cmd += str(TERM_CHAR_SEND)
timedTest.pubTo_maki_command( self, str(_pub_cmd) )
_sww_wi.sleepWhileWaitingMS( 100, 0.05 )
## publish neutral to GoalPosition: head turned to face left or right screen
_pub_cmd = str(_m_cmd_head_neutral_offlevel + _m_cmd_eye_neutral_offlevel )
## used for choosing direction of turn
## 0 = left, 1 = right
if (random.randint(0,1) == 0):
#_pub_cmd += str(_m_cmd_head_neutral_left + _m_cmd_eye_neutral_left)
_pub_cmd += str(_m_cmd_head_neutral_left)
rospy.loginfo("TURN TO RIGHT SCREEN")
else:
#_pub_cmd += str(_m_cmd_head_neutral_right + _m_cmd_eye_neutral_right)
_pub_cmd += str(_m_cmd_head_neutral_right)
rospy.loginfo("TURN TO LEFT SCREEN")
#_pub_cmd += str(SC_SET_IPT) + str(_ms_duration)
_pub_cmd += str(TERM_CHAR_SEND)
timedTest.pubTo_maki_command( self, str(_pub_cmd) )
_start_turn_time = rospy.get_time()
rospy.logerr("Start TURN...")
_sww_wi.sleepWhileWaitingMS( _ms_duration, 0.01, False )
_finish_turn_time = rospy.get_time()
_total_turn_time = abs( _finish_turn_time - _start_turn_time )
turn_count += 1
rospy.logerr( "Turn #" + str(turn_count)
#+ ": GS_LL = " + str(_gs_LL)
+ ", GS_HT = " + str(_gs_HT)
+ ", GS_HP = " + str(_gs_HP)
+ ", GS_ET = " + str(_gs_ET)
#+ ", GS_EP = " + str(_gs_EP)
+ "; expected time = " + str( _ms_duration )
+ "ms; actual turn time = " + str( _total_turn_time )
+ " seconds" )
rospy.loginfo("-----------------")
## make it easier to read
_sww_wi.sleepWhileWaiting( read_time, 0.5 )
#_sww_wi.sleepWhileWaitingMS( 1000, 0.01 )
## preset eyelid to neutral GoalSpeed
## return to neutral in a discernable manner from falling turn
#_pub_cmd = _m_cmd_prefix_LL + str(16) ## used spreadsheet to calculate these values
#_pub_cmd += _m_cmd_prefix_HT + str(13)
#_pub_cmd += _m_cmd_prefix_HP + str(88)
#_pub_cmd += _m_cmd_prefix_EP + str(22)
#_pub_cmd += _m_cmd_prefix_ET + str(50)
#_pub_cmd += str(TERM_CHAR_SEND)
#timedTest.pubTo_maki_command( self, str(_pub_cmd) )
#_sww_wi.sleepWhileWaitingMS( 100, 0.05 )
##timedTest.pubTo_maki_command( self, "reset" )
##_sww_wi.sleepWhileWaitingMS( 100, 0.05 )
##timedTest.pubTo_maki_command( self, "reset" )
##_sww_wi.sleepWhileWaitingMS( 100, 0.05 )
## return to neutral GoalPosition
_pub_cmd = str(_m_cmd_head_offlevel_neutral + _m_cmd_eye_offlevel_neutral )
_pub_cmd += str(_m_cmd_head_lr_neutral) #+ _m_cmd_eye_lr_neutral)
_pub_cmd += str(SC_SET_IPT) + str(resetting_time)
_pub_cmd += str(TERM_CHAR_SEND)
timedTest.pubTo_maki_command( self, str(_pub_cmd) )
rospy.loginfo("Start waiting for reset neutral position and speed")
_sww_wi.sleepWhileWaitingMS( resetting_time+1000, 0.05 )
rospy.loginfo("Done waiting for reset neutral position and speed")
## try to nicely end test
if self.mTT_INTERRUPT:
break ## break out of for loop
else:
## reset timers
_start_turn_time = None
_finish_turn_time = None
_total_turn_time = 0
# end for _ms_duration
return turn_count
def helper_macroBlink(self,
blink_count,
full_blink,
blink_rep,
read_time=1.0):
_sww_wi = ROS_sleepWhileWaiting_withInterrupt()
#global blink_time ## list of ints
_start_blink_time = None
_start_blink_close_time = None
_finish_blink_close_time = None
_start_blink_open_time = None
_finish_blink_open_time = None
_finish_blink_time = None
_total_blink_time = 0
## maki_command prefix
_m_cmd_prefix = "LL" + SC_SET_GP
rospy.loginfo("blink_rep=" + str(blink_rep))
rospy.loginfo("-----------------")
for _blink_time in blink_time:
_half_blink_time = int(float(_blink_time) * 0.5 + 0.5)
rospy.loginfo("blink_time=" + str(_blink_time) +
"; half_blink_time=" + str(_half_blink_time))
#_close_blink_time = int( float(_blink_time) * 0.35 + 0.5 )
#_open_blink_time = _blink_time - _close_blink_time
## maki_command suffix
#_m_cmd_suffix = SC_SET_IPT + str( _half_blink_time ) + TERM_CHAR_SEND
_m_cmd_suffix = TERM_CHAR_SEND
_blink_close = _m_cmd_prefix
if full_blink:
_blink_close += str(LL_CLOSE_MAX) + _m_cmd_suffix
blinkTest.presetGoalSpeed_ticks_durationMS(
self, "LL", abs(LL_CLOSE_MAX - LL_OPEN_DEFAULT),
_half_blink_time)
else:
_blink_close += str(LL_CLOSE_HALF) + _m_cmd_suffix
blinkTest.presetGoalSpeed_ticks_durationMS(
self, "LL", abs(LL_CLOSE_HALF - LL_OPEN_DEFAULT),
_half_blink_time)
_blink_open = _m_cmd_prefix + str(LL_OPEN_DEFAULT) + _m_cmd_suffix
_start_blink_time = rospy.get_time()
#for blink_rep in range(1, 6):
for _flutter_count in range(0, blink_rep):
rospy.logdebug(str(_flutter_count))
## blink close
_start_blink_close_time = rospy.get_time()
rospy.logdebug("blink close")
timedTest.pubTo_maki_command(self, str(_blink_close))
_sww_wi.sleepWhileWaitingMS(_half_blink_time, 0.01)
#_sww_wi.sleepWhileWaitingMS( _close_blink_time, 0.01 )
_finish_blink_close_time = rospy.get_time()
## blink open
_start_blink_open_time = rospy.get_time()
rospy.logdebug("blink open")
timedTest.pubTo_maki_command(self, str(_blink_open))
_sww_wi.sleepWhileWaitingMS(_half_blink_time, 0.01)
#_sww_wi.sleepWhileWaitingMS( _open_blink_time, 0.01 )
_finish_blink_open_time = rospy.get_time()
_finish_blink_time = rospy.get_time()
if self.mTT_INTERRUPT:
rospy.logerr("Abort blink_test")
break ## break out of inner for loop
# end for _flutter_count in range(1, blink_rep):
_finish_blink_time = rospy.get_time()
## make it easier to read
_sww_wi.sleepWhileWaiting(read_time, .5)
blink_count += 1
_total_blink_time = abs(_finish_blink_time - _start_blink_time)
#rospy.loginfo( "Completed " + str(blink_count) + " full eye blinks" )
rospy.loginfo(
"Eye blink #" + str(blink_count) + ": full eye blink = " +
str(_total_blink_time) + "; blink open->close = " +
str(abs(_finish_blink_close_time - _start_blink_close_time)) +
"; blink close->open = " +
str(abs(_finish_blink_open_time - _start_blink_open_time)))
rospy.loginfo("-----------------")
## try to nicely end testing the eye blink
if self.mTT_INTERRUPT:
break ## break out of outer for loop
else:
## reset timers
_start_blink_time = None
_start_blink_close_time = None
_finish_blink_close_time = None
_start_blink_open_time = None
_finish_blink_open_time = None
_finish_blink_time = None
_total_blink_time = 0
# end for _blink_time in blink_time:
return blink_count
class lookRosy(headTiltBaseBehavior):
## variables private to this class
## all instances of this class share the same value
# none
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
###########################
##
## To run, publish to /maki_macro
## lookAtRosy
##
###########################
def macroLookAtRosy(self):
rospy.logdebug("macrolookAtRosy: BEGIN")
print(
"====================HP_EXPERIMENTER VALUE: {}==================".
format(HP_EXPERIMENTER))
if self.ALIVE:
if not rospy.is_shutdown():
rospy.logdebug("NOT rospy.is_shutdown()")
pass
else:
return
if self.mTT_INTERRUPT:
rospy.logdebug("mTT_INTERRUPT=" + str(mTT_INTERRUPT))
return
if not self.mTT_INTERRUPT:
rospy.loginfo("-----------------")
baseBehavior.pubTo_maki_command(self, str(self.look_at))
self.sww_wi.sleepWhileWaitingMS(2000)
rospy.loginfo("-----------------")
#end if not self.mTT_INTERRUPT:
#end if self.ALIVE:
rospy.logdebug("macroLookAtRosy: END")
return
###########################
##
## To run, publish to /maki_macro
## lookAwayAlissa
##
###########################
def macroLookAwayFromRosy(self):
rospy.logdebug("macroLookAwayFromRosy: BEGIN")
if self.ALIVE:
if not rospy.is_shutdown():
rospy.logdebug("NOT rospy.is_shutdown()")
pass
else:
return
if self.mTT_INTERRUPT:
rospy.logdebug("mTT_INTERRUPT=" + str(mTT_INTERRUPT))
return
if not self.mTT_INTERRUPT:
rospy.loginfo("-----------------")
baseBehavior.pubTo_maki_command(self, str(self.look_away))
self.sww_wi.sleepWhileWaitingMS(2000)
rospy.loginfo("-----------------")
#end if not self.mTT_INTERRUPT:
#end if self.ALIVE:
rospy.logdebug("macroLookAwayFromRosy: END")
return
###########################
##
## To run, publish to /maki_macro
## lookNeutral
##
###########################
def macroLookNeutral(self):
rospy.logdebug("macroNeutral: BEGIN")
if self.ALIVE:
if not rospy.is_shutdown():
rospy.logdebug("NOT rospy.is_shutdown()")
pass
else:
return
if self.mTT_INTERRUPT:
rospy.logdebug("mTT_INTERRUPT=" + str(mTT_INTERRUPT))
return
if not self.mTT_INTERRUPT:
rospy.loginfo("-----------------")
baseBehavior.pubTo_maki_command(self, str(self.look_neutral))
self.sww_wi.sleepWhileWaitingMS(2000)
rospy.loginfo("-----------------")
#end if not self.mTT_INTERRUPT:
#end if self.ALIVE:
rospy.logdebug("macroLookNeutral: END")
return
def parse_maki_macro(self, msg):
print msg.data
if msg.data == "lookAtRosy":
## try to nicely startup without jerking MAKI's head tilt servo
headTiltBaseBehavior.start(self)
self.macroLookAtRosy()
headTiltBaseBehavior.stop(self)
elif msg.data == "lookAwayFromRosy":
## try to nicely startup without jerking MAKI's head tilt servo
headTiltBaseBehavior.start(self)
self.macroLookAwayFromRosy()
headTiltBaseBehavior.stop(self)
elif msg.data == "lookNeutral":
## try to nicely startup without jerking MAKI's head tilt servo
headTiltBaseBehavior.start(self)
self.macroLookNeutral()
headTiltBaseBehavior.stop(self)
else:
pass
return
def macroHeadNod(self):
## different versions of head nodding
_v1 = False
_v2 = True
_sww_wi = ROS_sleepWhileWaiting_withInterrupt(
verbose_debug=self.VERBOSE_DEBUG)
self.ALIVE = True
rospy.logdebug("macroHeadNod: BEGIN")
_start_headnod_time = None
_start_headnod_up_time = None
_start_headnod_down_time = None
_finish_headnod_up_time = None
_finish_headnod_down_time = None
_finish_headnod_time = None
_headnod_max_min_dist = float(abs(HT_UP - HT_DOWN))
_headnod_middle_down_dist = float(abs(HT_MIDDLE - HT_DOWN))
_headnod_middle_up_dist = float(abs(HT_MIDDLE - HT_UP))
## debugging
#print "_headnod_max_min_dist = "
#print _headnod_max_min_dist
#print "str(" + str(_headnod_max_min_dist) + ")"
##
#print "_headnod_middle_down_dist = "
#print _headnod_middle_down_dist
#print "str(" + str(_headnod_middle_down_dist) + ")"
##
#print "_headnod_middle_up_dist = "
#print _headnod_middle_up_dist
#print "str(" + str(_headnod_middle_up_dist) + ")"
##
#print _headnod_middle_down_dist / _headnod_max_min_dist
#print _headnod_middle_up_dist / _headnod_max_min_dist
_ipt_nod_middle_down = float(
(_headnod_middle_down_dist / _headnod_max_min_dist) * IPT_NOD)
_ipt_nod_middle_up = float(
(_headnod_middle_up_dist / _headnod_max_min_dist) * IPT_NOD)
_ipt_nod_middle_down = int(_ipt_nod_middle_down +
0.5) ## implicit rounding
_ipt_nod_middle_up = int(_ipt_nod_middle_up +
0.5) ## implicit rounding
rospy.logdebug("(full)\tIPT_NOD = " + str(IPT_NOD) + "ms")
rospy.logdebug("(partial)\t_ipt_nod_middle_down = " +
str(_ipt_nod_middle_down) + "ms")
rospy.logdebug("(partial)\t_ipt_nod_middle_up = " +
str(_ipt_nod_middle_up) + "ms")
rospy.logdebug("(summed partial)\t_ipt_nod_middle_* = " +
str(_ipt_nod_middle_down + _ipt_nod_middle_up) + "ms")
## maki_command prefix
_m_cmd_prefix = "HT" + SC_SET_GP
## nod macros
_nod_up_down = _m_cmd_prefix + str(HT_DOWN) + SC_SET_IPT + str(
IPT_NOD) + TERM_CHAR_SEND
_nod_down_up = _m_cmd_prefix + str(HT_UP) + SC_SET_IPT + str(
IPT_NOD) + TERM_CHAR_SEND
_nod_middle_up = _m_cmd_prefix + str(HT_UP) + SC_SET_IPT + str(
_ipt_nod_middle_up) + TERM_CHAR_SEND
_nod_middle_down = _m_cmd_prefix + str(HT_DOWN) + SC_SET_IPT + str(
_ipt_nod_middle_down) + TERM_CHAR_SEND
_nod_up_middle = _m_cmd_prefix + str(HT_MIDDLE) + SC_SET_IPT + str(
_ipt_nod_middle_up) + TERM_CHAR_SEND
_nod_down_middle = _m_cmd_prefix + str(HT_MIDDLE) + SC_SET_IPT + str(
_ipt_nod_middle_down) + TERM_CHAR_SEND
## this is a nested while loop
_print_once = True
while self.ALIVE:
if not rospy.is_shutdown():
pass
else:
break ## break out of outer while loop
if _print_once:
rospy.logdebug("Entering macroHeadNod outer while loop")
_print_once = False
if self.mTT_INTERRUPT:
#print "start sleep 5"
_sww_wi.sleepWhileWaiting(5) # 5 seconds
#print "end sleep 5"
continue ## begin loop again from the beginning skipping below
#print "shouldn't get here"
## try to nicely startup headnod testing without jerking MAKI's head tilt servo
headTiltTimedTest.enableHT(self)
## where is MAKI's HT closest to currently? HT_UP, HT_MIDDLE, HT_DOWN
_ht_pp = self.makiPP["HT"]
_delta_ht_pp = abs(_ht_pp - HT_MIDDLE)
_ht_macro_pose = HT_MIDDLE
if (abs(_ht_pp - HT_UP) < _delta_ht_pp):
_delta_ht_pp = abs(_ht_pp - HT_UP)
_ht_macro_pose = HT_UP
if (abs(_ht_pp - HT_DOWN) < _delta_ht_pp):
_delta_ht_pp = abs(_ht_pp - HT_DOWN)
_ht_macro_pose = HT_DOWN
## publish GP of _ht_macro_pose in case in between poses
timedTest.pubTo_maki_command(
self, "HT" + SC_SET_GP + str(_ht_macro_pose) + TERM_CHAR_SEND)
if (_ht_macro_pose == HT_MIDDLE):
## looking up first has strongest nodding cue
timedTest.pubTo_maki_command(self, str(_nod_middle_up))
_sww_wi.sleepWhileWaitingMS(_ipt_nod_middle_up)
rospy.logdebug("Entering macroHeadNod inner while loop")
_nod_count = 0
while not self.mTT_INTERRUPT:
rospy.loginfo("-----------------")
## VERSION 1: UP --> MIDDLE --> DOWN --> MIDDLE --> UP
if _v1:
_start_headnod_time = rospy.get_time()
_start_headnod_down_time = rospy.get_time()
timedTest.pubTo_maki_command(self, str(_nod_up_middle))
_sww_wi.sleepWhileWaitingMS(_ipt_nod_middle_up)
timedTest.pubTo_maki_command(self, str(_nod_middle_down))
_sww_wi.sleepWhileWaitingMS(_ipt_nod_middle_down)
timedTest.pubTo_maki_command(self, str(_nod_middle_down))
_sww_wi.sleepWhileWaitingMS(_ipt_nod_middle_down)
_finish_headnod_down_time = rospy.get_time()
_start_headnod_up_time = rospy.get_time()
timedTest.pubTo_maki_command(self, str(_nod_down_middle))
_sww_wi.sleepWhileWaitingMS(_ipt_nod_middle_down)
timedTest.pubTo_maki_command(self, str(_nod_middle_up))
_sww_wi.sleepWhileWaitingMS(_ipt_nod_middle_up)
_finish_headnod_up_time = rospy.get_time()
_finish_headnod_time = rospy.get_time()
## VERSION 2: UP --> DOWN --> UP
if _v2:
_start_headnod_time = rospy.get_time()
_start_headnod_down_time = rospy.get_time()
timedTest.pubTo_maki_command(self, str(_nod_up_down))
_sww_wi.sleepWhileWaitingMS(IPT_NOD)
_finish_headnod_down_time = rospy.get_time()
_start_headnod_up_time = rospy.get_time()
timedTest.pubTo_maki_command(self, str(_nod_down_up))
_sww_wi.sleepWhileWaitingMS(IPT_NOD)
_finish_headnod_up_time = rospy.get_time()
_finish_headnod_time = rospy.get_time()
_nod_count += 1
#rospy.loginfo( "Completed " + str(_nod_count) + " full head nods" )
rospy.loginfo(
"Head nod #" + str(_nod_count) + ": full nod = " +
str(abs(_finish_headnod_time - _start_headnod_time)) +
"; nod up->down = " + str(
abs(_finish_headnod_down_time -
_start_headnod_down_time)) + "; nod down->up = " +
str(abs(_finish_headnod_up_time - _start_headnod_up_time)))
rospy.loginfo("-----------------")
## try to nicely end testing the headnod
if self.mTT_INTERRUPT:
_sww_wi.sleepWhileWaiting(
1
) ## make sure to wait for message to reach head tilt Dynamixel servo
timedTest.pubTo_maki_command(self, "reset")
_sww_wi.sleepWhileWaiting(
1
) ## make sure to wait for message to reach head tilt Dynamixel servo
headTiltTimedTest.disableHT(self)
#end while not self.mTT_INTERRUPT:
headTiltTimedTest.disableHT(self)
#end while self.ALIVE:
headTiltTimedTest.disableHT(self)
rospy.logdebug("macroHeadNod: END")
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
## --------------------------------- ## 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() initSubFeedback()
def helper_macroAwake(self, awake_count, read_time=1.0):
_sww_wi = ROS_sleepWhileWaiting_withInterrupt(
verbose_debug=self.VERBOSE_DEBUG)
_start_awake_time = None
_finish_awake_time = None
_total_awake_time = 0
## neutral to LL_CLOSE_MAX
NEUTRAL_LL = LL_OPEN_DEFAULT
_delta_ticks_LL = abs(NEUTRAL_LL - LL_CLOSE_MAX)
## neutral to HT_DOWN
NEUTRAL_HT = HT_MIDDLE
_delta_ticks_HT = abs(NEUTRAL_HT - HT_DOWN)
## neutral to HP_LEFT (same delta as HP_RIGHT)
NEUTRAL_HP = HP_FRONT
_delta_ticks_HP = abs(NEUTRAL_HP - HP_LEFT)
## neutral to ET_DOWN
NEUTRAL_ET = ET_MIDDLE
_delta_ticks_ET = abs(NEUTRAL_ET - ET_DOWN)
## neutral to EP_LEFT (same delta as EP_RIGHT)
NEUTRAL_EP = EP_FRONT
_delta_ticks_EP = abs(NEUTRAL_EP - EP_LEFT)
rospy.logerr("delta_ticks[LL, HT, HP, ET, EP] = " +
str(_delta_ticks_LL) + ", " + str(_delta_ticks_HT) +
", " + str(_delta_ticks_HP) + ", " +
str(_delta_ticks_ET) + ", " + str(_delta_ticks_EP))
## maki_cmd
_m_cmd_prefix_LL = "LL" + str(SC_SET_GP)
_m_cmd_lid_neutral_close = _m_cmd_prefix_LL + str(LL_CLOSE_MAX)
_m_cmd_lid_close_neutral = _m_cmd_prefix_LL + str(NEUTRAL_LL)
_m_cmd_prefix_LL = "LL" + str(SC_SET_GS)
_m_cmd_prefix_HT = "HT" + str(SC_SET_GP)
_m_cmd_head_neutral_down = _m_cmd_prefix_HT + str(HT_DOWN)
_m_cmd_head_down_neutral = _m_cmd_prefix_HT + str(NEUTRAL_HT)
_m_cmd_prefix_HT = "HT" + str(SC_SET_GS)
_m_cmd_prefix_HP = "HP" + str(SC_SET_GP)
_m_cmd_head_neutral_left = _m_cmd_prefix_HP + str(HP_LEFT)
_m_cmd_head_neutral_right = _m_cmd_prefix_HP + str(HP_RIGHT)
_m_cmd_head_lr_neutral = _m_cmd_prefix_HP + str(NEUTRAL_HP)
_m_cmd_prefix_HP = "HP" + str(SC_SET_GS)
_m_cmd_prefix_ET = "ET" + str(SC_SET_GP)
_m_cmd_eye_neutral_down = _m_cmd_prefix_ET + str(ET_DOWN)
_m_cmd_eye_down_neutral = _m_cmd_prefix_ET + str(NEUTRAL_ET)
_m_cmd_prefix_ET = "ET" + str(SC_SET_GS)
_m_cmd_prefix_EP = "EP" + str(SC_SET_GP)
_m_cmd_eye_neutral_left = _m_cmd_prefix_EP + str(EP_LEFT)
_m_cmd_eye_neutral_right = _m_cmd_prefix_EP + str(EP_RIGHT)
_m_cmd_eye_lr_neutral = _m_cmd_prefix_EP + str(NEUTRAL_EP)
_m_cmd_prefix_HP = "EP" + str(SC_SET_GS)
headTiltTimedTest.enableHT(self)
_sww_wi.sleepWhileWaitingMS(1000, 0.05)
rospy.loginfo("-----------------")
for _ms_duration in range(5000, 1000, -500): ## [5000, 1500] ms
headTiltTimedTest.enableHT(self)
## calculate GoalSpeed
_gs_HT = int(
self.DC_helper.getGoalSpeed_ticks_durationMS(
_delta_ticks_HT, _ms_duration)) + 1
_gs_HP = int(
self.DC_helper.getGoalSpeed_ticks_durationMS(
_delta_ticks_HP, _ms_duration)) + 1
## eyes should finish first
_ms_duration_eyes = int((float(_ms_duration) * 0.3) + 0.5)
_gs_LL = int(
self.DC_helper.getGoalSpeed_ticks_durationMS(
_delta_ticks_LL, _ms_duration_eyes)) + 1
_gs_ET = int(
self.DC_helper.getGoalSpeed_ticks_durationMS(
_delta_ticks_ET, _ms_duration_eyes)) + 1
_gs_EP = int(
self.DC_helper.getGoalSpeed_ticks_durationMS(
_delta_ticks_EP, _ms_duration_eyes)) + 1
rospy.logerr("ms_duration_eyes = " + str(_ms_duration_eyes) + "ms")
## preset eyelid to neutral GoalSpeed
#_pub_cmd = _m_cmd_prefix_LL + str(16) ## used spreadsheet to calculate these values
#_pub_cmd += _m_cmd_prefix_HT + str(13)
#_pub_cmd += _m_cmd_prefix_HP + str(88)
#_pub_cmd += _m_cmd_prefix_EP + str(22)
#_pub_cmd += _m_cmd_prefix_ET + str(50)
#_pub_cmd += str(TERM_CHAR_SEND)
#timedTest.pubTo_maki_command( self, str(_pub_cmd) )
#_sww_wi.sleepWhileWaitingMS( 100, 0.05 )
## move to asleep position in a discernable manner from waking up
_pub_cmd = str(_m_cmd_lid_neutral_close +
_m_cmd_head_neutral_down + _m_cmd_eye_neutral_down)
_pub_cmd += str(_m_cmd_head_neutral_left + _m_cmd_eye_neutral_left)
_pub_cmd += str(SC_SET_IPT) + str(waking_up_time)
_pub_cmd += str(TERM_CHAR_SEND)
timedTest.pubTo_maki_command(self, str(_pub_cmd))
_sww_wi.sleepWhileWaitingMS(waking_up_time + 1000, 0.05)
### preset eyelid to open GoalSpeed
#_pub_cmd = _m_cmd_prefix_LL + str(_gs_LL)
#_pub_cmd += _m_cmd_prefix_HT + str(_gs_HT)
#_pub_cmd += _m_cmd_prefix_HP + str(_gs_HP)
#_pub_cmd += _m_cmd_prefix_ET + str(_gs_ET)
#_pub_cmd += _m_cmd_prefix_EP + str(_gs_EP)
#_pub_cmd += str(TERM_CHAR_SEND)
#timedTest.pubTo_maki_command( self, str(_pub_cmd) )
#_sww_wi.sleepWhileWaitingMS( 100, 0.05 )
## publish waking up: eyelid open to neutral GoalPosition
_pub_cmd = str(_m_cmd_lid_close_neutral +
_m_cmd_head_down_neutral + _m_cmd_eye_down_neutral)
_pub_cmd += str(_m_cmd_head_lr_neutral + _m_cmd_eye_lr_neutral)
_pub_cmd += str(SC_SET_IPT) + str(_ms_duration)
_pub_cmd += str(TERM_CHAR_SEND)
timedTest.pubTo_maki_command(self, str(_pub_cmd))
_start_awake_time = rospy.get_time()
rospy.loginfo("Start waking up")
_sww_wi.sleepWhileWaitingMS(_ms_duration, 0.01, False)
_finish_awake_time = rospy.get_time()
rospy.loginfo("Done waking up")
_total_awake_time = abs(_finish_awake_time - _start_awake_time)
awake_count += 1
rospy.loginfo("Awake #" + str(awake_count) + ": GS_LL = " +
str(_gs_LL) + ", GS_HT = " + str(_gs_HT) +
", GS_HP = " + str(_gs_HP) + ", GS_ET = " +
str(_gs_ET) + ", GS_EP = " + str(_gs_EP) +
"; expected time = " + str(_ms_duration) +
"ms; actual awake time = " + str(_total_awake_time) +
" seconds")
rospy.loginfo("-----------------")
## make it easier to read
_sww_wi.sleepWhileWaiting(read_time, 0.5)
## try to nicely end test
if self.mTT_INTERRUPT:
break ## break out of for loop
else:
## reset timers
_start_awake_time = None
_finish_awake_time = None
_total_awake_time = 0
# end for _ms_duration
return awake_count