diff --git a/src/lhw_intelligence/lhw_intelligence/extra_behaviours.py b/src/lhw_intelligence/lhw_intelligence/extra_behaviours.py
index 7010d9b5e547704db6b8e0ebc1483730c993bf0a..6f94af11b41844bf084d8ba784a95dfaf7d82078 100644
--- a/src/lhw_intelligence/lhw_intelligence/extra_behaviours.py
+++ b/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):
diff --git a/src/lhw_intelligence/lhw_intelligence/py_tree.py b/src/lhw_intelligence/lhw_intelligence/py_tree.py
index 4e2d7fcb9a7a74c88610ba3de3ce320d788ef2d5..4b4847b69c67a93be447aed939b9cf217d310732 100644
--- a/src/lhw_intelligence/lhw_intelligence/py_tree.py
+++ b/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)
diff --git a/src/lhw_intelligence/lhw_intelligence/services.py b/src/lhw_intelligence/lhw_intelligence/services.py
deleted file mode 100644
index 524f2a5772236090aa81064926a7f5ad4a880869..0000000000000000000000000000000000000000
--- a/src/lhw_intelligence/lhw_intelligence/services.py
+++ /dev/null
@@ -1,23 +0,0 @@
-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
diff --git a/src/lhw_interfaces/CMakeLists.txt b/src/lhw_interfaces/CMakeLists.txt
index 9213b0cf78ddd1dd0f0cb23c6b148a7604ac5e58..685cf8095b5050f8e897deba843f04b708f8e16f 100644
--- a/src/lhw_interfaces/CMakeLists.txt
+++ b/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
)
diff --git a/src/lhw_interfaces/msg_intelligence/FacesDectected.srv b/src/lhw_interfaces/msg_intelligence/FacesDectected.srv
deleted file mode 100644
index 9fd49e9968c5fbda9ca44c282c21903d123b8d6a..0000000000000000000000000000000000000000
--- a/src/lhw_interfaces/msg_intelligence/FacesDectected.srv
+++ /dev/null
@@ -1,3 +0,0 @@
-Bool run
----
-FacesDectected
\ No newline at end of file
diff --git a/src/lhw_interfaces/msg_vision/DetectFaces.srv b/src/lhw_interfaces/msg_vision/DetectFaces.srv
new file mode 100644
index 0000000000000000000000000000000000000000..ef67af7966b7b98cc38cafe2f09265fc88fb7149
--- /dev/null
+++ b/src/lhw_interfaces/msg_vision/DetectFaces.srv
@@ -0,0 +1,4 @@
+bool run
+---
+int64 status
+FacesDetected faces_detected
\ No newline at end of file
diff --git a/src/lhw_vision/lhw_vision/detect_faces_service.py b/src/lhw_vision/lhw_vision/detect_faces_service.py
new file mode 100644
index 0000000000000000000000000000000000000000..690333cef269fa165e51ff396998a9e4774bc354
--- /dev/null
+++ b/src/lhw_vision/lhw_vision/detect_faces_service.py
@@ -0,0 +1,32 @@
+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
diff --git a/src/lhw_vision/setup.py b/src/lhw_vision/setup.py
index ab82c1e51dfe1a36b7016e844cc7b912ad34e260..916a2be263a1ec2c8d7b8c6aa569f13b8e45bf39 100644
--- a/src/lhw_vision/setup.py
+++ b/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'
],
},
)