Lab manager (IEEE video challenge)

mdr_environments/brsu-c069/navigation_goals.yaml

@@ -12,3 +12,13 @@ observation_pose: [-0.386269, -0.149052, -0.013196]
 trash_can: [-1.991786, -3.080515, -1.644647]
 shelf_cupboard: [4.372, -3.300, 3.113]
 trash_bin1: [6.55, -3.75, -0.00277]
+
+monitoring_pose: [-1.249, -2.924, -2.888]
+spot_1: [-0.229, 0.517, -2.784]
+spot_2: [-0.263, 0.756, 0.548]
+spot_3: [0.601, -2.074, 0.823]
+spot_4: [4.238, -1.575, 1.468]
+spot_5: [6.388, -0.584, 1.506]
+spot_6: [6.846, -0.826, -1.132]
+spot_7: [8.274, -3.791, 0.778]
+disinfection_table: [7.095, -3.350, 3.132]

mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/launch/find_people.launch

@@ -1,8 +1,10 @@
 <?xml version="1.0"?>
 <launch>
     <arg name="pointcloud_topic" default="/mdr_perception/rectified_points"/>
+    <arg name="face_embedding_model_path" default="/home/robot/models/face_embedding_model.pt"/>
 
     <node pkg="mdr_find_people" type="find_people_server" name="find_people_server" output="screen" >
         <param name="pointcloud_topic" type="string" value="$(arg pointcloud_topic)" />
+        <param name="face_embedding_model_path" type="string" value="$(arg face_embedding_model_path)" />
     </node>
 </launch>

mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_people/ros/scripts/find_people_client

@@ -29,7 +29,7 @@ class FindPeopleClient(ActionClientBase):
         client.wait_for_result(rospy.Duration.from_sec(int(self.action_timeout)))
         result = client.get_result()
 
-        if result and result.person_list:
+        if result and result.person_list and result.person_list.persons:
             rospy.loginfo('[FIND_PEOPLE] Updating the knowledge base')
             self.update_knowledge_base(result.person_list)
             self.send_action_feedback(True)

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

@@ -1,19 +1,20 @@
-import math
 import rospy
 import smach
-import sympy
+import torch
 
+from PIL import Image as PILImage
 import tf
-from sensor_msgs.msg import PointCloud2
-from geometry_msgs.msg import Point, Pose, PoseStamped
+from sensor_msgs.msg import PointCloud2, Image
 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 mdr_find_people.find_people import FindPeople
 
+from dataset_interface.siamese_net.model import SiameseNetwork
+from dataset_interface.siamese_net.utils import get_transforms
 
 class FindPeopleState(smach.State):
     def __init__(self):
@@ -24,7 +25,16 @@ def __init__(self):
 
         self._listener = tf.TransformListener()
         self.pointcloud_topic = rospy.get_param("~pointcloud_topic", '/rectified_points')
+        self.face_embedding_model_path = rospy.get_param("~face_embedding_model_path", '')
 
+        self.face_embedding_model = None
+        if self.face_embedding_model_path:
+            self.face_embedding_model = SiameseNetwork()
+            self.face_embedding_model.load_state_dict(torch.load(self.face_embedding_model_path))
+            self.face_embedding_model.eval()
+
+            device = torch.device('cuda') if torch.cuda.is_available() else torch.device('cpu')
+            self.face_embedding_model.to(device)
 
     def execute(self, userdata):
         rospy.loginfo('Executing state FIND_PEOPLE')
@@ -39,10 +49,21 @@ def execute(self, userdata):
         cv_image = cloud_msg_to_cv_image(cloud_msg)
         bridge = CvBridge()
         images = []
+        face_images = []
         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 {0}'.format(i+1))
+            cropped_face_img = FindPeople.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)
 
         # Create the action result message
         pl = []
@@ -53,8 +74,23 @@ def execute(self, userdata):
 
             map_pose = self._listener.transformPose('/map', poses[i])
             p.pose = map_pose
-            p.body_image = images[i]
 
+            person_view = ObjectView()
+            person_view.image = images[i]
+            p.views.append(person_view)
+
+            face_view = ObjectView()
+            face_view.image = face_images[i]
+
+            if self.face_embedding_model is not None and face_view.image.data:
+                face_cv2 = bridge.imgmsg_to_cv2(face_view.image)
+                img_tensor = get_transforms()(PILImage.fromarray(face_cv2))
+                img_tensor.unsqueeze_(0)
+
+                embedding = self.face_embedding_model.forward_once(img_tensor)
+                face_view.embedding.embedding = embedding.detach().numpy().squeeze().tolist()
+
+            p.face.views.append(face_view)
             pl.append(p)
 
         # Package that actual PersonList message

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

