Object/Person/Face views

mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/src/mdr_find_people/action_states.py

@@ -1,20 +1,17 @@
-import math
 import rospy
 import smach
 import sympy
-import numpy as np
 
 import tf
 import face_recognition
 from sensor_msgs.msg import PointCloud2, Image
-from geometry_msgs.msg import Point, Pose, PoseStamped
 from mdr_find_people.msg import FindPeopleResult
-from mas_perception_msgs.msg import Person, PersonList
+from mas_perception_msgs.msg import Person, PersonList, ObjectView
 from mas_perception_libs import ImageDetectionKey
 from mas_perception_libs.visualization import crop_image
 from mas_perception_libs.utils import cloud_msg_to_cv_image
 from cv_bridge import CvBridge
-from find_people import FindPeople
+from mdf_find_people.find_people import FindPeople
 
 
 class FindPeopleState(smach.State):
@@ -45,15 +42,15 @@ def execute(self, userdata):
         for i, bb2d in enumerate(bb2ds):
             cropped_cv = crop_image(cv_image, bb2d)
             cropped_img_msg = bridge.cv2_to_imgmsg(cropped_cv, encoding="passthrough")
-            
+
             rospy.loginfo('[find_people] Attempting to extract face of person {}'.format(i+1))
             cropped_face_img = self._extract_face_image(cropped_cv)
             if cropped_face_img is not None:
                 cropped_face_img_msg = bridge.cv2_to_imgmsg(cropped_face_img,
                                                             encoding='passthrough')
             else:
                 cropped_face_img_msg = Image()
-            
+
             images.append(cropped_img_msg)
             face_images.append(cropped_face_img_msg)
 
@@ -66,8 +63,14 @@ def execute(self, userdata):
 
             map_pose = self._listener.transformPose('/map', poses[i])
             p.pose = map_pose
-            p.body_image = images[i]
-            p.face.image = face_images[i]
+
+            person_view = ObjectView()
+            person_view.image = images[i]
+            p.views.append(person_view)
+
+            face_view = ObjectView()
+            face_view.image = face_images[i]
+            p.face.views.append(face_view)
 
             pl.append(p)
 

mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/ros/src/mdr_demo_lab_manager/scenario_states/verify_person.py

@@ -36,13 +36,15 @@ def execute(self, userdata):
             return 'no_empty_spot'
 
         # Try to match face to current people in the lab
-        known_faces = self.kb_interface.get_obj_of_type(Person._type)
-        unknown_face = self.kb_interface.get_obj_instance('person_0', Person._type)
+        known_people = self.kb_interface.get_obj_of_type(Person._type)
+        unknown_person = self.kb_interface.get_obj_instance('person_0', Person._type)
 
-        for known_face in known_faces:
-            distance = known_face.face - unknown_face.face
-            if np.linalg.norm(distance) < self.threshold: 
-                self.say("Welcome back {0}".format(known_face.name))
+        unknown_person_embedding = np.array(unknown_person.face.views[0].embedding.embedding)
+        for known_person in known_people:
+            distance = np.linalg.norm(np.array(known_person.face.views[0].embedding.embedding) - \
+                                      unknown_person_embedding)
+            if np.linalg.norm(distance) < self.threshold:
+                self.say("Welcome back {0}".format(known_person.name))
                 return 'known_person'
 
         # No matching face, treat as new person

mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_find_my_mates/ros/src/mdr_find_my_mates/scenario_states/describe_person.py

@@ -27,7 +27,7 @@ def execute(self, userdata):
         person_msg = self.kb_interface.get_obj_instance(userdata.person_name, Person)
         person_info_msg = self.kb_interface.get_obj_instance(userdata.person_name, PersonInfo)
         try:
-            cv_image = self.bridge.imgmsg_to_cv2(person_msg.rgb_image, "bgr8")
+            cv_image = self.bridge.imgmsg_to_cv2(person_msg.views[0].image, "bgr8")
         except CvBridgeError as e:
             rospy.logerr(e.msg)