def main():
global myMessenger, msgTryTimer
global extProcCall, iksResolver
global ipkSpawner
extProcCall = extprocall.ProcConduit()
iksResolver = iksresolver.IKSResolver()
ipkSpawner = ipkspawner.IPKSpawner()
PyREEM.onUserLogOn = userLogon
PyREEM.onUserLogOff = userLogoff
PyREEM.onTimer = timerActions
PyREEM.onTimerLapsed = timerLapsedActions
PyREEM.onRemoteCommand = remoteCommandActions
PyREEM.onSystemShutdown = systemShutdownActions
PyREEM.onPowerPluggedChange = powerPlugChangeActions
PyREEM.onBatteryChargeChange = batteryChargeChangeActions
PyREEM.onNodeStatusUpdate = nodeStatusUpdate
myMessenger = messenger.Messenger()
if not myMessenger.checkin():
msgTryTimer = PyREEM.addTimer( 10*60, -1, 10*60 )
PyREEM.say( constants.INTRO_TEXT )
PyREEM.setLowPowerThreshold( 20 )
def checkin(self):
if self.appdisabled:
return False
try:
self.twtaccess = Twitter(
auth=OAuth(tininfo.TiNTwOAToken, tininfo.TiNTwOASecret,
tininfo.TiNTwConKey, tininfo.TiNTwConSecret))
except:
print 'Unable to check into PyRIDE REEM twitter app. Try again in 10mins.'
return False
self.startupTime = time.localtime()
self.updatestatus(
time.strftime("I'm back online at %H:%M to serve ",
time.localtime()) + tininfo.TiNLocation + ".")
self.token = self.lastoken
self.getmessages()
tinstate.updateStatus(constants.NEW_MESSAGES, len(self.messages) == 0)
tid = PyREEM.addTimer(60, -1, 60)
self.timercontext[tid] = 'getmsg'
timermanager.addTimer(tid, self)
#add purge archive time
purgetime = timermanager.calcTimePeriodFromNow("4:00")
if purgetime > 0:
tid = PyREEM.addTimer(purgetime, -1, 24 * 60 * 3600)
self.timercontext[tid] = 'purgemsg'
timermanager.addTimer(tid, self)
return True
def checkin( self ):
if self.appdisabled:
return False
try:
self.twtaccess = Twitter(auth=OAuth(tininfo.TiNTwOAToken, tininfo.TiNTwOASecret, tininfo.TiNTwConKey, tininfo.TiNTwConSecret))
except:
print 'Unable to check into PyRIDE REEM twitter app. Try again in 10mins.'
return False
self.startupTime = time.localtime()
self.updatestatus( time.strftime( "I'm back online at %H:%M to serve ", time.localtime() ) + tininfo.TiNLocation + "." )
self.token = self.lastoken
self.getmessages()
tinstate.updateStatus( constants.NEW_MESSAGES, len( self.messages ) == 0 )
tid = PyREEM.addTimer( 60, -1, 60 )
self.timercontext[tid] = 'getmsg'
timermanager.addTimer( tid, self )
#add purge archive time
purgetime = timermanager.calcTimePeriodFromNow( "4:00" )
if purgetime > 0:
tid = PyREEM.addTimer( purgetime, -1, 24*60*3600 )
self.timercontext[tid] = 'purgemsg'
timermanager.addTimer( tid, self )
return True
def systemShutdownActions(): global myMessenger global extProcCall PyREEM.say( 'I am going off line. Goodbye.' ) myMessenger.checkout() extProcCall.fini()
def batteryChargeChangeActions( batpc, isplugged, time_remain ):
global myMessenger
if batpc < 20 and not isplugged:
PyREEM.say( "I'm low on battery, please put me back on main power." )
if myMessenger:
myMessenger.updatestatus( "I have only %d percent battery power left!" % batpc )
def timerActions( id ):
global myMessenger, msgTryTimer
if msgTryTimer == id and myMessenger.checkin():
PyREEM.removeTimer( msgTryTimer )
msgTryTimer = -1
else:
timermanager.onTimerCall( id )
def play( self ):
if not self.action_finishes:
print "still playing action '%s'" % self.name
return
if self.text:
self.action_finishes = False
PyREEM.say( text )
def purgearchive(self):
if len(self.archive) == 0:
PyREEM.say("No archived message to be deleted.")
return
self.archive = []
tinstate.updateStatus(constants.ARCHIVE_MESSAGES, True)
PyREEM.say("All archived message have been deleted.")
def purgearchive( self ):
if len(self.archive) == 0:
PyREEM.say( "No archived message to be deleted." )
return
self.archive = []
tinstate.updateStatus( constants.ARCHIVE_MESSAGES, True )
PyREEM.say( "All archived message have been deleted." )
def play( self ):
if not self.action_finishes:
print "still playing action '%s'" % self.name
return
if self.hand_action:
self.action_finishes = False
PyREEM.setHandPostion( **(self.hand_action) )
def play(self):
if not self.action_finishes:
print "still playing action '%s'" % self.name
return
if self.text:
self.action_finishes = False
PyREEM.say(text)
def play(self):
if not self.action_finishes:
print "still playing action '%s'" % self.name
return
if self.hand_action:
self.action_finishes = False
PyREEM.setHandPostion(**(self.hand_action))
def play( self ):
if not self.action_finishes:
print "still playing action '%s'" % self.name
return
if self.arm_action:
self.action_finishes = False
if isinstance( self.arm_action, list ):
PyREEM.moveArmWithJointTrajectory( self.arm_action )
else:
PyREEM.moveArmWithJointPos( **(self.arm_action) )
def play(self):
if not self.action_finishes:
print "still playing action '%s'" % self.name
return
if self.arm_action:
self.action_finishes = False
if isinstance(self.arm_action, list):
PyREEM.moveArmWithJointTrajectory(self.arm_action)
else:
PyREEM.moveArmWithJointPos(**(self.arm_action))
def play( self ):
if not self.target_x:
print "No position set for target"
self.action_finishes = True
return
if not self.action_finishes:
print "still playing action '%s'" % self.name
return
self.action_finishes = False
PyREEM.pointHeadTo( "odom", self.target_x, self.target_y, self.target_z )
def play(self):
if not self.target_x:
print "No position set for target"
self.action_finishes = True
return
if not self.action_finishes:
print "still playing action '%s'" % self.name
return
self.action_finishes = False
PyREEM.pointHeadTo("odom", self.target_x, self.target_y, self.target_z)
def powerPlugChangeActions( isplugged ):
global myMessenger
text = ""
if isplugged:
text = "I'm on main power."
else:
text = "I'm on battery power."
PyREEM.say( text )
if myMessenger:
myMessenger.updatestatus( text )
def _start_kernel_thread(self):
if not self.app:
return
PyREEM.sendMessageToNode('jupyter', 'start')
self.app.start()
self.app.shell_socket.close()
self.app.stdin_socket.close()
self.app.control_socket.close()
self.app.iopub_socket.close()
self.app.heartbeat.socket.close()
self.kernelapp.IPKernelApp.clear_instance()
PyREEM.sendMessageToNode('jupyter', 'stop')
print("reinitiate app instance")
self.app = self.kernelapp.IPKernelApp.instance()
self.is_initialised = None
def respond( question ):
q = question.strip().lower()
if 'ip' in q or 'addr' in q:
return "My IP Address is %s." % PyREEM.getMyIPAddress()
elif 'tuck' in q and 'arm' in q:
PyREEM.tuckBothArms()
return "I'm tucking my arms."
elif 'battery' in q and ('how much' in q or 'status' in q):
(batpc, isplug, timeremain) = PyREEM.getBatteryStatus()
if isplug != 'unplugged':
return "I'm currently %s with %d percent battery power and finish charging in approximately %d minutes." % (isplug, batpc, timeremain % 60 )
else:
return "I'm currently %s with %d percent battery power and running out battery in approximately %d minutes." % (isplug, batpc, timeremain % 60 )
return "I don't understand your request."
def _start_kernel_thread( self ):
if not self.app:
return
PyREEM.sendMessageToNode( 'jupyter', 'start' )
self.app.start()
self.app.shell_socket.close()
self.app.stdin_socket.close()
self.app.control_socket.close()
self.app.iopub_socket.close()
self.app.heartbeat.socket.close()
self.kernelapp.IPKernelApp.clear_instance()
PyREEM.sendMessageToNode( 'jupyter', 'stop' )
print( "reinitiate app instance" )
self.app = self.kernelapp.IPKernelApp.instance()
self.is_initialised = None
def getArmPose( self, left_arm ):
if self.iks_in_use == 2:
self.spr2_obj.sync_object()
pos = None
orient = None
if left_arm:
pos = tuple(self.spr2_obj.larm_end_position())
orient = self.geometry.Orientation_3D( self.spr2_obj.larm_end_orientation(), representation = 'matrix' )
else:
pos = tuple(self.spr2_obj.rarm_end_position())
orient = self.geometry.Orientation_3D( self.spr2_obj.rarm_end_orientation(), representation = 'matrix' )
pose = {'position': pos, 'orientation': tuple(orient.quaternion())}
return pose
else:
if left_arm:
return PyREEM.getRelativeTF( '/base_footprint', '/hand_left_grasping_frame' )
else:
return PyREEM.getRelativeTF( '/base_footprint', '/hand_right_grasping_frame' )
def startDataRecording(self, mode, filename=""):
cmd = 'rosbag record -b 1024 '
str = ''
if mode & constants.REC_CAM:
#cmd = cmd + '-e "/(.*)_stereo/(left|right)/image_rect_color" '
cmd = cmd + '-e "/stereo/left/image" ' # record only one camera data
str = '_cam'
#if mode & constants.REC_KINECT:
#cmd = cmd + '"/camera/rgb/image_rect_color" "/camera/depth_registered/image_rect" '
#str = '_kinect'
if mode & constants.REC_SCAN:
cmd = cmd + '-e "/scan$" -e "/hokuyo/LAS_01" '
str = str + '_laser'
if mode & constants.REC_IMU: #temp disable for now.
cmd = cmd + '"/mobile_base_controller/odom" '
str = str + '_imu'
if mode & constants.REC_JOINTS:
cmd = cmd + '"/joint_states" '
str = str + '_joint'
if mode & constants.REC_TF:
cmd = cmd + '"/tf" '
str = str + '_tf'
if mode & constants.REC_SONAR:
cmd = cmd + '"/sonar_torso" "/sonar_base" '
str = str + '_sonar'
if mode & constants.REC_IR:
cmd = cmd + '"/ir_base" '
str = str + '_ir'
if filename == "":
cmd = cmd + '--duration=1m --split -O %s/%s%s_data.bag' % \
(RECORDED_DATA_DIR, time.strftime( "%Y%m%d_%H%M", time.localtime()), str)
else:
cmd = cmd + '--duration=1m --split -O %s/%s.bag' % \
(RECORDED_DATA_DIR, filename)
if self.recording:
self.killProc(self.recording)
self.recording = self.spawnProc(cmd)
PyREEM.say("Start data recording!")
def startDataRecording( self, mode, filename = "" ):
cmd = 'rosbag record -b 1024 '
str = ''
if mode & constants.REC_CAM:
#cmd = cmd + '-e "/(.*)_stereo/(left|right)/image_rect_color" '
cmd = cmd + '-e "/stereo/left/image" ' # record only one camera data
str = '_cam'
#if mode & constants.REC_KINECT:
#cmd = cmd + '"/camera/rgb/image_rect_color" "/camera/depth_registered/image_rect" '
#str = '_kinect'
if mode & constants.REC_SCAN:
cmd = cmd + '-e "/scan$" -e "/hokuyo/LAS_01" '
str = str + '_laser'
if mode & constants.REC_IMU: #temp disable for now.
cmd = cmd + '"/mobile_base_controller/odom" '
str = str + '_imu'
if mode & constants.REC_JOINTS:
cmd = cmd + '"/joint_states" '
str = str + '_joint'
if mode & constants.REC_TF:
cmd = cmd + '"/tf" '
str = str + '_tf'
if mode & constants.REC_SONAR:
cmd = cmd + '"/sonar_torso" "/sonar_base" '
str = str + '_sonar'
if mode & constants.REC_IR:
cmd = cmd + '"/ir_base" '
str = str + '_ir'
if filename == "":
cmd = cmd + '--duration=1m --split -O %s/%s%s_data.bag' % \
(RECORDED_DATA_DIR, time.strftime( "%Y%m%d_%H%M", time.localtime()), str)
else:
cmd = cmd + '--duration=1m --split -O %s/%s.bag' % \
(RECORDED_DATA_DIR, filename)
if self.recording:
self.killProc( self.recording )
self.recording = self.spawnProc( cmd )
PyREEM.say( "Start data recording!" )
def getArmPose(self, left_arm):
if self.iks_in_use == 2:
self.spr2_obj.sync_object()
pos = None
orient = None
if left_arm:
pos = tuple(self.spr2_obj.larm_end_position())
orient = self.geometry.Orientation_3D(
self.spr2_obj.larm_end_orientation(),
representation='matrix')
else:
pos = tuple(self.spr2_obj.rarm_end_position())
orient = self.geometry.Orientation_3D(
self.spr2_obj.rarm_end_orientation(),
representation='matrix')
pose = {'position': pos, 'orientation': tuple(orient.quaternion())}
return pose
else:
if left_arm:
return PyREEM.getRelativeTF('/base_footprint',
'/hand_left_grasping_frame')
else:
return PyREEM.getRelativeTF('/base_footprint',
'/hand_right_grasping_frame')
def announcemsg( self, mesgs ):
if len( mesgs ) == 0:
PyREEM.say( "There is no message to be announced" )
return
curTime = time.localtime()
for i in mesgs:
if i.created_at.tm_yday == curTime.tm_yday:
sayTime = time.strftime( "Message received at %H hour %M minute", i.created_at )
else:
sayTime = time.strftime( "Message received at %H hour %M minute on %A %B %d", i.created_at )
if i.sender in self.useraliases:
frmWho = "From %s," % self.useraliases[ i.sender ]
else:
frmWho = "From %s," % i.sender
PyREEM.say( "%s %s %s" % (sayTime, frmWho, i.text) )
PyREEM.say( "Message announcement is complete." )
def announcemsg(self, mesgs):
if len(mesgs) == 0:
PyREEM.say("There is no message to be announced")
return
curTime = time.localtime()
for i in mesgs:
if i.created_at.tm_yday == curTime.tm_yday:
sayTime = time.strftime(
"Message received at %H hour %M minute", i.created_at)
else:
sayTime = time.strftime(
"Message received at %H hour %M minute on %A %B %d",
i.created_at)
if i.sender in self.useraliases:
frmWho = "From %s," % self.useraliases[i.sender]
else:
frmWho = "From %s," % i.sender
PyREEM.say("%s %s %s" % (sayTime, frmWho, i.text))
PyREEM.say("Message announcement is complete.")
def stoptimers(self):
for tid in self.timercontext:
PyREEM.removeTimer(tid)
timermanager.delTimer(tid)
self.timercontext = {}
def userLogon( name ): PyREEM.say( '%s has logged on.' % name ) tinstate.updateStatus( constants.USER_PRESENT, False )
def userLogoff( name ): PyREEM.say( '%s has logged off.' % name ) tinstate.updateStatus( constants.USER_PRESENT, len(PyREEM.listCurrentUsers()) == 0)
def stoptimers( self ):
for tid in self.timercontext:
PyREEM.removeTimer( tid )
timermanager.delTimer( tid )
self.timercontext = {}
def stopDataRecording(self):
if self.recording:
self.killProc(self.recording)
self.recording = None
PyREEM.say("Stopped data recording!")
def useMoveIt(self):
if PyREEM.useMoveIt():
PyREEM.moveArmTo = PyREEM.moveArmPoseTo
self.iks_in_use = 1
print 'PyRIDE is using MoveIt! for REEM'
def useMoveIt( self ):
if PyREEM.useMoveIt():
PyREEM.moveArmTo = PyREEM.moveArmPoseTo
self.iks_in_use = 1
print 'PyRIDE is using MoveIt! for REEM'
def stopDataRecording( self ):
if self.recording:
self.killProc( self.recording )
self.recording = None
PyREEM.say( "Stopped data recording!" )