implemented a behaviour that calls for face detection via service

src/lhw_intelligence/lhw_intelligence/extra_behaviours.py

@@ -178,41 +178,50 @@ class Say(py_trees.behaviour.Behaviour):
         pass
 
 
-class AnalyseFace(py_trees.behaviour.Behaviour):
-    def __init__(self, name, node: str) -> None:
-        super(AnalyseFace, self).__init__(name)
+class DetectFaces(py_trees.behaviour.Behaviour):
+    def __init__(self, name, node) -> None:
+        super(DetectFaces, self).__init__(name)
         self.node = node
-        self.analyse_faces_publisher = self.node.analyse_faces_publisher
-    
+        self.detect_faces_client = self.node.detect_faces_client
+        
     def setup(self, **kwargs):
-        self.face_detect_service = FaceDetectServiced()
-        self.face_detect_client = FaceDetectClient()
+        pass
 
     def initialise(self):
-        self.face_detect_client.send_request()
+        self.request = DetectFaces.Request()
+        self.request.run = True
 
     def update(self):
-        rclpy.spin(face_detect_service)
-        
-        while rclpy.ok():
-            rclpy.spin_once(face_detect_client)
-            if face_detect_client.future.done():
-                try:
-                    response = face_detect_client.future.result()
-                except Exception as e:
-                    face_detect_client.get_logger().info('Service call failed %r' % (e,))
-                    return py_trees.common.Status.FAILURE
-                else:
-                    face_detect_client.get_logger().info('At least one person is detected')
-                    # add response to blackboard
-                    return py_trees.common.Status.SUCCESS
-
-        return py_trees.common.Status.RUNNING
+        self.result = self.detect_faces_client.call(self.request)
+        if (self.result.status == 1):
+            if not self.result.faces_detected:
+                return py_trees.common.Status.SUCCESS
+            print(self.result.faces_detected)
+            # Add the person and initialize different items if not recognized since earlier
+            if faces_detected.id not in self.blackboard.recognized_people:
+                self.blackboard.found_new_person = True
+                self.blackboard.recognized_people[faces_detected.id] = {
+                    "z_index" : faces_detected.z_index, 
+                    "name" : None, 
+                    "drinks" : None, 
+                    "age" : faces_detected.age, 
+                    "face_center" : [faces_detected.face_center_xy[0], faces_detected.face_center_xy[1]]
+                }
+            # Update the z_index of the detected person
+            else:
+                self.blackboard.recognized_people[faces_detected.id]["z_index"] = faces_detected.z_index
+                # Only change age if it is lower than previous, prevents age bouncing around
+                if faces_detected.age < self.blackboard.recognized_people[faces_detected.id]["age"]:
+                    self.blackboard.recognized_people[faces_detected.id]["age"] = faces_detected.age
+            print(self.blackboard.recognized_people)
+            return py_trees.common.Status.SUCCESS
+        elif (self.result.status == -1):
+            return py_trees.common.Status.FAILURE
+        else:
+            return py_trees.common.Status.RUNNING
 
     def terminate(self, new_status):
-        self.face_detect_service.destroy_node()
-        self.face_detect_client.destroy_node()
-        rclpy.shutdown()
+        pass
 
 
 class RobotPoint(py_trees.behaviour.Behaviour):

src/lhw_intelligence/lhw_intelligence/py_tree.py

@@ -15,6 +15,7 @@ from rclpy.action import ActionClient
 from lhw_interfaces.msg._goal import Goal
 from lhw_interfaces.msg._feedback import Feedback
 from lhw_interfaces.msg._result import Result
+from lhw_interfaces.srv import DetectFaces
 from diagnostic_msgs.msg._diagnostic_array import DiagnosticArray
 from diagnostic_msgs.msg._diagnostic_status import DiagnosticStatus
 