@@ -3,22 +3,20 @@
 import os
 import numpy as np
 
+import rospy
 from rospkg import RosPack
+import face_recognition
 
 from std_msgs.msg import Header
 from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion
 from mas_perception_libs import ImageDetectionKey, ImageDetectorBase
 from mas_perception_libs.utils import cloud_msg_to_image_msg, cloud_msg_to_cv_image, \
-                                      crop_cloud_to_xyz, draw_labeled_boxes
+                                      crop_cloud_to_xyz
+from mas_perception_libs.visualization import draw_labeled_boxes
 from ssd_keras_ros import SSDKerasObjectDetector
 
 
 class FindPeople(object):
-
-    def __init__(self):
-        pass
-
-
     @staticmethod
     def detect(cloud_msg):
         # Transform point cloud to base link
@@ -84,3 +82,13 @@ def render_image_with_detections(cloud_msg, bounding_boxes):
         cv_image = cloud_msg_to_cv_image(cloud_msg)
         image = draw_labeled_boxes(cv_image, bounding_boxes)
         return image
+
+    @staticmethod
+    def extract_face_image(image_array):
+        try:
+            top, right, bottom, left = face_recognition.face_locations(image_array)[0]
+            rospy.loginfo('[find_people] Successfully extracted face from person image.')
+            return image_array[top:bottom, left:right]
+        except IndexError:
+            rospy.logwarn('[find_people] Failed to extract face from person image!')
+            return None

mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/ask_name.py

@@ -0,0 +1,40 @@
+import rospy
+import actionlib
+from actionlib_msgs.msg import GoalStatus
+
+from mdr_listen_action.msg import ListenAction, ListenGoal
+from mas_execution_manager.scenario_state_base import ScenarioStateBase
+
+class AskName(ScenarioStateBase):
+    def __init__(self, save_sm_state=False, **kwargs):
+        ScenarioStateBase.__init__(self, 'ask_name',
+                                   save_sm_state=save_sm_state,
+                                   outcomes=['succeeded', 'failed'],
+                                   output_keys=['person_name'])
+        self.sm_id = kwargs.get('sm_id', '')
+        self.state_name = kwargs.get('state_name', 'ask_name')
+        self.timeout = 10.
+
+        # wait for listen action server
+        self.listen_client = actionlib.SimpleActionClient('listen_server', ListenAction)
+        listen_wait_result = self.listen_client.wait_for_server(timeout=rospy.Duration(self.timeout))
+        if not listen_wait_result:
+            raise RuntimeError('Failed to wait for "listen_server" action')
+
+    def execute(self, userdata):
+        sentence = 'Please tell me your first name'
+        rospy.loginfo('Saying {0}'.format(sentence))
+        self.say(sentence)
+
+        goal = ListenGoal()
+        self.listen_client.send_goal(goal)
+        self.listen_client.wait_for_result(rospy.Duration.from_sec(int(self.timeout)))
+        listen_state = self.listen_client.get_state()
+        listen_result = self.listen_client.get_result()
+
+        if listen_state == GoalStatus.SUCCEEDED:
+            rospy.loginfo('Understood: {}'.format(listen_result.message))
+            userdata.person_name = listen_result.message
+            return 'succeeded'
+        rospy.logerr('Could not get input, listen action returned None')
+        return 'failed'

mdr_planning/mdr_behaviours/mdr_hri_behaviours/ros/src/mdr_hri_behaviours/say_sentence.py

@@ -0,0 +1,24 @@
+import rospy
+import random
+
+from mas_execution_manager.scenario_state_base import ScenarioStateBase
+
+class SaySentence(ScenarioStateBase):
+    def __init__(self, save_sm_state=False, **kwargs):
+        ScenarioStateBase.__init__(self, 'say_sentence',
+                                   save_sm_state=save_sm_state,
+                                   outcomes=['succeeded'])
+        self.sm_id = kwargs.get('sm_id', '')
+        self.state_name = kwargs.get('state_name', 'say_sentence')
+        self.sentences = list(kwargs.get('sentences', list()))
+        self.sleep_time = kwargs.get('sleep_time', -1.)
+
+    def execute(self, userdata):
+        sentence = random.choice(self.sentences)
+
+        rospy.loginfo('Saying: %s' % sentence)
+        self.say(sentence)
+
+        if self.sleep_time > 0:
+            rospy.sleep(self.sleep_time)
+        return 'succeeded'

