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 execute(self, userdata):
if self.preempt_requested():
self.service_preempt()
return 'preempted'
self.goal = FaceRecognitionGoal()
self.goal.order_id = 1
self.goal.order_argument = ''
self._done_cond.acquire()
self.face_recog_client.send_goal(self.goal, self._goal_done_cb,
self._goal_active_cb,
self._goal_feedback_cb)
# Wait for action to finish
self._done_cond.wait()
self._done_cond.release()
print self._goal_status
print self._goal_result
# self.goal.order_id = 4
# self.face_recog_client.send_goal(self.goal)
if self._goal_status == GoalStatus.PREEMPTED:
self.service_preempt()
return 'preempted'
return 'succeeded'
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):
# Get the parameters from launchfile
self.param_prefix = "/rpi_integration"
self.autostart = rospy.get_param(self.param_prefix + '/autostart')
self.use_stt = rospy.get_param(self.param_prefix + '/use_stt', False)
self.use_tts = rospy.get_param(self.param_prefix + '/use_tts', True)
self.use_ontosem_comms = rospy.get_param(self.param_prefix +
'/use_ontosem_comms')
self.use_face_rec = rospy.get_param(self.param_prefix +
'/use_face_rec')
self.use_script = rospy.get_param(self.param_prefix + '/use_script',
True)
self.client = actionlib.SimpleActionClient("face_recognition",
FaceRecognitionAction)
self.goal = FaceRecognitionGoal()
self.storage_1 = [
] # For the purposes of this demo, storage_1 is empty
self.storage_2 = rospy.get_param(self.param_prefix +
"/objects_right").values()
self.workspace = [
] # stores ids of retrieved objects in shared workspace.
# Candidate for deletion
# self.strt_time = time.time()
self.LISTENING = False
# Listens for speech commands from microphone
self._listen_sub = rospy.Subscriber(
self.STT_TOPIC, #self.SPEECH_SERVICE,
transcript,
self._listen_query_cb)
self._cmd = None # Will store the current command POSTed by OntoSem
BaseController.__init__(
self,
use_left=True,
use_right=True,
use_stt=self.use_stt,
use_tts=self.use_tts,
recovery=False,
)
RESTOntoSemUtils.__init__(self)
if self.use_face_rec:
rospy.logwarn("Waiting for actionserver....")
self.client.wait_for_server()
rospy.logwarn("Action server ready!")
self._bootstrap()
def start_recognition(self):
while not rospy.is_shutdown():
if self.ready_for_recognition == True:
# send orders to face recognition server
self.recognitionGoal = FaceRecognitionGoal()
self.recognitionGoal.order_id = 0
self.recognitionGoal.order_argument = "none"
self.recognition.send_goal(self.recognitionGoal)
self.recognition.wait_for_result()
#get result from face recognition server
result = self.recognition.get_result()
name = result.names
#confidence = result.confidence
self.recognizedName = str(name).replace("['",
"").replace("']", "")
#self.recognizedConfidence = str(confidence)
self.pubFace.publish(self.recognizedName)
state = self.recognition.get_state()
print "voici l'etat:" + str(
self.goal_states[state]
) + " et le nom: " + self.recognizedName
else:
continue
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
