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