mdr_planning/mdr_behaviours/mdr_navigation_behaviours/ros/src/mdr_navigation_behaviours/move_base.py

@@ -12,12 +12,14 @@ class MoveBase(ScenarioStateBase):
     def __init__(self, save_sm_state=False, **kwargs):
         ScenarioStateBase.__init__(self, 'move_base',
                                    save_sm_state=save_sm_state,
-                                   outcomes=['succeeded', 'failed', 'failed_after_retrying'])
+                                   outcomes=['succeeded', 'failed', 'failed_after_retrying'],
+                                   input_keys=['destination_locations'])
         self.sm_id = kwargs.get('sm_id', '')
         self.state_name = kwargs.get('state_name', 'move_base')
         self.move_base_server = kwargs.get('move_base_server', 'move_base_server')
         self.destination_locations = list(kwargs.get('destination_locations', list()))
         self.timeout = kwargs.get('timeout', 120.)
+        self.debug = kwargs.get('debug', False)
 
         self.number_of_retries = kwargs.get('number_of_retries', 0)
         self.retry_count = 0
@@ -28,12 +30,16 @@ def __init__(self, save_sm_state=False, **kwargs):
 
     def execute(self, userdata):
         original_location = self.kb_interface.get_robot_location(self.robot_name)
+        if len(self.destination_locations) == 0:
+            self.destination_locations = userdata.destination_locations
+            rospy.loginfo("Using userdata's destination_locations {0}".format(self.destination_locations))
+
         for destination_location in self.destination_locations:
             dispatch_msg = self.get_dispatch_msg(original_location,
                                                  destination_location)
 
             rospy.loginfo('Sending the base to %s' % destination_location)
-            self.say('Going to ' + destination_location)
+            if self.debug: self.say('Going to ' + destination_location)
             self.action_dispatch_pub.publish(dispatch_msg)
 
             self.executing = True
@@ -49,9 +55,9 @@ def execute(self, userdata):
                 original_location = destination_location
             else:
                 rospy.logerr('Could not reach %s' % destination_location)
-                self.say('Could not reach ' + destination_location)
+                if self.debug: self.say('Could not reach ' + destination_location)
                 if self.retry_count == self.number_of_retries:
-                    self.say('Aborting operation')
+                    if self.debug: self.say('Aborting operation')
                     return 'failed_after_retrying'
                 self.retry_count += 1
                 return 'failed'

mdr_planning/mdr_behaviours/mdr_perception_behaviours/ros/src/mdr_perception_behaviours/find_people.py

@@ -12,6 +12,7 @@ def __init__(self, save_sm_state=False, **kwargs):
         self.sm_id = kwargs.get('sm_id', '')
         self.state_name = kwargs.get('state_name', 'find_people')
         self.number_of_retries = kwargs.get('number_of_retries', 0)
+        self.debug = kwargs.get('debug', False)
         self.retry_count = 0
         self.timeout = 120.
 
@@ -30,14 +31,14 @@ def execute(self, userdata):
 
         if self.succeeded:
             rospy.loginfo('[find_people] Successfully found people')
-            self.say('Successfully found people')
+            if self.debug: self.say('Successfully found people')
             return 'succeeded'
 
         rospy.loginfo('Could not find people')
-        self.say('Could not find people')
+        if self.debug: self.say('Could not find people')
         if self.retry_count == self.number_of_retries:
             rospy.loginfo('[find_people] Failed to find people')
-            self.say('Aborting operation')
+            if self.debug: self.say('Aborting operation')
             return 'failed_after_retrying'
         rospy.loginfo('[find_people] Retrying to find people')
         self.retry_count += 1

mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_lab_manager/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(mdr_demo_lab_manager)
+
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+  mdr_move_arm_action
+  mdr_move_base_action
+  mdr_detect_person
+  mas_execution_manager
+  mdr_navigation_behaviours
+  mdr_perception_behaviours
+  mdr_manipulation_behaviours
+  mas_knowledge_base
+)
+
+catkin_python_setup()
+
+catkin_package(
+  CATKIN_DEPENDS
+    rospy
+    mdr_move_arm_action
+    mdr_move_base_action
+    mdr_detect_person
+    mas_execution_manager
+    mdr_navigation_behaviours
+    mdr_perception_behaviours
+    mdr_manipulation_behaviours
+    mas_knowledge_base
+)
+
+install(DIRECTORY ros/launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ros/launch
+)