def __init__(self):
smach.StateMachine.__init__(
self,
ourcomes=[succeeded, preempted, aborted],
output_keys=['FSNS_name_of_person'])
with self:
face_recog_goal = FaceRecognitionGoal()
face_recog_goal.order_id = 3
smach.StateMachine.add(
'AC_ASK_PERSON_RECOG_TO_ENROLL_PERSON',
SimpleActionState('face_recognition', FaceRecognitionAction, goal=face_recog_goal, result_slots=['order_id', 'names', 'confidence']),
transitions={succeeded: 'Process_Fake_NameServer_Data'},
remapping={'names': 'LP_names'})
smach.StateMachine.add(
'Process_Fake_NameServer_Data',
ProcessData2(),
transitions={succeeded: 'PRINT_USERDATA', face_not_found: 'AC_ASK_PERSON_RECOG_TO_ENROLL_PERSON'},
remapping={'process_names2': 'LP_names', 'person_name2': 'FSNS_name_of_person'})
smach.StateMachine.add(
'PRINT_USERDATA',
PrintUserData2(),
transitions={succeeded: succeeded},
remapping={})
def face_recog_goal_cb(userdata, goal):
face_recog_goal = FaceRecognitionGoal()
#In Retrain we are just learning one face,order_id = 3 (re)train
face_recog_goal.order_id = 3
#For the moment we won't use this, but it's needed for face recognition.
face_recog_goal.order_argument = 'Referee'
print 'This is the name we are learning : ' + face_recog_goal.order_argument
return face_recog_goal
def recog(itemName, itemValue):
#How long to sleep
sleepTime = 4
#Start the iot_bridge. Starts roscore too
subprocess.Popen(["screen","-dmS", "roslaunchBridge", "roslaunch","iot_bridge","iot.launch"])
#Wait for the server to start
time.sleep(sleepTime)
#Start the camera
subprocess.Popen(["screen","-dmS", "roslaunchCamera", "roslaunch","/home/administrator/catkin_ws/start-cam.launch"])
#wait for the camera to start
time.sleep(sleepTime)
#Set the confidence_value for the face recognition server.No need to wait
subprocess.Popen(["rosparam" ,"set", "/face_recognition/confidence_value", "0.80"])
subprocess.Popen(["rosparam" ,"set", "/face_recognition/show_screen_flag", "false"])
#Start the Fserver
subprocess.Popen(["screen","-dmS", "rosrunServer", "rosrun","face_recognition","Fserver"])
#Wait for the server to start
time.sleep(sleepTime)
#Init the client node
rospy.init_node('recog_node', anonymous=True)
# Creates the SimpleActionClient, passing the type of the action
client = actionlib.SimpleActionClient('face_recognition', FaceRecognitionAction)
# Waits until the action server has started up and started
# listening for goals.
client.wait_for_server()
# Creates a goal to send to the server.
goal = FaceRecognitionGoal()
goal.order_id = 0
goal.order_argument = "none"
# Sends the goal to the action server.
client.send_goal(goal)
# Waits for the server to finish performing the action.
client.wait_for_result()
# Prints out the result of executing the action
print(client.get_result().names[0] + ":" + str(client.get_result().confidence[0]))
pub = rospy.Publisher('openhab_command', KeyValue, queue_size=10)
time.sleep(1)
pub.publish(itemName, itemValue)
time.sleep(sleepTime)
def set_name(self):
reco = actionlib.SimpleActionClient("face_recognition", FaceRecognitionAction)
rospy.loginfo("Connected to face recognition server")
reco.wait_for_server(rospy.Duration(10))
reco_goal = FaceRecognitionGoal()
reco_goal.order_id = 0
reco_goal.order_argument="none"
reco.send_goal(reco_goal)
rospy.loginfo("Waiting the result to face recognition server")
finished_within_time = reco.wait_for_result(rospy.Duration(4))
def get_name(self):
reco = actionlib.SimpleActionClient("face_recognition", FaceRecognitionAction)
rospy.loginfo("Connected to face recognition server")
reco.wait_for_server(rospy.Duration(10))
reco_goal = FaceRecognitionGoal()
reco_goal.order_id = 0
reco_goal.order_argument="none"
reco.send_goal(reco_goal)
rospy.loginfo("Waiting the result to face recognition server")
finished_within_time = reco.wait_for_result(rospy.Duration(2))
if not finished_within_time:
self.recognized_name = "unknown"
reco.cancel_goal()
#self.kernel.setPredicate('name',str(self.recognized_name))
#self.kernel.setPredicate('confidence','00')
return "unknown"
else:
state = reco.get_state()
result = reco.get_result()
name = result.names
confidence = "%.2f" %result.confidence
confidence = float(confidence)*100
confidence = int(confidence)
name = str(name).replace("[","").replace("]","").replace("'","").capitalize()
if confidence >= self.recog_name_certainty :
rospy.loginfo("Il s'agit de :" + str(name))
rospy.loginfo("Avec une reconnaissance de :" + str(confidence)+"%")
self.kernel.setPredicate('valConfidence',str(confidence),str(name))
self.kernel.setPredicate('name',str(name),str(name))
self.kernel.setPredicate('confidence','ok',str(name))
self.recognized_name = name
return str(name)
def __init__(self, announce_unknown_face=True, instructions_tts=True):
"""Constructor for RecognizeFaceStateMachine.
@type announce_unknown_face: boolean
@param announce_unknown_face: If True, the robot will say something if can't recognize the person.
@type instructions_tts: boolean
@param instructions_tts: If True, the robot will ask the person to look at his face. Otherwise, will try recognize without instructions.
Output Keys:
@type out_person_name: string|None
@return out_person_name: If a person is recognized, out_person_name="Person name", otherwise out_person_name=None
"""
smach.StateMachine.__init__(self,
output_keys=['out_person_name'],
outcomes=[succeeded, preempted, aborted, unknown_face]
)
with self:
self.variables = cocktail_party_variables
if instructions_tts is True:
smach.StateMachine.add(
'INSTRUCTIONS_TTS',
SpeakActionFromPoolStateMachine(["Please, look at my face!"]),
transitions={succeeded: 'RECOGNIZE_FACE'})
@smach.cb_interface(outcomes=[succeeded, unknown_face], output_keys=['out_person_name'])
def recognize_face_result_cb(userdata, status, result):
userdata.out_person_name = None
if status == GoalStatus.SUCCEEDED:
print "Face recognition result:\n", result
if len(result.names) > 0 and result.confidence[0] > FACE_RECOGNITION_CONFIDENCE_THRESHOLD:
userdata.out_person_name = result.names[0]
return succeeded
return unknown_face
return aborted
faceGoal = FaceRecognitionGoal()
faceGoal.order_id = 0 # recognize_once
smach.StateMachine.add(
'RECOGNIZE_FACE',
SimpleActionState(
self.variables.A_FACE_RECOGNITION,
FaceRecognitionAction,
exec_timeout=Duration(RECOGNIZE_UNKNOWN_PERSON_TIMEOUT),
result_cb=recognize_face_result_cb,
goal=faceGoal),
transitions={succeeded: "ANNOUNCE_PERSON_RECOGNIZED",
unknown_face: "ANNOUNCE_UNKNOWN_FACE" if announce_unknown_face else unknown_face,
aborted: "ANNOUNCE_UNKNOWN_FACE" if announce_unknown_face else unknown_face})
def announce_recognized_cb(userdata):
# "nice" must be lowercase so it isn't pronounced like the city!
# return 'Hello, %s. nice to see you again.' % userdata.out_person_name
return 'Hello, %s!' % userdata.out_person_name
smach.StateMachine.add(
'ANNOUNCE_PERSON_RECOGNIZED',
SpeakActionState(text_cb=announce_recognized_cb, input_keys=['out_person_name']),
transitions={succeeded: succeeded, aborted: succeeded, preempted: succeeded}
)
smach.StateMachine.add(
'ANNOUNCE_UNKNOWN_FACE',
SpeakActionState("I'm sorry, I don't recognize you"),
transitions={succeeded: unknown_face})
#!/usr/bin/env python
import sys
import rospy
import actionlib
from face_recognition.msg import FaceRecognitionAction, FaceRecognitionGoal
if __name__ == '__main__':
rospy.init_node('face_recognition_starter')
c = actionlib.ActionClient('face_recognition', FaceRecognitionAction)
c.wait_for_server()
g = FaceRecognitionGoal()
g.order_id = 1
g.order_argument = "none"
c.send_goal(g)
sys.exit(0)
#!/usr/bin/env python
import sys
import rospy
import actionlib
from face_recognition.msg import FaceRecognitionAction, FaceRecognitionGoal
if __name__ == '__main__':
rospy.init_node('face_recognition_starter')
c = actionlib.ActionClient('face_recognition', FaceRecognitionAction)
c.wait_for_server()
g = FaceRecognitionGoal()
g.order_id = 1
g.order_argument = "none"
c.send_goal(g)
sys.exit(0)
def learn_face_goal_cb(userdata, old_goal):
faceGoal = FaceRecognitionGoal()
faceGoal.order_id = 3 # (re)train
faceGoal.order_argument = userdata.in_person_name
return faceGoal