def disableHT( self ): if headTiltTimedTest.__ht_enabled == False: ## already disabled return timedTest.pubTo_maki_command( self, str(headTiltTimedTest.__ht_disable_cmd) ) self.SWW_WI.sleepWhileWaitingMS( 100, 0.05 ) ## make sure command propogates headTiltTimedTest.__ht_enabled = False
def enableHT( self ): if headTiltTimedTest.__ht_enabled == True: ## already enabled return if (self.makiPP["HT"] <= HT_UP and self.makiPP["HT"] >= HT_DOWN): _pub_cmd_GP = "HT" + str(SC_SET_GP) + str(self.makiPP["HT"]) + str(TERM_CHAR_SEND) timedTest.pubTo_maki_command( self, str(_pub_cmd_GP) ) self.SWW_WI.sleepWhileWaitingMS( 100, 0.05 ) ## make sure command propogates timedTest.pubTo_maki_command( self, str(headTiltTimedTest.__ht_enable_cmd) ) self.SWW_WI.sleepWhileWaitingMS( 100, 0.05 ) ## make sure command propogates headTiltTimedTest.__ht_enabled = True
def presetGoalSpeed_ticks_duration(self, servo, ticks, s_duration):
_gs = int(self.DC_helper.getGoalSpeed_ticks_duration(
ticks, s_duration)) + 1
if (_gs > 0) and (_gs < 1024):
_pub_cmd = servo + str(SC_SET_GS) + str(_gs) + str(TERM_CHAR_SEND)
rospy.loginfo(str(_pub_cmd))
timedTest.pubTo_maki_command(self, str(_pub_cmd))
elif (_gs == 0):
rospy.logerr("setting GoalSpeed to 0 (unlimited) is not allowed!")
rospy.logwarn("LL GoalSpeed is not changed")
else:
rospy.logerr(
"calculated GoalSpeed is out of bounds (0, 1023]; _gs = " +
str(_gs))
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 pubTo_maki_command_presetGoalSpeed( self, dict_servo_gs ):
_pub_cmd = ""
_pub_flag = False
for _servo, _gs in dict_servo_gs.iteritems():
if (_gs > 0) and (_gs < 1024):
_pub_cmd += _servo + str(SC_SET_GS) + str(_gs)
_pub_flag = True
elif (_gs == 0):
rospy.logerr("setting GoalSpeed to 0 (unlimited) is not allowed!")
rospy.logwarn( _servo + " GoalSpeed will not be changed")
else:
rospy.logerr("calculated GoalSpeed is out of bounds (0, 1023]; _gs = " + str(_gs))
if _pub_flag:
_pub_cmd += str(TERM_CHAR_SEND)
rospy.logerr( str(_pub_cmd) )
timedTest.pubTo_maki_command( self, str(_pub_cmd) )
return _pub_cmd
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
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 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
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
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 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_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