@@ -62,7 +63,7 @@ class Py_Tree(Node):
         self.move_to_publisher = self.create_publisher(Twist, "move_to", 10)
         self.say_publisher = self.create_publisher(String, 'say', 100)
         self.robot_action_pub = self.create_publisher(RobotAction, 'robot_action', 1)
-        self.face_detected_pub = self.create_publisher(Bool, 'face_detected', 1)
+        self.detected_faces_pub = self.create_publisher(Bool, 'face_detected', 1)
         
         # ------- Subscribers ---------
         self.dialogflow_sub = self.create_subscription(Response, 'dialogflow_response', self.dialogflow_response_callback, 1)
@@ -76,9 +77,13 @@ class Py_Tree(Node):
         #client server (/navigate_to_pose)
         self._action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
         
+
+        # ------- Services ---------
+        self.detect_faces_client = self.create_client(DetectFaces, "detect_faces")
         self.pixel_to_xyz_serv = self.create_client(PixelToXYZ, "pixel_to_xyz")
 
 
+
         # Yolo subscriber
         #self.entities_sub = self.create_subscription(Entities, 'entities', self.entities_callback, 1)
         

src/lhw_intelligence/lhw_intelligence/services.py

-from lhw_interfaces.srv import FacesDetected
-
-import rclpy
-from rclpy.node import Node
-
-class FaceDetectService(Node):
-
-    def __init__(self):
-        super().__init__('face_detect_service')
-        self.srv = self.create_service(FacesDetected, 'faces_detected', self.faces_detected_callback)
-
-    def faces_detected_callback(self, request, response):
-        if not request:
-            return
-        self.get_logger().info('Incoming request\nrun, %b' % (request.run))
-
-        
-
-        return response
-
-class FaceDetectClient(Node):
-    
-    
\ No newline at end of file

src/lhw_interfaces/CMakeLists.txt

@@ -39,7 +39,6 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg_intelligence/Goal.msg"
   "msg_intelligence/Feedback.msg"
   "msg_intelligence/Result.msg"
-  "msg_intelligence/FacesDetected.srv"
   "msg_nlp/Parameter.msg"
   "msg_nlp/Response.msg"
   "msg_nlp/Event.msg"
@@ -58,6 +57,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg_vision/FacesDetected.msg"
   "msg_vision/PixelToXYZ.srv"
   "msg_vision/TransformXYZ.srv"
+  "msg_vision/DetectFaces.srv"
   DEPENDENCIES builtin_interfaces geometry_msgs sensor_msgs std_msgs
 )
 

src/lhw_interfaces/msg_intelligence/FacesDectected.srv

-Bool    run
----
-FacesDectected
\ No newline at end of file

src/lhw_interfaces/msg_vision/DetectFaces.srv

+bool run
+---
+int64 status
+FacesDetected faces_detected
\ No newline at end of file

src/lhw_vision/lhw_vision/detect_faces_service.py

+from lhw_interfaces.srv import DetectFaces
+
+import rclpy
+from rclpy.node import Node
+
+class DetectFacesService(Node):
+
+    def __init__(self):
+        super().__init__('detect_faces_service')
+        self.srv = self.create_service(DetectFaces, 'detect_faces', self.detect_faces_callback)
+
+    def detect_faces_callback(self, request, response):
+        self.get_logger().info('Incoming request\nrun, %b' % (request.run))
+
+        if (request.run == True):
+            # enable face detection
+            pass
+        else:
+            # disable face detection
+            pass
+        
+        return response
+
+def main(args=None):
+    rclpy.init(args=args)
+    detect_faces_service = DetectFacesService()
+    rclpy.spin(detect_faces_service)
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
\ No newline at end of file

src/lhw_vision/setup.py

@@ -38,6 +38,7 @@ setup(
             'debug_image_node = lhw_vision.debug_image_node:main',
             'debug_tracker_node = lhw_vision.debug_tracker_node:main',
             'depth_node = lhw_vision.depth_node:main',
+            'detect_faces_service = lhw_vision.detect_faces_service:main'
         ],
     },
 )