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 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()