def beatCheck(time_pre, timeval):
global context
global timer
global pub
if time.time() - time_pre < timeval:
return
else:
print('==========>>>>>>>>>>>>>>>> Hello Timer!')
print('==========>>>>>>>>>>>>>>>> HeatBeat Stop!\n\n')
# reset context
context.update({
'target': -1,
'mode': 'idle',
'ts': time.time(),
})
msg = FaceTarget()
msg.cmd = context['mode']
if not rospy.is_shutdown():
rospy.loginfo(msg)
pub.publish(msg)
context['ts'] = time.time()
timer = threading.Timer(timeval, beatCheck, [context['ts'], timeval])
timer.start()
def ros_talker(message):
global context
try:
face_msg = json.loads(message)
face, speed = Status_Machine(face_msg)
if face is None: return
header_eyes.stamp = rospy.Time.now()
eyesmsg = FaceTarget()
eyesmsg.header = header_eyes
eyesmsg.cmd = context['mode']
eyesmsg.target.x = (face['location'][0] + face['location'][2]) / 2
eyesmsg.target.y = (face['location'][1] + face['location'][3]) / 2
eyesmsg.target.z = 200 if context['mode'] == 'init' else 400
voicemsg = Core2Voice()
voicemsg.header = header_eyes
voicemsg.cmd = context['mode']
voicemsg.name = context['user_name']
voicemsg.gender = face['gender'] if face[
'gender'] is not None else ""
voicemsg.age = face['age'] if face['age'] is not None else 0
# print('age: ', type(face['age']))
workinginfo = ('name first tracking' if context['user_name'] != ''
else 'depth first tracking')
rospy.loginfo('\n\n------------------- %s -------------------',
workinginfo)
if not rospy.is_shutdown():
print('frame_id: ', eyesmsg.header.frame_id)
print('seq: ', eyesmsg.header.seq)
print('cmd: ', eyesmsg.cmd)
print()
print(' ----------eyesmsg----------')
print(' core2eyes/FaceTarget')
print(' target: ')
print(' x: ', eyesmsg.target.x)
print(' y: ', eyesmsg.target.y)
print(' z: ', eyesmsg.target.z)
print()
print(' ----------voicemsg----------')
print(' core2voice/Core2Voice')
print(' name: ', voicemsg.name)
print(' gender: ', voicemsg.gender)
print(' age: ', voicemsg.age)
print('\n\n')
pub_eyes.publish(eyesmsg)
pub_voice.publish(voicemsg)
face['fsm'] = json.dumps(name_list)
qrect.publish(face)
return True
except Exception as e:
print("[Exception] talk()", e)
def fun_timer():
global timer
global pub_eyes
global context
global face_msg_list
if time.time() - context['ts'] < 3:
return
print(
'=============>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>Hello Timer!'
)
context.update({
'target': -1,
'mode': 'idle',
'user_name': '',
'ts': time.time(),
'name_timer': time.time(),
'working': time.time()
})
face_msg_list = []
msg = FaceTarget()
msg.cmd = context['mode']
if not rospy.is_shutdown():
rospy.loginfo(msg)
pub_eyes.publish(msg)
timer = threading.Timer(3, fun_timer)
timer.start()
def eyes_pub_wrapper(msg, context, face, speed, header):
header.stamp = rospy.Time.now()
msg = FaceTarget()
msg.header = header_eyes
msg.cmd = context['mode']
msg.target.x = (face['location'][0] + face['location'][2]) / 2
msg.target.y = (face['location'][1] + face['location'][3]) / 2
msg.target.z = speed
#回显信息
workinginfo = ('name first tracking'
if context['user_name'] != '' else 'depth first tracking')
rospy.loginfo('\n\n------------------- %s -------------------',
workinginfo)
if not rospy.is_shutdown():
print('frame_id: ', msg.header.frame_id)
print('seq: ', msg.header.seq)
print('cmd: ', msg.cmd)
print()
print(' ----------eyesmsg----------')
print(' core2eyes/FaceTarget')
print(' target: ')
print(' x: ', msg.target.x)
print(' y: ', msg.target.y)
print(' z: ', msg.target.z)
pub.publish(msg)
def ros_talker(message):
global context
global face_msg
global face_msg_list
face_msg = json.loads(message)
face_msg = list(filter(lambda x: x['depth'] < 4000, face_msg))
if len(face_msg) == 0:
print('所有人4000以外')
return
face_msg_list.append(dict(ts=time.time(), msg=face_msg))
if len(face_msg_list) > 20:
face_msg_list.pop(0)
face = Status_Machine()
# print(face)
header.stamp = rospy.Time.now()
eyesmsg = FaceTarget()
eyesmsg.header = header
eyesmsg.cmd = context['mode']
eyesmsg.target.x = (face['location'][0] + face['location'][2]) / 2
eyesmsg.target.y = (face['location'][1] + face['location'][3]) / 2
eyesmsg.target.z = 400
if context['mode'] == 'init':
eyesmsg.target.x = 960
eyesmsg.target.y = 540
eyesmsg.target.z = 200
workinginfo = ('init_proxy: depth first tracking')
rospy.loginfo('\n\n------------------- %s -------------------',
workinginfo)
if not rospy.is_shutdown():
print('frame_id: ', eyesmsg.header.frame_id)
print('seq: ', eyesmsg.header.seq)
print('cmd: ', eyesmsg.cmd)
print()
print(' ----------eyesmsg----------')
print(' core2eyes/FaceTarget')
print(' target: ')
print(' x: ', eyesmsg.target.x)
print(' y: ', eyesmsg.target.y)
print(' z: ', eyesmsg.target.z)
print('\n\n')
pub.publish(eyesmsg)
qrect.publish(face)
return True
def talker():
pub_eyes = rospy.Publisher('core_eyes', FaceTarget, queue_size=10)
rospy.init_node('core', anonymous=True)
header_eyes = Header(seq=0, stamp=rospy.Time.now(), frame_id='core')
qrect = RSMQueue('faceTarget', '127.0.0.1')
#start FaceIO
faceIO = FaceIO()
faceIO.run()
lastTarget = FaceTarget(header=header_eyes, cmd='idle')
lastTarget.target.x = 50
lastTarget.target.y = 50
lastTarget.target.z = 100
# start CardIO
cardIO = CardIO()
cardIO.run()
rate = rospy.Rate(20)
while not rospy.is_shutdown():
header_eyes.stamp = rospy.Time.now()
eyesmsg = FaceTarget()
eyesmsg.header = header_eyes
eyesmsg.cmd = getStatus(cardIO, faceIO)
eyesmsg.target = lastTarget.target
face = getTarget(cardIO, faceIO)
if len(face) > 0:
eyesmsg.target.x = (face['location'][0] +
face['location'][2]) * 100 / 2
eyesmsg.target.y = (face['location'][1] +
face['location'][3]) * 100 / 2
eyesmsg.target.z = 100
lastTarget.target = eyesmsg.target
if getStatus(cardIO, faceIO) == 'working':
workinginfo = 'depth first tracking'
print('\n\n------------------- %s -------------------',
workinginfo)
print('frame_id: ', eyesmsg.header.frame_id)
print('seq: ', eyesmsg.header.seq)
print('cmd: ', eyesmsg.cmd)
print()
print(' ----------eyesmsg----------')
print(' core2eyes/FaceTarget')
print(' target: ')
print(' x: ', eyesmsg.target.x)
print(' y: ', eyesmsg.target.y)
print(' z: ', eyesmsg.target.z)
pub_eyes.publish(eyesmsg)
mqPub(qrect, face)
rate.sleep()
def talker():
pub_eyes = rospy.Publisher('core_eyes', FaceTarget, queue_size=1)
rospy.init_node('core', anonymous=True)
header_eyes = Header(seq=0, stamp=rospy.Time.now(), frame_id='core')
#start FaceIO
faceIO = FaceIO()
faceIO.run()
faceMode = 'name'
lastFaceTime = faceIO.time
lastTarget = FaceTarget(header=header_eyes, cmd=faceIO.status)
lastTarget.target.x = 50
lastTarget.target.y = 50
lastTarget.target.z = 200
# FacemodeSwitch(faceIO, faceMode)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
header_eyes.stamp = rospy.Time.now()
status = faceIO.status
eyesmsg = FaceTarget()
eyesmsg.header = header_eyes
eyesmsg.cmd = status
eyesmsg.target = lastTarget.target
face = faceIO.trackingFace
if len(face) > 0:
eyesmsg.target.x = (face['location'][0] +
face['location'][2]) * 100 / 2
eyesmsg.target.y = (face['location'][1] +
face['location'][3]) * 100 / 2
eyesmsg.target.z = speedControl(faceIO)
lastTarget.target = eyesmsg.target
# if lastFaceTime < faceIO.time:
workinginfo = ('name first tracking'
if faceMode != 'depth' else 'depth first tracking')
rospy.loginfo('\n\n------------------- %s -------------------',
workinginfo)
print('frame_id: ', eyesmsg.header.frame_id)
print('seq: ', eyesmsg.header.seq)
print('cmd: ', eyesmsg.cmd)
print()
print(' ----------eyesmsg----------')
print(' core2eyes/FaceTarget')
print(' target: ')
print(' x: ', eyesmsg.target.x)
print(' y: ', eyesmsg.target.y)
print(' z: ', eyesmsg.target.z)
pub_eyes.publish(eyesmsg)
# lastFaceTime = faceIO.time
rate.sleep()
def beatCheck(time_pre, timeval):
global context
global timer
if time.time() - time_pre < timeval:
return
else:
print('==========>>>>>>>>>>>>>>>> Hello Timer!')
print('==========>>>>>>>>>>>>>>>> HeatBeat Stop!\n\n')
context.update({
'target': -1,
'mode': 'idle',
'user_name': '',
'ts': time.time(),
'name_timer': time.time(),
'working': time.time()
})
msg = FaceTarget()
msg.cmd = context['mode']
def ros_talker(message):
try:
face_msg = json.loads(message)
print(face_msg)
header_eyes.stamp = rospy.Time.now()
face = face_msg[0]
eyesmsg = FaceTarget()
eyesmsg.header = header_eyes
eyesmsg.target.x = (face['location'][0] + face['location'][2]) / 2
eyesmsg.target.y = (face['location'][1] + face['location'][3]) / 2
eyesmsg.target.z = 400
workinginfo = ('raw_proxy: depth first tracking')
rospy.loginfo('\n\n------------------- %s -------------------',
workinginfo)
if not rospy.is_shutdown():
print('frame_id: ', eyesmsg.header.frame_id)
print('seq: ', eyesmsg.header.seq)
print('cmd: ', eyesmsg.cmd)
print()
print(' ----------eyesmsg----------')
print(' core2eyes/FaceTarget')
print(' target: ')
print(' x: ', eyesmsg.target.x)
print(' y: ', eyesmsg.target.y)
print(' z: ', eyesmsg.target.z)
pub_eyes.publish(eyesmsg)
qrect.publish(face)
return True
except Exception as e:
print("[Exception] talk()", e)
def talker():
pub_eyes = rospy.Publisher('core_eyes', FaceTarget, queue_size=10)
pub_voice = rospy.Publisher('core_voice', Core2Voice, queue_size=10)
rospy.init_node('core', anonymous=True)
header_eyes = Header(seq=0, stamp=rospy.Time.now(), frame_id='core')
header_voice = Header(seq=0, stamp=rospy.Time.now(), frame_id='core')
#start FaceIO
faceIO = FaceIO()
faceIO.run()
faceMode = 'name'
lastFaceTime = faceIO.time
lastTarget = FaceTarget(header=header_eyes,cmd = faceIO.status)
lastTarget.target.x = 50
lastTarget.target.y = 50
lastTarget.target.z = 100
# FacemodeSwitch(faceIO, faceMode)
# start VoiceIO
voiceIO = VoiceIO()
voiceIO.run()
rate = rospy.Rate(50)
while not rospy.is_shutdown():
Check(faceIO, voiceIO)
header_eyes.stamp = rospy.Time.now()
header_voice.stamp = header_eyes.stamp
status = faceIO.status
eyesmsg = FaceTarget()
eyesmsg.header = header_eyes
eyesmsg.cmd = status if status != 'activating' else 'idle'
eyesmsg.cmd = status
eyesmsg.target = lastTarget.target
voiceIO.name = faceIO.trackingName
voicemsg = Core2Voice()
voicemsg.header = header_voice
voicemsg.cmd = status
face = faceIO.trackingFace
if len(face) > 0:
eyesmsg.target.x = (face['location'][0] + face['location'][2])*100/2
eyesmsg.target.y = (face['location'][1] + face['location'][3])*100/2
eyesmsg.target.z = speedControl(faceIO)
lastTarget.target = eyesmsg.target
voicemsg.name = faceIO.trackingName if 'unknown' not in faceIO.trackingName else 'unknown'
voicemsg.name += '--'
voicemsg.name += face['user_id'] if face['user_id'] is not None else "None"
voicemsg.gender = face['gender'] if face['gender'] is not None else ""
# print('age: ', face['age'])
if voicemsg.gender != '':
if voicemsg.gender == 'female':
voicemsg.age = 18
else:
voicemsg.age = face['age'] if face['age'] is not None else 0
if lastFaceTime < faceIO.time:
workinginfo = ('name first tracking' if faceMode !=
'depth' else 'depth first tracking')
rospy.loginfo(
'\n\n------------------- %s -------------------', workinginfo)
print('frame_id: ', eyesmsg.header.frame_id)
print('seq: ', eyesmsg.header.seq)
print('cmd: ', eyesmsg.cmd)
print()
print(' ----------eyesmsg----------')
print(' core2eyes/FaceTarget')
print(' target: ')
print(' x: ', eyesmsg.target.x)
print(' y: ', eyesmsg.target.y)
print(' z: ', eyesmsg.target.z)
print()
print(' ----------voicemsg----------')
print(' core2voice/Core2Voice')
print(' name: ', voicemsg.name)
print(' gender: ', voicemsg.gender)
print(' age: ', voicemsg.age)
print('\n\n')
pub_eyes.publish(eyesmsg)
print('1111111111111')
pub_voice.publish(voicemsg)
print('2222222222')
lastFaceTime = faceIO.time
rate.sleep()