From 17ac09c8e24f0649e523aaeb67c010562a1d6bca Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Tue, 16 Jan 2018 22:53:03 +0100
Subject: [PATCH 01/46] refactor(bring): split hand over from bring action,
 assuming recursive configuration

---
 .../src/action_server/actions/hand_over.py    | 192 ++++++++++++++++++
 .../src/action_server/actions/navigate_to.py  |   5 +
 2 files changed, 197 insertions(+)
 create mode 100644 action_server/src/action_server/actions/hand_over.py

diff --git a/action_server/src/action_server/actions/hand_over.py b/action_server/src/action_server/actions/hand_over.py
new file mode 100644
index 00000000..eed850df
--- /dev/null
+++ b/action_server/src/action_server/actions/hand_over.py
@@ -0,0 +1,192 @@
+from action import Action, ConfigurationData
+from find import Find
+from pick_up import PickUp
+from navigate_to import NavigateTo
+from place import Place
+from entity_description import resolve_entity_description
+
+import rospy
+
+
+class HandOver(Action):
+    """
+    The HandOver class implements the action to hand over an object to a person.
+
+    Parameters to pass to the configure() method are 'source-location' (required), 'target-location' (required) and
+    an object to bring (required).
+    """
+    def __init__(self):
+        Action.__init__(self)
+        self._required_field_prompts = {'target-location': " Who would you like me to hand the object? ",
+                                        'object': " What would you like me to hand over? "}
+        self._required_skills = ['base']
+
+    class Semantics:
+        def __init__(self):
+            self.target_location = None
+            self.object = None
+            self.source_location = None
+
+    @staticmethod
+    def _parse_semantics(semantics_dict):
+        semantics = HandOver.Semantics()
+
+        semantics.target_location = resolve_entity_description(semantics_dict['target-location'])
+        semantics.object = resolve_entity_description(semantics_dict['object'])
+
+        if 'source-location' in semantics_dict:
+            semantics.source_location = resolve_entity_description(semantics_dict['source-location'])
+
+        return semantics
+
+    class Knowledge:
+        def __init__(self):
+            self.arm_designator = None
+            self.object_designator = None
+            self.object_type = None
+            self.location_designator = None
+
+    @staticmethod
+    def _parse_knowledge(knowledge_dict):
+        knowledge = HandOver.Knowledge()
+
+        if 'arm-designator' in knowledge_dict:
+            knowledge.arm_designator = knowledge_dict['arm-designator']
+
+        if 'object-designator' in knowledge_dict:
+            knowledge.arm_designator = knowledge_dict['object-designator']
+
+        if 'object-type' in knowledge_dict:
+            knowledge.object_type = knowledge_dict['object-type']
+
+        if 'location-designator' in knowledge_dict:
+            knowledge.location_designator = knowledge_dict['location-designator']
+
+        return knowledge
+
+    def _configure(self, robot, config):
+        self._robot = robot
+        self._pick_action = None
+        self._nav_action = None
+
+        # Parse semantics and knowledge to a convenient object
+        self.semantics = self._parse_semantics(config.semantics)
+        self.knowledge = self._parse_knowledge(config.knowledge)
+
+        # We assume we already got the object if previous action passed an arm, an object and this object has the
+        # required type, or the required type is a reference.
+        got_object = (
+            self.knowledge.arm_designator is not None and (self.knowledge.object_type == self.semantics.object.type or
+                                                           self.semantics.object.type == 'reference'))
+
+        if not got_object:
+            # Request pick_up action
+            self._config_result._required_prior_action = {'action': 'pick-up',
+                                                          'object': self.semantics.object}
+            self._pick_action = PickUp()
+            pick_up_configuration_data = ConfigurationData({'object': self.semantics.object}, config.knowledge)
+            pick_up_configuration_result = self._pick_action.configure(self._robot, pick_up_configuration_data)
+            if not pick_up_configuration_result.succeeded:
+                self._config_result = pick_up_configuration_result
+                return
+            self.knowledge.arm_designator = pick_up_configuration_result.resulting_knowledge['arm-designator']
+
+        # Now we can assume we picked up the item!
+
+        at_destination = False  # TODO: Replace this hack with a decent check for the planned location of the robot.
+
+        if not at_destination:
+            # Request navigation action
+            self._nav_action = NavigateTo()
+            nav_config = ConfigurationData({'object': config.semantics['target-location']})
+            nav_config_result = self._nav_action.configure(self._robot, nav_config)
+            if not nav_config_result.succeeded:
+                self._config_result = nav_config_result
+                return
+
+        # We can now assume that we are at the destination for handover!
+
+        self._config_result.succeeded = True
+
+    def _handover(self):
+        # TODO: Move this code to the handover smach state
+        self._robot.speech.speak("I will hand over the {} now".format(self.semantics.object.type))
+        arm = self.knowledge.arm_designator.resolve()
+        arm.send_joint_goal('handover_to_human')
+        arm.wait_for_motion_done()
+
+        self._robot.speech.speak("Please take it from my gripper.", block=False)
+
+        attempt = 0
+
+        while not arm.handover_to_human(timeout=10) and attempt < 2:
+            self._robot.speech.speak("Please take it from my gripper.", block=False)
+            attempt += 1
+
+        self._robot.speech.speak("I will open my gripper now.", block=False)
+
+        self._robot.ed.update_entity(id=arm.occupied_by.id, action='remove')
+        arm.send_gripper_goal('open')
+        arm.wait_for_motion_done()
+
+        arm.reset()
+        arm.wait_for_motion_done()
+
+        arm.occupied_by = None
+
+    def _start(self):
+        # Grab
+        if self._pick_action:
+            grab_result = self._pick_action.start()
+
+            if not grab_result.succeeded:
+                self._execute_result.message = grab_result.message
+                return
+
+        # Navigate
+        if self._nav_action:
+            nav_result = self._nav_action.start()
+
+            if not nav_result.succeeded:
+                self._execute_result.message = " I was unable to go to the {}. ".format(
+                    self.semantics.target_location.id)
+
+        # Handover
+        self._handover()
+
+        self._execute_result.succeeded = True
+        if self.semantics.source_location:
+            self._execute_result.message += " I brought a {} from {} to {}. ".format(self.semantics.object.type,
+                                                                                     self.semantics.source_location.id,
+                                                                                     self.semantics.target_location.id)
+        else:
+            self._execute_result.message += " I brought a {} to {}. ".format(self.semantics.object.type,
+                                                                             self.semantics.target_location.id)
+
+    def _cancel(self):
+        pass
+
+
+if __name__ == "__main__":
+    rospy.init_node('bring_test')
+
+    import sys
+    robot_name = sys.argv[1]
+    if robot_name == 'amigo':
+        from robot_skills.amigo import Amigo as Robot
+    elif robot_name == 'sergio':
+        from robot_skills.sergio import Sergio as Robot
+    else:
+        from robot_skills.mockbot import Mockbot as Robot
+
+    robot = Robot()
+
+    action = HandOver()
+
+    config = ConfigurationData({'action': 'hand-over',
+                                'object': {'location': 'cabinet'},
+                                'source-location': {'id': 'cabinet'},
+                                'target-location': {'id': 'dinner_table'}})
+
+    action.configure(robot, config)
+    action.start()
diff --git a/action_server/src/action_server/actions/navigate_to.py b/action_server/src/action_server/actions/navigate_to.py
index 019de092..e73d7b15 100644
--- a/action_server/src/action_server/actions/navigate_to.py
+++ b/action_server/src/action_server/actions/navigate_to.py
@@ -25,6 +25,11 @@ def _configure(self, robot, config):
         self._goal_name = ""
         self._fsm = None
 
+        # If we need to navigate to "me", which resolves to "operator", plant a waypoint at the current position to
+        # navigate to.
+        # self._robot.ed.update_entity(id="operator", frame_stamped=self._robot.base.get_location(),
+        #                              type="waypoint")
+
         entity_description = config.semantics['object']
 
         (entities, error_msg) = entities_from_description(entity_description, self._robot)

From 8dae1ea6d73a5a7dfdb22764c428d2bfa0b3a02f Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Tue, 30 Jan 2018 19:48:11 +0100
Subject: [PATCH 02/46] fix(find, pick-up): use source-location instead of
 location

---
 action_server/src/action_server/actions/find.py    | 12 ++++++------
 action_server/src/action_server/actions/pick_up.py |  4 ++--
 2 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/action_server/src/action_server/actions/find.py b/action_server/src/action_server/actions/find.py
index 17ed6977..a611c7c0 100644
--- a/action_server/src/action_server/actions/find.py
+++ b/action_server/src/action_server/actions/find.py
@@ -55,8 +55,8 @@ def _configure(self, robot, config):
         self._object = resolve_entity_description(config.semantics['object'])
         self._location = None
 
-        if 'location' in config.semantics:
-            self._location = resolve_entity_description(config.semantics['location'])
+        if 'source-location' in config.semantics:
+            self._location = resolve_entity_description(config.semantics['source-location'])
         elif 'location-designator' in config.knowledge:
             e = config.knowledge['location-designator'].resolve()
             if not e:
@@ -64,12 +64,12 @@ def _configure(self, robot, config):
                     self._config_result.message = " Where should I look for the {}?".format(self._object.id)
                 else:
                     self._config_result.message = " Where should I look for the {}?".format(self._object.type)
-                self._config_result.missing_field = 'location'
+                self._config_result.missing_field = 'source-location'
                 return
             self._location = EntityDescription(id=e.id)
         else:
             self._config_result.message = " Where should I look for the {}?".format(self._object.id)
-            self._config_result.missing_field = 'location'
+            self._config_result.missing_field = 'source-location'
             return
 
         # We (safely) assume that self._location is an EntityDescription object
@@ -187,7 +187,7 @@ def _cancel(self):
     action = Find()
 
     config = ConfigurationData({'action': 'find',
-              'location': {'id': 'cabinet',
+              'source-location': {'id': 'cabinet',
                            'area': 'on_top_of'},
               'object': {'type': 'coke'}})
 
@@ -195,7 +195,7 @@ def _cancel(self):
     action.start()
 
     config = ConfigurationData({'action': 'find',
-              'location': {'id': 'livingroom',
+              'source-location': {'id': 'livingroom',
                            'area': 'in'},
               'object': {'type': 'person'}})
 
diff --git a/action_server/src/action_server/actions/pick_up.py b/action_server/src/action_server/actions/pick_up.py
index 08ef4781..7b327d63 100644
--- a/action_server/src/action_server/actions/pick_up.py
+++ b/action_server/src/action_server/actions/pick_up.py
@@ -47,7 +47,7 @@ def _configure(self, robot, config):
         if not using_knowledge_for_object:
             # Check if the task included a location to grasp from, otherwise try to get the information from previous
             # actions' knowledge.
-            if 'location' in config.semantics:
+            if 'source-location' in config.semantics:
                 pass
             elif 'location-designator' in config.knowledge:
                 pass
@@ -55,7 +55,7 @@ def _configure(self, robot, config):
                 using_knowledge_for_object = True
                 pass
             else:
-                self._config_result.missing_field = 'location'
+                self._config_result.missing_field = 'source-location'
                 self._config_result.message = " Where would you like me to pick that up? "
                 return
 

From cda9b18838ccfa87f077d348cb24295a38c18635 Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Tue, 30 Jan 2018 19:49:27 +0100
Subject: [PATCH 03/46] fix(hand-over): pass complete semantics and knowledge
 to pick-up

---
 action_server/src/action_server/actions/hand_over.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/action_server/src/action_server/actions/hand_over.py b/action_server/src/action_server/actions/hand_over.py
index eed850df..252e3139 100644
--- a/action_server/src/action_server/actions/hand_over.py
+++ b/action_server/src/action_server/actions/hand_over.py
@@ -84,7 +84,7 @@ def _configure(self, robot, config):
             self._config_result._required_prior_action = {'action': 'pick-up',
                                                           'object': self.semantics.object}
             self._pick_action = PickUp()
-            pick_up_configuration_data = ConfigurationData({'object': self.semantics.object}, config.knowledge)
+            pick_up_configuration_data = ConfigurationData(config.semantics, config.knowledge)
             pick_up_configuration_result = self._pick_action.configure(self._robot, pick_up_configuration_data)
             if not pick_up_configuration_result.succeeded:
                 self._config_result = pick_up_configuration_result

From 0883ba72f533009b288a395b2b64959701c61992 Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Tue, 30 Jan 2018 19:50:25 +0100
Subject: [PATCH 04/46] feat(action_server): expose hand-over action

---
 action_server/src/action_server/actions/__init__.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/action_server/src/action_server/actions/__init__.py b/action_server/src/action_server/actions/__init__.py
index c48895da..5a4c10e8 100644
--- a/action_server/src/action_server/actions/__init__.py
+++ b/action_server/src/action_server/actions/__init__.py
@@ -16,3 +16,4 @@
 from guide import Guide
 from demo_presentation import DemoPresentation
 from turn_toward_sound import TurnTowardSound
+from hand_over import HandOver

From 3c163f1c5acdd398631568d34f80bd1d872f3a33 Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Sun, 4 Feb 2018 16:02:09 +0100
Subject: [PATCH 05/46] refactor(hand-over): hand-over working with action
 inference

---
 .../src/action_server/actions/action.py       | 42 +++++----
 .../src/action_server/actions/bring.py        |  6 +-
 .../actions/entity_description.py             | 10 +++
 .../action_server/actions/example_action.py   | 16 ++--
 .../src/action_server/actions/find.py         |  6 +-
 .../src/action_server/actions/hand_over.py    | 89 +++++++------------
 .../src/action_server/actions/navigate_to.py  |  9 +-
 .../src/action_server/actions/pick_up.py      | 45 +++++-----
 action_server/src/action_server/server.py     |  3 +-
 .../src/action_server/task_manager.py         | 52 ++++++++++-
 10 files changed, 160 insertions(+), 118 deletions(-)

diff --git a/action_server/src/action_server/actions/action.py b/action_server/src/action_server/actions/action.py
index cfe5b904..f9ed2f75 100644
--- a/action_server/src/action_server/actions/action.py
+++ b/action_server/src/action_server/actions/action.py
@@ -8,39 +8,40 @@ class ConfigurationData(object):
     """
     The ConfigurationData class defines the input data structure for configuration of an action.
     """
-    def __init__(self, semantics, knowledge=None):
+    def __init__(self, semantics, context=None):
         """
         :param semantics: Dictionary of the following structure: {'action': <action-name>, 'param1': <param-value>}.
-        :param knowledge: Dictionary of parameter names to knowledge provided by previous actions.
+        :param context: Dictionary of parameter names to context provided by previous actions.
         """
         self.semantics = semantics
-        if knowledge:
-            self.knowledge = knowledge
+        if context:
+            self.context = context
         else:
-            self.knowledge = {}
+            self.context = {}
 
     def __repr__(self):
-        return "ConfigurationData(semantics=%s, knowledge=%s)" % (self.semantics, self.knowledge)
+        return "ConfigurationData(semantics={}, context={})".format(self.semantics, self.context)
 
 
 class ConfigurationResult(object):
     """
     The ConfigurationResult class defines the data structure that is returned by the configure() methods of actions
     """
-    def __init__(self, succeeded=False, resulting_knowledge=None):
-        if resulting_knowledge is None:
-            resulting_knowledge = {}
+    def __init__(self, succeeded=False, context=None):
+        if context is None:
+            context = {}
 
         self.succeeded = succeeded
-        self.resulting_knowledge = resulting_knowledge
+        self.context = context
+        self.required_context = None
         self.missing_field = ""
         self.missing_skill = None
         self.message = ""
 
     def __repr__(self):
-        return "ConfigurationResult(succeeded=%s, resulting_knowledge=%s, missing_field=%s, missing_skill=%s, " \
-               "message=%s)" % (self.succeeded, self.resulting_knowledge, self.missing_field, self.missing_skill,
-                                self.message)
+        return "ConfigurationResult(succeeded={}, context={}, required_context={}, missing_field={}, " \
+               "missing_skill={}, message={})".format(self.succeeded, self.context, self.required_context,
+                                                  self.missing_field, self.missing_skill, self.message)
 
 
 class ActionResult(object):
@@ -76,8 +77,8 @@ def _check_parameters(self, config):
                 return False
 
         for k, v in self._required_passed_knowledge.items():
-            if k not in config.knowledge:
-                rospy.logerr("Missing required knowledge {}".format(k))
+            if k not in config.context:
+                rospy.logerr("Missing required context {}".format(k))
                 self._config_result.missing_field = k
                 self._config_result.message = v
                 return False
@@ -97,18 +98,21 @@ def configure(self, robot, config):
         Configure the action with a robot and configuration data
         :param robot: The robot to use for this action
         :type robot: Robot
-        :param config: The configuration data. Contains semantics from input and implied knowledge from previous tasks.
+        :param config: The configuration data. Contains semantics from input and implied context from previous tasks.
         :type config: ConfigurationData
         :return: The result of configuration
         :rtype: ConfigurationResult
         """
-        rospy.loginfo("Configuring action {} with semantics {} and knowledge {}.".
-                      format(self.get_name(), config.semantics, config.knowledge))
+        self._config_result = ConfigurationResult()
+
         if not isinstance(config, ConfigurationData):
             rospy.logerr("Action: the specified config should be ConfigurationData! I received: %s" % str(config))
             self._config_result.message = " Something's wrong with my wiring. I'm so sorry, but I cannot do this. "
             return self._config_result
 
+        rospy.loginfo("Configuring action {} with semantics {} and context {}.".
+                      format(self.get_name(), config.semantics, config.context))
+
         if not isinstance(robot, Robot):
             rospy.logerr("Action: the specified robot should be a Robot! I received: %s" % str(robot))
             self._config_result.message = " I don't know what to say. I'm having an identity crisis. I'm so sorry. "
@@ -117,7 +121,7 @@ def configure(self, robot, config):
         if self._check_parameters(config) and self._check_skills(robot):
             self._configure(robot, config)
 
-        rospy.loginfo("Resulting knowledge = {}".format(self._config_result.resulting_knowledge))
+        rospy.loginfo("Resulting context = {}".format(self._config_result.context))
 
         return self._config_result
 
diff --git a/action_server/src/action_server/actions/bring.py b/action_server/src/action_server/actions/bring.py
index 1e12c40b..acf43b26 100644
--- a/action_server/src/action_server/actions/bring.py
+++ b/action_server/src/action_server/actions/bring.py
@@ -55,7 +55,7 @@ def _configure(self, robot, config):
                     return
 
                 # Use the object designator from the find action to resolve to the object we want to bring
-                self._found_object_designator = find_config_result.resulting_knowledge['object-designator']
+                self._found_object_designator = find_config_result.context['object-designator']
                 self._source_location = resolve_entity_description(config.semantics['source-location'])
 
             # If the task is something like "... and bring it to me", the "it" refers to something we already found or even
@@ -86,7 +86,7 @@ def _configure(self, robot, config):
                     return
 
                 # Use the object designator from the find action to resolve to the object we want to bring
-                self._found_object_designator = find_config_result.resulting_knowledge['object-designator']
+                self._found_object_designator = find_config_result.context['object-designator']
 
                 # rospy.loginfo("I really don't know where to get this thing. ")
                 # self._config_result.message = " Where would you like me to get it? "
@@ -101,7 +101,7 @@ def _configure(self, robot, config):
             if not grab_config_result.succeeded:
                 self._config_result = grab_config_result
                 return
-            self._arm_designator = grab_config_result.resulting_knowledge['arm-designator']
+            self._arm_designator = grab_config_result.context['arm-designator']
 
 
         self._target_location = resolve_entity_description(config.semantics['target-location'])
diff --git a/action_server/src/action_server/actions/entity_description.py b/action_server/src/action_server/actions/entity_description.py
index 7389506d..84a6fc6e 100644
--- a/action_server/src/action_server/actions/entity_description.py
+++ b/action_server/src/action_server/actions/entity_description.py
@@ -6,6 +6,16 @@ def __init__(self, id=None, type=None, location=None, area=None):
         self.location = location
         self.area = area
 
+    def __repr__(self):
+        return "EntityDescription(id={id},type={type},location={location},area={area})".format(id=self.id,
+                                                                                               type=self.type,
+                                                                                               location=self.location,
+                                                                                               area=self.area)
+
+    def __eq__(self, other):
+        return self.__dict__ == other.__dict__
+
+
 def resolve_entity_description(parameters):
     description = EntityDescription()
 
diff --git a/action_server/src/action_server/actions/example_action.py b/action_server/src/action_server/actions/example_action.py
index 587e965d..8d3ebe36 100644
--- a/action_server/src/action_server/actions/example_action.py
+++ b/action_server/src/action_server/actions/example_action.py
@@ -8,7 +8,7 @@ class ExampleAction(Action):
     """
     The ExampleAction class explains how Actions, as handled by the TaskManager, work.
 
-    As by example, it takes passed knowledge of a color passed by a previous Action, passes an Entity to the next
+    As by example, it takes passed context of a color passed by a previous Action, passes an Entity to the next
     Action and uses a dict mapping from object types to colors from the common knowledge. A practical use would be the
     following:
 
@@ -41,7 +41,7 @@ def __init__(self):
         self._required_skills = ['ed']
 
         """
-        Similarly, you can set the required keys in the knowledge. Their availability is also checked in the configure
+        Similarly, you can set the required keys in the context. Their availability is also checked in the configure
         step.
         """
         self._required_passed_knowledge = ['color']
@@ -62,7 +62,7 @@ def __init__(self):
     def _configure(self, robot, config):
         """
         :param robot: The robot to use for this action
-        :param config: The configuration of the action, containing fields semantics and knowledge
+        :param config: The configuration of the action, containing fields semantics and context
         :return: ConfigurationResult
 
         The configure step is typically done just after the command is given. It is intended to check if the task makes
@@ -91,7 +91,7 @@ def _configure(self, robot, config):
 
         """
         Knowledge gained in previous Actions passed on to this Action can be accessed through config. The availability
-        of this field is checked by the base class (because we specified 'color' as required passed knowledge), but you
+        of this field is checked by the base class (because we specified 'color' as required passed context), but you
         should perform all the necessary checks on this data to make sure that it is what you expect it to be!
         """
         if not isinstance(config.knowledge['color'], VariableDesignator):
@@ -114,14 +114,14 @@ def _configure(self, robot, config):
         self._entity_designator = VariableDesignator(resolve_type=Entity)
 
         """
-        Pass on the resulting knowledge by putting it in the resulting knowledge (this action results in some Entity).
-        The following action can use this data by accessing its config.knowledge['object']
+        Pass on the resulting context by putting it in the resulting context (this action results in some Entity).
+        The following action can use this data by accessing its config.context['object']
         """
         self._config_result.resulting_knowledge['object'] = self._entity_designator
 
         """
         Typically, we instantiate a (smach) state machine here, which will do the actual work of the action. In this
-        case, it will be some state machine filling an object designator based on the knowledge passed to this Action.
+        case, it will be some state machine filling an object designator based on the context passed to this Action.
         For more information on designators, how they work and why we need them, ask @LoyVanBeek.
         """
         self._action_fsm = DummyStateMachine(robot, self._color_designator, self._entity_designator, object_colors)
@@ -139,7 +139,7 @@ def _start(self):
 
         """
         We typically run the state machine here. Under the hood, this state machine fills the entity designator. Because
-        this is the same object as the one in our _config_result.resulting_knowledge['object'], the knowledge of this
+        this is the same object as the one in our _config_result.resulting_knowledge['object'], the context of this
         entity is passed to any following states that used this Designator.
         """
         result = self._action_fsm.execute()
diff --git a/action_server/src/action_server/actions/find.py b/action_server/src/action_server/actions/find.py
index a611c7c0..0ed7a96f 100644
--- a/action_server/src/action_server/actions/find.py
+++ b/action_server/src/action_server/actions/find.py
@@ -57,8 +57,8 @@ def _configure(self, robot, config):
 
         if 'source-location' in config.semantics:
             self._location = resolve_entity_description(config.semantics['source-location'])
-        elif 'location-designator' in config.knowledge:
-            e = config.knowledge['location-designator'].resolve()
+        elif 'location-designator' in config.context:
+            e = config.context['location-designator'].resolve()
             if not e:
                 if self._object.id:
                     self._config_result.message = " Where should I look for the {}?".format(self._object.id)
@@ -123,7 +123,7 @@ def _configure(self, robot, config):
                                                              navigation_area_designator=navigation_area_designator,
                                                              found_entity_designator=self._found_entity_designator))
 
-        self._config_result.resulting_knowledge['object-designator'] = self._found_entity_designator
+        self._config_result.context['object-designator'] = self._found_entity_designator
         self._config_result.succeeded = True
 
     def _start(self):
diff --git a/action_server/src/action_server/actions/hand_over.py b/action_server/src/action_server/actions/hand_over.py
index 252e3139..2d94236d 100644
--- a/action_server/src/action_server/actions/hand_over.py
+++ b/action_server/src/action_server/actions/hand_over.py
@@ -1,8 +1,4 @@
 from action import Action, ConfigurationData
-from find import Find
-from pick_up import PickUp
-from navigate_to import NavigateTo
-from place import Place
 from entity_description import resolve_entity_description
 
 import rospy
@@ -39,71 +35,70 @@ def _parse_semantics(semantics_dict):
 
         return semantics
 
-    class Knowledge:
+    class Context:
         def __init__(self):
             self.arm_designator = None
             self.object_designator = None
             self.object_type = None
             self.location_designator = None
+            self.location = None
 
     @staticmethod
-    def _parse_knowledge(knowledge_dict):
-        knowledge = HandOver.Knowledge()
+    def _parse_context(context_dict):
+        context = HandOver.Context()
 
-        if 'arm-designator' in knowledge_dict:
-            knowledge.arm_designator = knowledge_dict['arm-designator']
+        if 'arm-designator' in context_dict:
+            context.arm_designator = context_dict['arm-designator']
 
-        if 'object-designator' in knowledge_dict:
-            knowledge.arm_designator = knowledge_dict['object-designator']
+        if 'object-designator' in context_dict:
+            context.object_designator = context_dict['object-designator']
 
-        if 'object-type' in knowledge_dict:
-            knowledge.object_type = knowledge_dict['object-type']
+        if 'object-type' in context_dict:
+            context.object_type = context_dict['object-type']
 
-        if 'location-designator' in knowledge_dict:
-            knowledge.location_designator = knowledge_dict['location-designator']
+        if 'location-designator' in context_dict:
+            context.location_designator = context_dict['location-designator']
 
-        return knowledge
+        if 'location' in context_dict:
+            context.location = resolve_entity_description(context_dict['location'])
+
+        return context
 
     def _configure(self, robot, config):
         self._robot = robot
         self._pick_action = None
         self._nav_action = None
 
-        # Parse semantics and knowledge to a convenient object
+        # Parse semantics and context to a convenient object
         self.semantics = self._parse_semantics(config.semantics)
-        self.knowledge = self._parse_knowledge(config.knowledge)
+        self.context = self._parse_context(config.context)
 
+        # Express the initial conditions in rules based on input
         # We assume we already got the object if previous action passed an arm, an object and this object has the
         # required type, or the required type is a reference.
         got_object = (
-            self.knowledge.arm_designator is not None and (self.knowledge.object_type == self.semantics.object.type or
-                                                           self.semantics.object.type == 'reference'))
+            self.context.arm_designator is not None and (self.context.object_type == self.semantics.object.type or
+                                                         self.semantics.object.type == 'reference'))
 
+        # If precondition not met, request prior action from the task manager
         if not got_object:
             # Request pick_up action
-            self._config_result._required_prior_action = {'action': 'pick-up',
-                                                          'object': self.semantics.object}
-            self._pick_action = PickUp()
-            pick_up_configuration_data = ConfigurationData(config.semantics, config.knowledge)
-            pick_up_configuration_result = self._pick_action.configure(self._robot, pick_up_configuration_data)
-            if not pick_up_configuration_result.succeeded:
-                self._config_result = pick_up_configuration_result
-                return
-            self.knowledge.arm_designator = pick_up_configuration_result.resulting_knowledge['arm-designator']
-
+            self._config_result.required_context = {'action': 'pick-up',
+                                                    'object': config.semantics['object'],
+                                                    'source-location': config.semantics['source-location']}
+            return
         # Now we can assume we picked up the item!
 
-        at_destination = False  # TODO: Replace this hack with a decent check for the planned location of the robot.
+        # We should have navigated to the place where we should hand over
+        at_destination = (self.context.location is not None and
+                          (self.context.location == self.semantics.target_location or
+                           self.semantics.target_location.type == 'reference'))
 
         if not at_destination:
             # Request navigation action
-            self._nav_action = NavigateTo()
-            nav_config = ConfigurationData({'object': config.semantics['target-location']})
-            nav_config_result = self._nav_action.configure(self._robot, nav_config)
-            if not nav_config_result.succeeded:
-                self._config_result = nav_config_result
-                return
-
+            self._config_result.required_context = {'action': 'navigate-to',
+                                                    'object': config.semantics['target-location']}
+            return
         # We can now assume that we are at the destination for handover!
 
         self._config_result.succeeded = True
@@ -111,7 +106,7 @@ def _configure(self, robot, config):
     def _handover(self):
         # TODO: Move this code to the handover smach state
         self._robot.speech.speak("I will hand over the {} now".format(self.semantics.object.type))
-        arm = self.knowledge.arm_designator.resolve()
+        arm = self.context.arm_designator.resolve()
         arm.send_joint_goal('handover_to_human')
         arm.wait_for_motion_done()
 
@@ -135,22 +130,6 @@ def _handover(self):
         arm.occupied_by = None
 
     def _start(self):
-        # Grab
-        if self._pick_action:
-            grab_result = self._pick_action.start()
-
-            if not grab_result.succeeded:
-                self._execute_result.message = grab_result.message
-                return
-
-        # Navigate
-        if self._nav_action:
-            nav_result = self._nav_action.start()
-
-            if not nav_result.succeeded:
-                self._execute_result.message = " I was unable to go to the {}. ".format(
-                    self.semantics.target_location.id)
-
         # Handover
         self._handover()
 
diff --git a/action_server/src/action_server/actions/navigate_to.py b/action_server/src/action_server/actions/navigate_to.py
index e73d7b15..62fab6e5 100644
--- a/action_server/src/action_server/actions/navigate_to.py
+++ b/action_server/src/action_server/actions/navigate_to.py
@@ -27,8 +27,8 @@ def _configure(self, robot, config):
 
         # If we need to navigate to "me", which resolves to "operator", plant a waypoint at the current position to
         # navigate to.
-        # self._robot.ed.update_entity(id="operator", frame_stamped=self._robot.base.get_location(),
-        #                              type="waypoint")
+        self._robot.ed.update_entity(id="operator", frame_stamped=self._robot.base.get_location(),
+                                     type="waypoint")
 
         entity_description = config.semantics['object']
 
@@ -58,7 +58,6 @@ def _configure(self, robot, config):
                 rospy.loginfo("No knowledge of a {} in the world model.".format(entity_description['id']))
                 self._config_result.message = " I have no knowledge of a {} in my world model. ".format(
                     entity_description["id"])
-            return
         else:
             # Take the best match and set up the state machine
             e = entities[0]
@@ -85,8 +84,10 @@ def _configure(self, robot, config):
                                                entity_designator_area_name_map={entity_designator: area},
                                                entity_lookat_designator=entity_designator)
 
-        self._config_result.resulting_knowledge['location-designator'] = entity_designator
+        self._config_result.context['location-designator'] = entity_designator
+        self._config_result.context['location'] = config.semantics['object']
         self._config_result.succeeded = True
+        return
 
 
     def _start(self):
diff --git a/action_server/src/action_server/actions/pick_up.py b/action_server/src/action_server/actions/pick_up.py
index 7b327d63..21efeefe 100644
--- a/action_server/src/action_server/actions/pick_up.py
+++ b/action_server/src/action_server/actions/pick_up.py
@@ -25,12 +25,12 @@ def __init__(self):
 
     def _configure(self, robot, config):
         # Check if the task included an object to grab, otherwise try to get the information from previous actions'
-        # knowledge.
-        using_knowledge_for_object = False
+        # context.
+        using_context_for_object = False
         if 'object' in config.semantics:
             if config.semantics['object']['type'] == 'reference':
-                if 'object-designator' in config.knowledge:
-                    using_knowledge_for_object = True
+                if 'object-designator' in config.context:
+                    using_context_for_object = True
                     pass
                 else:
                     self._config_result.missing_field = 'object'
@@ -44,22 +44,22 @@ def _configure(self, robot, config):
             self._config_result.message = " What would you like me to pick up? "
             return
 
-        if not using_knowledge_for_object:
+        if not using_context_for_object:
             # Check if the task included a location to grasp from, otherwise try to get the information from previous
-            # actions' knowledge.
+            # actions' context.
             if 'source-location' in config.semantics:
                 pass
-            elif 'location-designator' in config.knowledge:
+            elif 'location-designator' in config.context:
                 pass
-            elif 'object-designator' in config.knowledge:
-                using_knowledge_for_object = True
+            elif 'object-designator' in config.context:
+                using_context_for_object = True
                 pass
             else:
                 self._config_result.missing_field = 'source-location'
                 self._config_result.message = " Where would you like me to pick that up? "
                 return
 
-        if not using_knowledge_for_object:
+        if not using_context_for_object:
             # Only filter to entities that can be picked up, e.g not furniture etc
             manipulable_object_types = [o['name'] for o in self._knowledge.objects]
             if not config.semantics['object']['type'] in manipulable_object_types:
@@ -68,15 +68,15 @@ def _configure(self, robot, config):
                 return
 
             self._find_action = Find()
-            find_config = ConfigurationData(config.semantics, config.knowledge)
+            find_config = ConfigurationData(config.semantics, config.context)
             find_config_result = self._find_action.configure(robot, find_config)
             if not find_config_result.succeeded:
                 self._config_result = find_config_result
                 return
-            config.knowledge['object-designator'] = find_config_result.resulting_knowledge['object-designator']
+            config.context['object-designator'] = find_config_result.context['object-designator']
 
-        # Add the found object to the knowledge that is passed to the next task
-        self._config_result.resulting_knowledge['object-designator'] = config.knowledge['object-designator']
+        # Add the found object to the context that is passed to the next task
+        self._config_result.context['object-designator'] = config.context['object-designator']
 
         self._robot = robot
 
@@ -86,10 +86,11 @@ def _configure(self, robot, config):
         arm_des.lock()
 
         self._fsm = robot_smach_states.grab.Grab(self._robot,
-                                                 item=config.knowledge['object-designator'],
+                                                 item=config.context['object-designator'],
                                                  arm=arm_des)
 
-        self._config_result.resulting_knowledge['arm-designator'] = arm_des
+        self._config_result.context['arm-designator'] = arm_des
+        self._config_result.context['object-type'] = config.semantics['object']['type']
         self._config_result.succeeded = True
 
     def _start(self):
@@ -106,19 +107,19 @@ def _start(self):
             self._execute_result.succeeded = True
             if self._find_action:
                 self._execute_result.message += " And I picked it up. "
-            elif not self._config_result.resulting_knowledge['object-designator'].resolve():
+            elif not self._config_result.context['object-designator'].resolve():
                 self._execute_result.message += " I could not pick anything up. ".\
-                    format(self._config_result.resulting_knowledge)
+                    format(self._config_result.context)
             else:
                 self._execute_result.message += " I picked up the {}. ".\
-                    format(self._config_result.resulting_knowledge['object-designator'].resolve().type)
+                    format(self._config_result.context['object-designator'].resolve().type)
         else:
-            if not self._config_result.resulting_knowledge['object-designator'].resolve():
+            if not self._config_result.context['object-designator'].resolve():
                 self._execute_result.message += " I could not pick anything up. ". \
-                    format(self._config_result.resulting_knowledge)
+                    format(self._config_result.context)
             else:
                 self._execute_result.message += " I could not pick up the {}. ".\
-                    format(self._config_result.resulting_knowledge['object-designator'].resolve().type)
+                    format(self._config_result.context['object-designator'].resolve().type)
 
     def _cancel(self):
         if self._fsm.is_running:
diff --git a/action_server/src/action_server/server.py b/action_server/src/action_server/server.py
index df8e6df2..0480e68c 100644
--- a/action_server/src/action_server/server.py
+++ b/action_server/src/action_server/server.py
@@ -88,7 +88,8 @@ def _add_action_cb(self, goal):
                     rospy.logdebug("Execution of state machine aborted because action failed.")
                     return
         except Exception as e:
-            raise type(e)("An error occurred using task recipe: %s\n" % goal.recipe + e.message)
+            rospy.logerr("An error occurred using task recipe: %s\n" % goal.recipe)
+            raise
 
         rospy.logdebug("Execution of state machine succeeded.")
         self._result.result = action_server_msgs.msg.TaskResult.RESULT_SUCCEEDED
diff --git a/action_server/src/action_server/task_manager.py b/action_server/src/action_server/task_manager.py
index 52228a94..a012f3b1 100644
--- a/action_server/src/action_server/task_manager.py
+++ b/action_server/src/action_server/task_manager.py
@@ -25,6 +25,52 @@ def clear(self):
         self.done = True
         self._active_action = None
 
+    def get_action_from_context(self, context):
+        # TODO: The content of 'context' should be thought out better.
+        action_name = context['action']
+        return self._action_factory.get_action(action_name)()
+
+    def recursive_configure(self, action, configuration_data):
+        preconditions_met = False
+        action_list = []  # List of configured actions
+        configuration_result = ConfigurationResult()
+
+        # Try configuring the action until all of its preconditions are met
+        while not preconditions_met:
+            configuration_result = action.configure(self._robot, configuration_data)
+
+            # If context is required, we will need to configure an action prior to the current one
+            if configuration_result.required_context:
+                # We derive the required action from the required context
+                # TODO: Using postconditions of actions, we could infer what kind of prior action is necessary
+                prior_action = self.get_action_from_context(configuration_result.required_context)
+
+                # Configure the prior action using the same recursive strategy
+                prior_config_data = ConfigurationData(configuration_result.required_context,
+                                                      configuration_data.context)
+                prior_action_list, prior_action_config_result = self.recursive_configure(prior_action,
+                                                                                         prior_config_data)
+
+                # Add all of the resulting actions from this configuration to the action list
+                action_list += prior_action_list
+
+                # If configuration did not succeed, we need to stop and report what went wrong there.
+                if not prior_action_config_result.succeeded:
+                    configuration_result = prior_action_config_result
+                    break
+
+                # Add the resulting context to the context of the current action, to get ahead in its configuration
+                configuration_data.context.update(prior_action_config_result.context)
+
+            # If configuration succeeded and no more context is required, we can append the current action to the list
+            else:
+                if configuration_result.succeeded:
+                    action_list.append(action)
+                preconditions_met = True
+
+        # Return the list of configured actions and the current configuration result
+        return action_list, configuration_result
+
     def set_up_state_machine(self, recipe):
         configuration_result = ConfigurationResult()
 
@@ -40,11 +86,11 @@ def set_up_state_machine(self, recipe):
             Action = self._action_factory.get_action(action_name)
             action = Action()
 
-            config_data = ConfigurationData(instruction, configuration_result.resulting_knowledge)
+            config_data = ConfigurationData(instruction, configuration_result.context)
 
             # Try to configure the action
             try:
-                configuration_result = action.configure(self._robot, config_data)
+                action_list, configuration_result = self.recursive_configure(action, config_data)
             except Exception as e:
                 # if the action crashes, assume that the other actions in the sequence become invalid,
                 # so clear the action sequence before re-raising the exception
@@ -54,7 +100,7 @@ def set_up_state_machine(self, recipe):
 
             # If action configuration succeeded, append configured action to action sequence
             if configuration_result.succeeded:
-                self._action_sequence.append(action)
+                self._action_sequence += action_list
             # If action configuration failed, return the configuration result, specifying what went wrong
             elif configuration_result.missing_field:
                 configuration_result.missing_field = "[{}].".format(i) + configuration_result.missing_field

From 9442a5e7dba0c44e57d6999889cf434b69ea349d Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Wed, 7 Feb 2018 09:17:22 +0100
Subject: [PATCH 06/46] refactor(handover): remove unused instance variables

---
 action_server/src/action_server/actions/hand_over.py | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/action_server/src/action_server/actions/hand_over.py b/action_server/src/action_server/actions/hand_over.py
index 2d94236d..1bb9c4c3 100644
--- a/action_server/src/action_server/actions/hand_over.py
+++ b/action_server/src/action_server/actions/hand_over.py
@@ -38,7 +38,7 @@ def _parse_semantics(semantics_dict):
     class Context:
         def __init__(self):
             self.arm_designator = None
-            self.object_designator = None
+            self.object_designator = None  # TODO: bundle object_designator and object (as a description and a ref)
             self.object_type = None
             self.location_designator = None
             self.location = None
@@ -66,8 +66,6 @@ def _parse_context(context_dict):
 
     def _configure(self, robot, config):
         self._robot = robot
-        self._pick_action = None
-        self._nav_action = None
 
         # Parse semantics and context to a convenient object
         self.semantics = self._parse_semantics(config.semantics)

From 7871db985c08dbd3999b459872d50a9d424ea7d2 Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Wed, 7 Feb 2018 09:18:06 +0100
Subject: [PATCH 07/46] wip(place): align with hand over action

---
 .../src/action_server/actions/place.py        | 190 ++++++++++--------
 1 file changed, 106 insertions(+), 84 deletions(-)

diff --git a/action_server/src/action_server/actions/place.py b/action_server/src/action_server/actions/place.py
index ee3bb28a..4a8483ce 100644
--- a/action_server/src/action_server/actions/place.py
+++ b/action_server/src/action_server/actions/place.py
@@ -21,122 +21,144 @@ class Place(Action):
     '''
     def __init__(self):
         Action.__init__(self)
-        self._place = None
-        self._thread = None
-        self._goal_entity = None
 
-        self._required_field_prompts = {'location': " Where should I leave the object? "}
-
-        self._required_passed_knowledge = {'object-designator': " I won't be able to place anything before I grasped "
-                                                                "it. Please tell me what to get. "}
+        self._required_field_prompts = {'target-location': " Where should I leave the object? "}
 
         self._required_skills = ['arms']
 
-    def _configure(self, robot, config):
-        # TODO: remove right and left
-        if not hasattr(robot, 'rightArm') or not hasattr(robot, 'leftArm'):
-            rospy.logerr("Robot {} does not have attribute 'speech'".format(robot.robot_name))
-            self._config_result.missing_skill = "speech"
-            return
+    class Semantics:
+        def __init__(self):
+            self.target_location = None
+            self.object = None
+            self.source_location = None
+            self.arm = None
 
-        self._robot = robot
+    @staticmethod
+    def _parse_semantics(semantics_dict):
+        semantics = Place.Semantics()
 
-        self._goal_entity = resolve_entity_description(config.semantics["location"])
+        semantics.target_location = resolve_entity_description(semantics_dict['target-location'])
 
-        (entities, error_msg) = entities_from_description(config.semantics["location"], robot)
+        if 'object' in semantics_dict:
+            semantics.object = resolve_entity_description(semantics_dict['object'])
 
-        if not entities:
-            rospy.logwarn(error_msg)
-            self._config_result.message = " I have no knowledge of a {} in my world model. ".\
-                format(config.semantics['location']['id'])
-            return
-        self._place_entity = entities[0]
+        if 'source-location' in semantics_dict:
+            semantics.source_location = resolve_entity_description(semantics_dict['source-location'])
 
-        try:
-            side = config.semantics["side"]
-        except KeyError:
-            side = "right"  # Default
+        if 'arm' in semantics_dict:
+            semantics.arm = semantics_dict['arm']
 
-        if side == "left":
-            self._arm = robot.leftArm
-            self._goal_y = 0.2
-        else:
-            self._arm = robot.rightArm
-            self._goal_y = -0.2
+        return semantics
 
-        try:
-            self._arm_designator = config.knowledge['arm-designator']
-        except:
-            self._arm_designator = None
+    class Context:
+        def __init__(self):
+            self.arm_designator = None
+            self.object_designator = None
+            self.object_type = None
+            self.location_designator = None
+            self.location = None
 
-        self._object_designator = config.knowledge['object-designator']
+    @staticmethod
+    def _parse_context(context_dict):
+        context = Place.Context()
 
-        try:
-            self._height = config.semantics["height"]
-        except KeyError:
-            self._height = 0.8
+        if 'arm-designator' in context_dict:
+            context.arm_designator = context_dict['arm-designator']
+
+        if 'object-designator' in context_dict:
+            context.object_designator = context_dict['object-designator']
+
+        if 'object-type' in context_dict:
+            context.object_type = context_dict['object-type']
+
+        if 'location-designator' in context_dict:
+            context.location_designator = context_dict['location-designator']
+
+        if 'location' in context_dict:
+            context.location = resolve_entity_description(context_dict['location'])
+
+        return context
+
+    def _configure(self, robot, config):
+        self._robot = robot
+
+        # Parse semantics and context to a convenient object
+        self.semantics = self._parse_semantics(config.semantics)
+        self.context = self._parse_context(config.context)
+
+        # Express the initial conditions in rules based on input
+        # We assume we already got the object if previous action passed an arm, an object and this object has the
+        # required type, or the required type is a reference.
+        got_object = (
+            self.context.arm_designator is not None and (self.context.object_type == self.semantics.object.type or
+                                                         self.semantics.object.type == 'reference'))
+        got_object = got_object or [arm_name for arm_name, arm in self._robot.arms.iteritems() if arm.occupied_by]
+
+        # If precondition not met, request prior action from the task manager
+        if not got_object:
+            # Request pick_up action
+            self._config_result.required_context = {'action': 'pick-up',
+                                                    'object': config.semantics['object'],
+                                                    'source-location': config.semantics['source-location']}
+            return
+        # Now we can assume we picked up the item!
+
+        # We should have navigated to the place where we should hand over
+        at_destination = (self.context.location is not None and
+                          (self.context.location == self.semantics.target_location or
+                           self.semantics.target_location.type == 'reference'))
+
+        if not at_destination:
+            # Request navigation action
+            self._config_result.required_context = {'action': 'navigate-to',
+                                                    'object': config.semantics['target-location']}
+            return
+        # We can now assume that we are at the destination for handover!
 
         self._config_result.succeeded = True
+        return
 
     def _start(self):
-        # Resolve the arm we want to place with
-        if self._arm_designator:
-            arm = self._arm_designator.resolve()
-            if arm:
-                self._arm = arm
-
-        # Which item do we want to place? The object in the hand we indicated
-        item_to_place = robot_smach_states.util.designators.Designator(self._arm.occupied_by, resolve_type=Entity)
-
-        # In the case of a task like "Find the coke and place it on the table" grasping is implied, so grasp it here
-        if item_to_place.resolve():
-            self._object_designator = item_to_place
+        # We either got an arm, or we know which arm to place with
+        arm_designator = None
+        if self.semantics.arm:
+            arm_designator = robot_smach_states.util.designators.Designator(self.semantics.arm, resolve_type=Arm)
+        elif self.context.arm_designator:
+            arm_designator = self.context.arm_designator
         else:
-            arm_des = UnoccupiedArmDesignator(self._robot.arms, self._robot.arms['right']).lockable()
-            arm_des.lock()
-            self._grab_state_machine = robot_smach_states.grab.Grab(self._robot,
-                                                                    item=self._object_designator,
-                                                                    arm=arm_des)
-
-            self._grab_state_machine.execute()
-
-            self._arm = arm_des.resolve()
-            if not self._arm:
-                self._execute_result.message += \
-                    ' I was unable to grasp the object because I believe that my grippers were occupied. '
-                self._execute_result.succeeded = False
-                return
-
-            if not self._object_designator.resolve():
-                self._execute_result.message += ' I was unable to grasp the object because I lost it while grasping. '
-                self._execute_result.succeeded = False
-                return
-
-        # No need for ArmHoldingEntityDesignator, we already know that from the config
-        arm_with_item_designator = robot_smach_states.util.designators.Designator(self._arm, resolve_type=Arm)
+            arms = [arm for arm_name, arm in self._robot.arms.iteritems() if
+                    arm.occupied_by == self.semantics.object.type]
+            if arms:
+                arm_designator = robot_smach_states.util.designators.Designator(arms[0], resolve_type=Arm)
+
+        if not arm_designator:
+            self._execute_result.message = " I was unable to resolve which arm to place with. "
+            self._execute_result.succeeded = False
+            return
+
+        item_to_place = robot_smach_states.util.designators.Designator(arm_designator.resolve().occupied_by,
+                                                                       resolve_type=Entity)
+
         place_position = robot_smach_states.util.designators.EmptySpotDesignator(
             self._robot,
-            robot_smach_states.util.designators.EdEntityDesignator(self._robot, id=self._place_entity.id)
+            robot_smach_states.util.designators.EdEntityDesignator(self._robot, id=self.context.location.id)
         )
 
-        self._place = PlaceSmachState(self._robot, self._object_designator, place_position, arm_with_item_designator)
+        self._place = PlaceSmachState(self._robot, item_to_place, place_position,
+                                      arm_designator)
 
-        self._thread = threading.Thread(name='grab', target=self._place.execute)
-        self._thread.start()
+        self._place.execute()
 
-        self._thread.join()
-        self._execute_result.message = " I placed the {} on the {}. ".format(self._object_designator.resolve().type,
-                                                                             self._place_entity.id)
+        self._execute_result.message = " I placed successfully. "
         self._execute_result.succeeded = True
 
     def _cancel(self):
         if self._place.is_running:
             self._place.request_preempt()
 
-        # Wait until canceled
-        self._thread.join()
 
 if __name__ == "__main__":
+
     rospy.init_node('place_test')
 
     import sys

From b6adc5f9b35f783a3a3bcd4cfe5df33f3f6bb2a0 Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Tue, 3 Apr 2018 20:34:14 +0200
Subject: [PATCH 08/46] wip(bring): some work on actions requiring prior
 actions

---
 .../actions/entity_description.py             |   2 +
 .../src/action_server/actions/hand_over.py    |   2 +-
 .../src/action_server/actions/navigate_to.py  | 141 +++++++++---------
 3 files changed, 72 insertions(+), 73 deletions(-)

diff --git a/action_server/src/action_server/actions/entity_description.py b/action_server/src/action_server/actions/entity_description.py
index 84a6fc6e..be93def4 100644
--- a/action_server/src/action_server/actions/entity_description.py
+++ b/action_server/src/action_server/actions/entity_description.py
@@ -35,5 +35,7 @@ def resolve_entity_description(parameters):
             description.type = parameters["type"]
         if "loc" in parameters:
             description.location = resolve_entity_description(parameters["loc"])
+        if "designator" in parameters:
+            description.designator = parameters["designator"]
 
     return description
diff --git a/action_server/src/action_server/actions/hand_over.py b/action_server/src/action_server/actions/hand_over.py
index 1bb9c4c3..227ba1a9 100644
--- a/action_server/src/action_server/actions/hand_over.py
+++ b/action_server/src/action_server/actions/hand_over.py
@@ -95,7 +95,7 @@ def _configure(self, robot, config):
         if not at_destination:
             # Request navigation action
             self._config_result.required_context = {'action': 'navigate-to',
-                                                    'object': config.semantics['target-location']}
+                                                    'target-location': config.semantics['target-location']}
             return
         # We can now assume that we are at the destination for handover!
 
diff --git a/action_server/src/action_server/actions/navigate_to.py b/action_server/src/action_server/actions/navigate_to.py
index 62fab6e5..d4ea1cc4 100644
--- a/action_server/src/action_server/actions/navigate_to.py
+++ b/action_server/src/action_server/actions/navigate_to.py
@@ -1,12 +1,10 @@
 from action import Action, ConfigurationData
-from util import entities_from_description
+from entity_description import resolve_entity_description
 
-import threading
 import rospy
 
-import robot_smach_states
 from robot_smach_states.navigation import NavigateToWaypoint, NavigateToSymbolic
-from robot_smach_states.util.designators import EdEntityDesignator
+from robot_smach_states.util.designators import EdEntityDesignator, EntityByIdDesignator
 
 
 class NavigateTo(Action):
@@ -17,56 +15,67 @@ class NavigateTo(Action):
     '''
     def __init__(self):
         Action.__init__(self)
-        self._required_parameters = {'object' : ' Where would you like me to go? '}
+        self._required_parameters = {'target-location': ' Where would you like me to go? '}
         self._required_skills = ['base']
 
+    class Semantics:
+        def __init__(self):
+            self.target_location = None
+
+    @staticmethod
+    def _parse_semantics(semantics_dict):
+        semantics = NavigateTo.Semantics()
+        semantics.target_location = resolve_entity_description(semantics_dict['target-location'])
+        return semantics
+
+    class Context:
+        def __init__(self):
+            self.target_location = None
+            self.object = None
+
+    @staticmethod
+    def _parse_context(context_dict):
+        context = NavigateTo.Context()
+
+        # Location to navigate to
+        if 'location' in context_dict:
+            context.target_location = context_dict['location']
+
+        # Object to navigate to
+        if 'object-designator' in context_dict:
+            context.object = context_dict['object-designator']
+
+        return context
+
     def _configure(self, robot, config):
         self._robot = robot
-        self._goal_name = ""
-        self._fsm = None
 
         # If we need to navigate to "me", which resolves to "operator", plant a waypoint at the current position to
         # navigate to.
+        # TODO: learn to recognize the operator so that you know you found him later on
         self._robot.ed.update_entity(id="operator", frame_stamped=self._robot.base.get_location(),
                                      type="waypoint")
 
-        entity_description = config.semantics['object']
-
-        (entities, error_msg) = entities_from_description(entity_description, self._robot)
-
-        # Check if we got any entities already...
-        if 'type' in entity_description and entity_description['type'] == "reference" and \
-                'object-designator' in config.knowledge:
-            entity_designator = config.knowledge['object-designator']
-
-            area = "near"
-            self._fsm = NavigateToSymbolic(self._robot,
-                                           entity_designator_area_name_map={entity_designator: area},
-                                           entity_lookat_designator=entity_designator)
-
-        elif not entities:
-            # If not, check if we at least know a type...
-            if 'type' in entity_description:
-                # If we have a type, return succeeded, because that may resolve to an ID later. But remember to check
-                # again before actually executing the task.
-                rospy.loginfo("No knowledge of a {} in the world model yet, but who knows what I will encounter.".
-                              format(entity_description['type']))
-                self._config_result.succeeded = True
-                self._entity_description = entity_description
-            else:
-                # If we don't even have a type, we cannot navigate.
-                rospy.loginfo("No knowledge of a {} in the world model.".format(entity_description['id']))
-                self._config_result.message = " I have no knowledge of a {} in my world model. ".format(
-                    entity_description["id"])
-        else:
-            # Take the best match and set up the state machine
-            e = entities[0]
-            entity_designator = EdEntityDesignator(self._robot, id=e.id)
+        semantics = self._parse_semantics(config.semantics)
+        context = self._parse_context(config.context)
 
-            self._goal_name = e.id
+        know_target = (semantics.target_location.id or
+                       (semantics.target_location.type == 'reference' and context.object))
+
+        if not know_target:
+            # Request find action
+            self._config_result.required_context = {'action': 'find',
+                                                    'object': config.semantics['object'],
+                                                    'source-location': config.semantics['source-location']}
+            return
+        # Now we can assume we know the navigation goal entity!
+
+        if semantics.target_location.id:
+            entity_designator = EntityByIdDesignator(self._robot, id=semantics.target_location.id)
+            e = entity_designator.resolve()
 
             if e.is_a("waypoint"):
-                self._fsm = NavigateToWaypoint(self._robot,
+                self._navigation_state_machine = NavigateToWaypoint(self._robot,
                                                waypoint_designator=entity_designator,
                                                radius=0.1)
                 rospy.loginfo("Navigation set up for a waypoint")
@@ -80,52 +89,40 @@ def _configure(self, robot, config):
                 else:
                     area = "near"
 
-                self._fsm = NavigateToSymbolic(self._robot,
-                                               entity_designator_area_name_map={entity_designator: area},
-                                               entity_lookat_designator=entity_designator)
+                self._navigation_state_machine = NavigateToSymbolic(self._robot,
+                                                                    entity_designator_area_name_map={
+                                                                        entity_designator: area},
+                                                                    entity_lookat_designator=entity_designator)
+        else:
+            entity_designator = context.object
+            self._navigation_state_machine = NavigateToSymbolic(self._robot,
+                                                                entity_designator_area_name_map={
+                                                                    entity_designator: "near"},
+                                                                entity_lookat_designator=entity_designator)
 
         self._config_result.context['location-designator'] = entity_designator
-        self._config_result.context['location'] = config.semantics['object']
+        self._config_result.context['location'] = config.semantics['target-location']
         self._config_result.succeeded = True
         return
 
-
     def _start(self):
-        # Check if we already set up a state machine (may not have happened if we could not get an ID earlier)
-        if not self._fsm:
-            (entities, error_msg) = entities_from_description(self._entity_description, self._robot)
-            if not entities:
-                self._execute_result.message = " I don't have knowledge of a {}.".\
-                    format(self._entity_description['type'])
-                return
-
-            e = entities[0]
-
-            self._goal_name = e.type
-
-            entity_designator = EdEntityDesignator(robot=self._robot, id=e.id)
-            self._fsm = NavigateToSymbolic(self._robot,
-                                           entity_designator_area_name_map={entity_designator: "near"},
-                                           entity_lookat_designator=entity_designator)
-
-        result = self._fsm.execute()
+        result = self._navigation_state_machine.execute()
 
         if result == 'arrived':
             self._execute_result.succeeded = True
-            self._execute_result.message = " I successfully navigated to the {}.".format(self._goal_name)
-            self._robot.speech.speak("I arrived at the {}".format(self._goal_name))
+            # self._execute_result.message = " I successfully navigated to the {}.".format(self._goal_name)
+            # self._robot.speech.speak("I arrived at the {}".format(self._goal_name))
         elif result == 'unreachable':
-            self._execute_result.message = " I was unable to get to the {} because my path was blocked. ".\
-                format(self._goal_name)
+            # self._execute_result.message = " I was unable to get to the {} because my path was blocked. ".\
+                # format(self._goal_name)
             self._robot.speech.speak("Oops, it seems that I can't get there right now.")
         else:
             self._execute_result.message = " I don't know why, but I couldn't find the place I should go. "
             self._robot.speech.speak("I don't know why, but I couldn't find the place I should go.")
 
-
     def _cancel(self):
-        if self._fsm.is_running:
-            self._fsm.request_preempt()
+        if self._navigation_state_machine.is_running:
+            self._navigation_state_machine.request_preempt()
 
 
 if __name__ == "__main__":
@@ -145,7 +142,7 @@ def _cancel(self):
     action = NavigateTo()
 
     config = ConfigurationData({'action': 'navigate-to',
-              'object': {'id': 'cabinet'}})
+                                'target-location': {'id': 'cabinet'}})
 
     action.configure(robot, config)
     action.start()

From 2f373b33828b2bc80c8b6ebd38fc50daa385a4da Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Thu, 26 Apr 2018 12:52:08 +0200
Subject: [PATCH 09/46] fix(answer_question): working using spr knowledge and
 functionality

---
 .../action_server/actions/answer_question.py  | 25 +++++++++++--------
 1 file changed, 15 insertions(+), 10 deletions(-)

diff --git a/action_server/src/action_server/actions/answer_question.py b/action_server/src/action_server/actions/answer_question.py
index dbb9929e..0831063c 100644
--- a/action_server/src/action_server/actions/answer_question.py
+++ b/action_server/src/action_server/actions/answer_question.py
@@ -3,6 +3,7 @@
 
 import rospy
 from robocup_knowledge import load_knowledge
+import challenge_spr
 import hmi
 
 
@@ -21,7 +22,7 @@ def __init__(self):
     def _configure(self, robot, config):
         self._robot = robot
 
-        self._speech_data = load_knowledge('challenge_gpsr')
+        self._speech_data = load_knowledge('challenge_spr')
         if not self._speech_data:
             rospy.logerr("Failed to load speech data for 'AnswerQuestion' action")
             return
@@ -60,8 +61,8 @@ def _start(self):
         while tries < 3:
             try:
                 res = self._robot.hmi.query(description="",
-                                            grammar=self._speech_data.question_grammar,
-                                            target=self._speech_data.question_grammar_target)
+                                            grammar=self._speech_data.grammar,
+                                            target=self._speech_data.grammar_target)
             except hmi.TimeoutException:
                 self._robot.speech.speak("My ears are not working properly, sorry!")
                 self._execute_result.message = " I was unable to hear anything when listening for a question, so I " \
@@ -69,14 +70,18 @@ def _start(self):
                 tries += 1
                 continue
 
-            print res.semantics
-
             if res.semantics and "actions" in res.semantics:
-                rospy.loginfo("Question was: '%s'?" % res.sentence)
-                self._robot.speech.speak("The answer is %s" % res.semantics['actions'][0]['solution'])
-                self._execute_result.message = " I answered the question {}".format(res.sentence)
-                self._execute_result.succeeded = True
-                return
+                try:
+                    # TODO: remove this from challenge states, because apparently, this is more generic.
+                    challenge_spr.riddle_game.answer(self._robot, res, None)
+                    self._execute_result.message = " I answered the question {}".format(res.sentence)
+                    self._execute_result.succeeded = True
+                    return
+                except ValueError:
+                    self._robot.speech.speak("I don't know these people you are talking about. ")
+                    self._execute_result.message = " I couldn't answer the question about the crowd."
+                    tries += 1
+                    continue
             else:
                 if tries < 2:
                     self._robot.speech.speak("Sorry, I did not understand your question, try another one.")

From cd2a6da3953aca0b8bdec253b1848c2848510787 Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Thu, 26 Apr 2018 13:16:56 +0200
Subject: [PATCH 10/46] feat(say): different answers for amigo and sergio

---
 .../src/action_server/actions/say.py          | 19 ++++++++++++++-----
 1 file changed, 14 insertions(+), 5 deletions(-)

diff --git a/action_server/src/action_server/actions/say.py b/action_server/src/action_server/actions/say.py
index c2b8eda5..29703cc6 100644
--- a/action_server/src/action_server/actions/say.py
+++ b/action_server/src/action_server/actions/say.py
@@ -93,11 +93,20 @@ def _start(self):
             ])
             self._execute_result.message = " I told a joke. "
         elif self._sentence == 'SOMETHING_ABOUT_SELF':
-            line = random.choice([
-                "I once dragged a person across the floor for meters.",
-                "I've been to tournaments in seven countries already, and still counting. However, I feel like I'm getting a little too old for this shit.",
-                "I once tripped over the border of a Middle Size League soccer field at full speed."
-            ])
+            if self._robot.robot_name == 'amigo':
+                line = random.choice([
+                    "I once dragged a person across the floor for meters.",
+                    "I've been to tournaments in seven countries already, and still counting.",
+                    "I once tripped over the border of a Middle Size League soccer field at full speed."
+                ])
+            elif self._robot.robot_name == 'sergio':
+                line = random.choice([
+                    "I am very proud of my older brother Amigo. He has been competing in Robocup for a long time.",
+                    "I have ankles, knees and hips to reach to the floor as well as the ceiling. "
+                    "Although that would be hard here. ",
+                    "Sometimes I feel a little tense. But then I touch something metal and it helps me discharge and "
+                    "relax."
+                ])
             self._execute_result.message = " I told something about myself. "
         else:
             line = self._sentence

From 076cffb50e9abd9792455f850a29b2b6efd7d22e Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Thu, 26 Apr 2018 22:21:33 +0200
Subject: [PATCH 11/46] refactor(bring): remove unused action bring

---
 .../src/action_server/actions/__init__.py     |   1 -
 .../src/action_server/actions/bring.py        | 255 ------------------
 2 files changed, 256 deletions(-)
 delete mode 100644 action_server/src/action_server/actions/bring.py

diff --git a/action_server/src/action_server/actions/__init__.py b/action_server/src/action_server/actions/__init__.py
index 5a4c10e8..700cd86d 100644
--- a/action_server/src/action_server/actions/__init__.py
+++ b/action_server/src/action_server/actions/__init__.py
@@ -2,7 +2,6 @@
 
 from answer_question import AnswerQuestion
 from arm_goal import ArmGoal
-from bring import Bring
 from gripper_goal import GripperGoal
 from inspect import Inspect
 from look_at import LookAt
diff --git a/action_server/src/action_server/actions/bring.py b/action_server/src/action_server/actions/bring.py
deleted file mode 100644
index acf43b26..00000000
--- a/action_server/src/action_server/actions/bring.py
+++ /dev/null
@@ -1,255 +0,0 @@
-from action import Action, ConfigurationData
-from find import Find
-from pick_up import PickUp
-from navigate_to import NavigateTo
-from place import Place
-from entity_description import resolve_entity_description
-
-import rospy
-
-class Bring(Action):
-    """
-    The Bring class implements the action to bring an object from a source to a target location.
-
-    Parameters to pass to the configure() method are 'source-location' (required), 'target-location' (required) and
-     an object to bring (required).
-    """
-    def __init__(self):
-        Action.__init__(self)
-        self._required_field_prompts = {'target-location': " Where would you like me to take it? ",
-                                        'object' : " What would you like me to bring, exactly? "}
-        self._required_skills = ['base']
-
-    def _configure(self, robot, config):
-        self._robot = robot
-        self._source_location = None
-        self._object = resolve_entity_description(config.semantics['object'])
-        self._find_action = None
-        self._grab_action = None
-        self._nav_action = None
-        self._find_person_action = None
-
-        if 'arm-designator' in config.knowledge:
-            self._arm_designator = config.knowledge['arm-designator']
-            if 'object-designator' in config.knowledge:
-                self._found_object_designator = config.knowledge['object-designator']
-            else:
-                rospy.logfatal("No object designator while there is an arm designator. How can this be happening?!")
-                self._config_result.message = "No object designator while there is an arm designator. How can this be happening?!"
-                return
-        else:
-            # If we got a source location in the semantics, use that to find the object
-            if 'source-location' in config.semantics or 'location-designator' in config.knowledge:
-                self._find_action = Find()
-
-                # Put the knowledge passed to the bring action to the find action.
-                find_semantics = {}
-                if 'source-location' in config.semantics:
-                    find_semantics['location'] = config.semantics['source-location']
-                find_semantics['object'] = config.semantics['object']
-
-                find_config = ConfigurationData(find_semantics, config.knowledge)
-                find_config_result = self._find_action.configure(self._robot, find_config)
-                if not find_config_result.succeeded:
-                    self._config_result = find_config_result
-                    return
-
-                # Use the object designator from the find action to resolve to the object we want to bring
-                self._found_object_designator = find_config_result.context['object-designator']
-                self._source_location = resolve_entity_description(config.semantics['source-location'])
-
-            # If the task is something like "... and bring it to me", the "it" refers to something we already found or even
-            # grasped the past, meaning we don't need to do a find action, but we need to use that object.
-            elif config.semantics['object']['type'] == 'reference' and 'object-designator' in config.knowledge:
-                rospy.loginfo("object is a reference, but I have an object designator")
-                self._found_object_designator = config.knowledge['object-designator']
-            else:
-                rospy.loginfo("No source given, trying to find at default location.")
-                self._find_action = Find()
-
-                # Put the knowledge passed to the bring action to the find action.
-                find_semantics = {}
-                if config.semantics['object']['type'] in self._knowledge.object_categories:
-                    category = config.semantics['object']['type']
-                else:
-                    category = self._knowledge.get_object_category(config.semantics['object']['type'])
-
-                expected_object_location = self._knowledge.get_object_category_location(category)[0]
-
-                find_semantics['location'] = expected_object_location
-                find_semantics['object'] = config.semantics['object']
-
-                find_config = ConfigurationData(find_semantics)
-                find_config_result = self._find_action.configure(self._robot, find_config)
-                if not find_config_result.succeeded:
-                    self._config_result = find_config_result
-                    return
-
-                # Use the object designator from the find action to resolve to the object we want to bring
-                self._found_object_designator = find_config_result.context['object-designator']
-
-                # rospy.loginfo("I really don't know where to get this thing. ")
-                # self._config_result.message = " Where would you like me to get it? "
-                # self._config_result.missing_field = 'source-location'
-                # self._config_result.succeeded = False
-                # return
-
-            self._grab_action = PickUp()
-            grab_config = ConfigurationData({'object': config.semantics['object']},
-                                            {'object-designator': self._found_object_designator})
-            grab_config_result = self._grab_action.configure(self._robot, grab_config)
-            if not grab_config_result.succeeded:
-                self._config_result = grab_config_result
-                return
-            self._arm_designator = grab_config_result.context['arm-designator']
-
-
-        self._target_location = resolve_entity_description(config.semantics['target-location'])
-        if self._target_location.type == "person":
-            print self._target_location.type
-            print self._target_location.id
-            print self._target_location.location
-            if self._target_location.id and self._target_location.id == "operator":
-                self._robot.ed.update_entity(id="operator", frame_stamped=self._robot.base.get_location(),
-                                             type="waypoint")
-                self._nav_action = NavigateTo()
-                nav_config = ConfigurationData({'object': config.semantics['target-location']})
-                nav_config_result = self._nav_action.configure(self._robot, nav_config)
-                if not nav_config_result.succeeded:
-                    self._config_result = nav_config_result
-                    return
-
-            # If we need to bring to someone else than the operator, we need to go and find that person
-            elif self._target_location.id and self._target_location.location:
-                self._find_person_action = Find()
-
-                find_person_semantics = {}
-                find_person_semantics['location'] = {'id' : self._target_location.location.id}
-                find_person_semantics['object'] = {'id' : self._target_location.id,
-                                                   'type' : self._target_location.type}
-
-                find_person_config = ConfigurationData(find_person_semantics)
-                find_person_config_result = self._find_person_action.configure(self._robot, find_person_config)
-                if not find_person_config_result.succeeded:
-                    self._config_result = find_person_config_result
-                    return
-            elif self._target_location.id:
-                self._config_result.message = \
-                    "Please give me the assignment again and then also tell me were to find {}".\
-                        format(self._target_location.id)
-                return
-        else:
-            self._place_action = Place()
-            place_config = ConfigurationData({'location': config.semantics['target-location']},
-                                             {'arm-designator': self._arm_designator,
-                                              'object-designator': self._found_object_designator})
-            place_config_result = self._place_action.configure(self._robot, place_config)
-            if not place_config_result.succeeded:
-                self._config_result = place_config_result
-                return
-
-        self._config_result.succeeded = True
-
-    def _handover(self):
-        self._robot.speech.speak("I will hand over the {} now".format(self._object.type))
-        arm = self._arm_designator.resolve()
-        arm.send_joint_goal('handover_to_human')
-        arm.wait_for_motion_done()
-
-        self._robot.speech.speak("Please take it from my gripper.", block=False)
-
-        attempt = 0
-
-        while not arm.handover_to_human(timeout=10) and attempt < 2:
-            self._robot.speech.speak("Please take it from my gripper.", block=False)
-            attempt += 1
-
-        self._robot.speech.speak("I will open my gripper now.", block=False)
-
-        self._robot.ed.update_entity(id=arm.occupied_by.id, action='remove')
-        arm.send_gripper_goal('open')
-        arm.wait_for_motion_done()
-
-        arm.reset()
-        arm.wait_for_motion_done()
-
-        arm.occupied_by = None
-
-    def _start(self):
-        self._robot.speech.speak("Bring the action!")
-
-        # Find
-        if self._find_action:
-            find_result = self._find_action.start()
-
-            if not find_result.succeeded:
-                if self._object.type == "reference":
-                    self._object.type = self._found_object_designator.resolve().type
-                self._execute_result.message = " I was unable to find the {}. ".format(self._object.type)
-                return
-
-        if self._object.type == "reference":
-            self._object.type = self._found_object_designator.resolve().type
-
-        # Grab
-        if self._grab_action:
-            grab_result = self._grab_action.start()
-
-            if not grab_result.succeeded:
-                self._execute_result.message = " I was unable to grab the {}. ".format(self._object.type)
-                return
-
-        # Navigate
-        if self._nav_action:
-            nav_result = self._nav_action.start()
-
-            if not nav_result.succeeded:
-                self._execute_result.message = " I was unable to go to the {}. ".format(self._target_location.id)
-        elif self._find_person_action:
-            find_person_result = self._find_person_action.start()
-
-            if not find_person_result.succeeded:
-                self._execute_result.message = " I found {}. ".format(self._target_location.id)
-
-        # Handover or place
-        if self._target_location.type == "person":
-            self._handover()
-        else:
-            place_result = self._place_action.start()
-
-            if not place_result.succeeded:
-                self._execute_result.message = " I was unable to place the {}".format(self._object.type)
-                return
-
-        self._execute_result.succeeded = True
-        if self._source_location:
-            self._execute_result.message += " I brought a {} from {} to {}. ".format(self._object.type,
-                                                                                     self._source_location.id,
-                                                                                     self._target_location.id)
-        else:
-            self._execute_result.message += " I brought a {} to {}. ".format(self._object.type,
-                                                                             self._target_location.id)
-
-if __name__ == "__main__":
-    rospy.init_node('bring_test')
-
-    import sys
-    robot_name = sys.argv[1]
-    if robot_name == 'amigo':
-        from robot_skills.amigo import Amigo as Robot
-    elif robot_name == 'sergio':
-        from robot_skills.sergio import Sergio as Robot
-    else:
-        from robot_skills.mockbot import Mockbot as Robot
-
-    robot = Robot()
-
-    action = Bring()
-
-    config = ConfigurationData({'action': 'bring',
-              'entity': {'location': 'cabinet'},
-              'from': {'id': 'cabinet'},
-              'to': {'id': 'dinner_table'}})
-
-    action.configure(robot, config)
-    action.start()

From 85a8d3328c591c415608a73349f6f894685eb23a Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Thu, 26 Apr 2018 22:22:23 +0200
Subject: [PATCH 12/46] fix(hand_over): using bring the action from soundboard

---
 action_server/src/action_server/actions/hand_over.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/action_server/src/action_server/actions/hand_over.py b/action_server/src/action_server/actions/hand_over.py
index 227ba1a9..32c82007 100644
--- a/action_server/src/action_server/actions/hand_over.py
+++ b/action_server/src/action_server/actions/hand_over.py
@@ -128,6 +128,7 @@ def _handover(self):
         arm.occupied_by = None
 
     def _start(self):
+        self._robot.speech.speak("Bring the action!")
         # Handover
         self._handover()
 

From 9be4f316e5d99f44981a7b36713d468f1c04e1b0 Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Thu, 26 Apr 2018 22:22:51 +0200
Subject: [PATCH 13/46] refactor(place): use new action structure with
 preconditions

---
 .../src/action_server/actions/place.py        | 43 +++++++++++++------
 1 file changed, 30 insertions(+), 13 deletions(-)

diff --git a/action_server/src/action_server/actions/place.py b/action_server/src/action_server/actions/place.py
index 4a8483ce..68cfed90 100644
--- a/action_server/src/action_server/actions/place.py
+++ b/action_server/src/action_server/actions/place.py
@@ -1,15 +1,13 @@
 from action import Action, ConfigurationData
-from util import entities_from_description
 from entity_description import resolve_entity_description
 
 from robot_skills.arms import Arm
 import robot_smach_states
 from robot_smach_states.manipulation import Place as PlaceSmachState
 from robot_skills.util.entity import Entity
-from robot_smach_states.util.designators import UnoccupiedArmDesignator, EdEntityDesignator
+from robot_smach_states.util.designators import ArmDesignator, EdEntityDesignator
 
 import rospy
-import threading
 
 
 class Place(Action):
@@ -89,17 +87,30 @@ def _configure(self, robot, config):
         # Express the initial conditions in rules based on input
         # We assume we already got the object if previous action passed an arm, an object and this object has the
         # required type, or the required type is a reference.
-        got_object = (
+        got_object_in_task = (
             self.context.arm_designator is not None and (self.context.object_type == self.semantics.object.type or
                                                          self.semantics.object.type == 'reference'))
-        got_object = got_object or [arm_name for arm_name, arm in self._robot.arms.iteritems() if arm.occupied_by]
+
+        object_in_gripper = False
+        if not got_object_in_task:
+            object_to_arm_dict = {arm.occupied_by.type: arm for arm_name, arm in self._robot.arms.iteritems() if
+                                  arm.occupied_by and arm.occupied_by.is_a(self.semantics.object.type)}
+
+            if bool(object_to_arm_dict):
+                object_in_gripper = True
+                arm = object_to_arm_dict[self.semantics.object.type]
+                self.context.arm_designator = \
+                    ArmDesignator({arm.side: arm})
+
+        got_object = got_object_in_task or object_in_gripper
 
         # If precondition not met, request prior action from the task manager
         if not got_object:
             # Request pick_up action
             self._config_result.required_context = {'action': 'pick-up',
-                                                    'object': config.semantics['object'],
-                                                    'source-location': config.semantics['source-location']}
+                                                    'object': config.semantics['object']}
+            if 'source-location' in config.semantics:
+                self._config_result.required_context['source-location'] = config.semantics['source-location']
             return
         # Now we can assume we picked up the item!
 
@@ -111,7 +122,7 @@ def _configure(self, robot, config):
         if not at_destination:
             # Request navigation action
             self._config_result.required_context = {'action': 'navigate-to',
-                                                    'object': config.semantics['target-location']}
+                                                    'target-location': config.semantics['target-location']}
             return
         # We can now assume that we are at the destination for handover!
 
@@ -119,6 +130,8 @@ def _configure(self, robot, config):
         return
 
     def _start(self):
+        self._robot.speech.speak("Bring the action!")
+
         # We either got an arm, or we know which arm to place with
         arm_designator = None
         if self.semantics.arm:
@@ -141,16 +154,20 @@ def _start(self):
 
         place_position = robot_smach_states.util.designators.EmptySpotDesignator(
             self._robot,
-            robot_smach_states.util.designators.EdEntityDesignator(self._robot, id=self.context.location.id)
+            robot_smach_states.util.designators.EdEntityDesignator(self._robot, id=self.context.location.id),
+            area="on_top_of"
         )
 
         self._place = PlaceSmachState(self._robot, item_to_place, place_position,
                                       arm_designator)
 
-        self._place.execute()
-
-        self._execute_result.message = " I placed successfully. "
-        self._execute_result.succeeded = True
+        state_machine_result = self._place.execute()
+        if state_machine_result == 'done':
+            self._execute_result.message = " I placed successfully. "
+            self._execute_result.succeeded = True
+        else:
+            self._execute_result.message = " I failed to place. "
+            self._execute_result.succeeded = True
 
     def _cancel(self):
         if self._place.is_running:

From bdf2f35463ebf455e954964b5b85784ee8171ff4 Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Thu, 26 Apr 2018 22:43:19 +0200
Subject: [PATCH 14/46] fix(follow): rename camelcase to underscored

---
 .../src/action_server/actions/follow.py       | 24 ++++++-------------
 1 file changed, 7 insertions(+), 17 deletions(-)

diff --git a/action_server/src/action_server/actions/follow.py b/action_server/src/action_server/actions/follow.py
index 5beb7c19..37793fb9 100644
--- a/action_server/src/action_server/actions/follow.py
+++ b/action_server/src/action_server/actions/follow.py
@@ -28,6 +28,7 @@ def navigate(robot, entity_description):
     )
     return navigation_sm.execute() == "succeeded"
 
+
 class Follow(Action):
     ''' The Follow class implements the action to follow a person.
 
@@ -54,17 +55,13 @@ def _configure(self, robot, config):
                 self._config_result.message = " Where can I find {}? ".format(self._target.id)
                 return
 
-            if "location-from" in config.semantics:
+            if "location-from" in config.semantics and "location-designator" not in config.context:
                 self._origin = resolve_entity_description(config.semantics["location-from"])
-                self._find_action = Find()
-                find_config = ConfigurationData({'location': {'id' : self._origin.id},
-                               'object': {'type': 'person',
-                                          'id': self._target.id}})
-                find_config_result = self._find_action.configure(robot, find_config)
-                if not find_config_result.succeeded:
-                    self._config_result.message = " I don't know how to find {} in the {}. ".format(self._origin.id,
-                                                                                                    self._target.id)
-                    return
+                self._config_result.required_context = {'action': 'find',
+                                                        'source-location': {'id': self._origin.id},
+                                                        'object': {'type': 'person',
+                                                                   'id': self._target.id}}
+                return
 
         if self._target.id == "operator":
             self._target.id = "you"
@@ -81,13 +78,6 @@ def _configure(self, robot, config):
         self._config_result.succeeded = True
 
     def _start(self):
-        if self._find_action:
-            find_result = self._find_action.start()
-            self._execute_result.message += find_result.message
-            if not find_result.succeeded:
-                return
-            # TODO: Navigate to the found person before following
-
         # Do some awesome following
         res = self._follow_sm.execute()
 

From 5ecad561911062ab6154cbbd388e323acdf1ad67 Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Thu, 26 Apr 2018 22:43:30 +0200
Subject: [PATCH 15/46] fix(follow): rename camelcase to underscored

---
 action_server/src/action_server/actions/find.py | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/action_server/src/action_server/actions/find.py b/action_server/src/action_server/actions/find.py
index 0ed7a96f..8f9910e5 100644
--- a/action_server/src/action_server/actions/find.py
+++ b/action_server/src/action_server/actions/find.py
@@ -29,7 +29,7 @@ def __init__(self):
     def _point_at_person(self, person):
         pose_base_link_kdl = person.pose.projectToFrame(self._robot.robot_name + '/base_link',
                                                         self._robot.tf_listener)
-        pose_base_link = kdl.kdlFrameStampedToPoseStampedMsg(pose_base_link_kdl)
+        pose_base_link = kdl.kdl_frame_stamped_to_pose_stamped_msg(pose_base_link_kdl)
 
         x = pose_base_link.pose.position.x
         y = pose_base_link.pose.position.y
@@ -124,6 +124,7 @@ def _configure(self, robot, config):
                                                              found_entity_designator=self._found_entity_designator))
 
         self._config_result.context['object-designator'] = self._found_entity_designator
+        self._config_result.context['location-designator'] = location_designator
         self._config_result.succeeded = True
 
     def _start(self):

From 528853df62a5cca606f7846a69ce70ceeb015030 Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Fri, 27 Apr 2018 08:11:01 +0200
Subject: [PATCH 16/46] fix(hand_over): source location optional

---
 action_server/src/action_server/actions/hand_over.py | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/action_server/src/action_server/actions/hand_over.py b/action_server/src/action_server/actions/hand_over.py
index 32c82007..8bdfc56f 100644
--- a/action_server/src/action_server/actions/hand_over.py
+++ b/action_server/src/action_server/actions/hand_over.py
@@ -82,8 +82,9 @@ def _configure(self, robot, config):
         if not got_object:
             # Request pick_up action
             self._config_result.required_context = {'action': 'pick-up',
-                                                    'object': config.semantics['object'],
-                                                    'source-location': config.semantics['source-location']}
+                                                    'object': config.semantics['object']}
+            if 'source-location' in config.semantics:
+                self._config_result.required_context['source-location'] = config.semantics['source-location']
             return
         # Now we can assume we picked up the item!
 

From 2ebb4544874e40cd215e82039bc25864e7e95ce1 Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Fri, 27 Apr 2018 09:27:38 +0200
Subject: [PATCH 17/46] fix(navigate_to): <person> at the <location>

---
 .../src/action_server/actions/navigate_to.py     | 16 +++++++++++-----
 1 file changed, 11 insertions(+), 5 deletions(-)

diff --git a/action_server/src/action_server/actions/navigate_to.py b/action_server/src/action_server/actions/navigate_to.py
index d4ea1cc4..f4b3f2d1 100644
--- a/action_server/src/action_server/actions/navigate_to.py
+++ b/action_server/src/action_server/actions/navigate_to.py
@@ -59,18 +59,24 @@ def _configure(self, robot, config):
         semantics = self._parse_semantics(config.semantics)
         context = self._parse_context(config.context)
 
-        know_target = (semantics.target_location.id or
-                       (semantics.target_location.type == 'reference' and context.object))
+        know_target = (semantics.target_location.id and semantics.target_location.type != 'person' or
+                       (semantics.target_location.type == 'reference' and context.object) or
+                       (semantics.target_location.type == 'person' and context.object))
 
         if not know_target:
             # Request find action
             self._config_result.required_context = {'action': 'find',
-                                                    'object': config.semantics['object'],
-                                                    'source-location': config.semantics['source-location']}
+                                                    'object': config.semantics['target-location']}
+            if 'type' in config.semantics['target-location'] and \
+                config.semantics['target-location']['type'] == 'person' and \
+                    'location' in config.semantics['target-location']:
+                self._config_result.required_context['source-location'] = {'id': config.semantics['target-location']['location']}
+            elif 'source-location' in config.semantics:
+                self._config_result.required_context['source-location'] = config.semantics['source-location']
             return
         # Now we can assume we know the navigation goal entity!
 
-        if semantics.target_location.id:
+        if semantics.target_location.id and semantics.target_location.type != 'person':
             entity_designator = EntityByIdDesignator(self._robot, id=semantics.target_location.id)
             e = entity_designator.resolve()
 

From fa79aab1fbead458813b20700eb51cf84e3e2fc8 Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Fri, 27 Apr 2018 15:59:55 +0200
Subject: [PATCH 18/46] feat(entity_description): add category

---
 .../src/action_server/actions/entity_description.py          | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/action_server/src/action_server/actions/entity_description.py b/action_server/src/action_server/actions/entity_description.py
index be93def4..721baa28 100644
--- a/action_server/src/action_server/actions/entity_description.py
+++ b/action_server/src/action_server/actions/entity_description.py
@@ -1,10 +1,11 @@
 
 class EntityDescription(object):
-    def __init__(self, id=None, type=None, location=None, area=None):
+    def __init__(self, id=None, type=None, location=None, area=None, category=None):
         self.id = id
         self.type = type
         self.location = location
         self.area = area
+        self.category = category
 
     def __repr__(self):
         return "EntityDescription(id={id},type={type},location={location},area={area})".format(id=self.id,
@@ -33,6 +34,8 @@ def resolve_entity_description(parameters):
             description.id = parameters["id"]
         if "type" in parameters:
             description.type = parameters["type"]
+        if "category" in parameters:
+            description.category = parameters["category"]
         if "loc" in parameters:
             description.location = resolve_entity_description(parameters["loc"])
         if "designator" in parameters:

From c9125983d564904707ee432bbc56eba4992f3626 Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Fri, 27 Apr 2018 16:00:25 +0200
Subject: [PATCH 19/46] fix(say): add your teams country

---
 action_server/src/action_server/actions/say.py | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/action_server/src/action_server/actions/say.py b/action_server/src/action_server/actions/say.py
index 29703cc6..8f6c0e02 100644
--- a/action_server/src/action_server/actions/say.py
+++ b/action_server/src/action_server/actions/say.py
@@ -61,6 +61,9 @@ def _start(self):
         elif self._sentence == "TEAM_NAME":
             line = "My team's name is Tech United Eindhoven"
             self._execute_result.message = " I told my team's name. "
+        elif self._sentence == "COUNTRY":
+            line = "My team is from the Netherlands, also known as Holland."
+            self._execute_result.message = " I told my team's name. "
         elif self._sentence == "TEAM_AFFILIATION":
             line = "My team is affiliated with the University of Technology Eindhoven"
             self._execute_result.message = " I told my team's affiliation. "

From d36ecbf2590739131e6ba3842cf092b7c5999907 Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Fri, 27 Apr 2018 16:02:45 +0200
Subject: [PATCH 20/46] fix(find): dont crash when trying to find an object in
 a category

---
 action_server/src/action_server/actions/find.py | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/action_server/src/action_server/actions/find.py b/action_server/src/action_server/actions/find.py
index 8f9910e5..f44baa45 100644
--- a/action_server/src/action_server/actions/find.py
+++ b/action_server/src/action_server/actions/find.py
@@ -155,7 +155,8 @@ def _start(self):
                     self._robot.speech.speak("I don't see what I am looking for here.")
 
                 self._execute_result.message = " I couldn't find {} {} the {} ".format(
-                    self._object.id if self._object.id and not self._object.id == "None" else "a " + self._object.type,
+                    self._object.id if self._object.id and not self._object.id == "None" else \
+                        "a " + self._object.type if self._object.type else "a " + self._object.category,
                     "in" if self._location.id in self._knowledge.location_rooms else "at",
                     self._location.id
                 )
@@ -163,7 +164,8 @@ def _start(self):
                 self._robot.speech.speak(" I'm unable to inspect the {} ".format(self._location.id))
                 self._execute_result.message = " I was unable to inspect the {} to find {}. ".format(
                     self._location.id,
-                    self._object.id if self._object.id else "a " + self._object.type
+                    self._object.id if self._object.id else "a " + self._object.type if self._object.type else \
+                        "a " + self._object.category
                 )
 
     def _cancel(self):

From c4b56d2691a0cc4937b3ca61c4d91d5bfbd26356 Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Sat, 28 Apr 2018 11:24:27 +0200
Subject: [PATCH 21/46] fix(navigate): return message

---
 action_server/src/action_server/actions/navigate_to.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/action_server/src/action_server/actions/navigate_to.py b/action_server/src/action_server/actions/navigate_to.py
index f4b3f2d1..8d7d79a8 100644
--- a/action_server/src/action_server/actions/navigate_to.py
+++ b/action_server/src/action_server/actions/navigate_to.py
@@ -116,8 +116,8 @@ def _start(self):
 
         if result == 'arrived':
             self._execute_result.succeeded = True
-            # self._execute_result.message = " I successfully navigated to the {}.".format(self._goal_name)
-            # self._robot.speech.speak("I arrived at the {}".format(self._goal_name))
+            self._execute_result.message = " I successfully navigated."
+            self._robot.speech.speak("I arrived!")
         elif result == 'unreachable':
             # self._execute_result.message = " I was unable to get to the {} because my path was blocked. ".\
                 # format(self._goal_name)

From 5ecc5807b20f9ce04c36c164b008ea21e60b1a47 Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Sat, 28 Apr 2018 11:56:55 +0200
Subject: [PATCH 22/46] fix by janno for weh speech fails during open challenge
 follow me to the X

---
 action_server/src/action_server/actions/follow.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/action_server/src/action_server/actions/follow.py b/action_server/src/action_server/actions/follow.py
index 37793fb9..e2acb641 100644
--- a/action_server/src/action_server/actions/follow.py
+++ b/action_server/src/action_server/actions/follow.py
@@ -17,14 +17,14 @@ def navigate(robot, entity_description):
     else:
         origin_area = "near"
 
-    origin_entity_designator = EdEntityDesignator(id=entity_description.id)
+    origin_entity_designator = EdEntityDesignator(robot, id=entity_description.id)
     navigation_sm = NavigateToSymbolic(
         robot=robot,
         entity_designator_area_name_map=
         {
             origin_entity_designator: origin_area
         },
-        lookat_entity_designator=origin_entity_designator
+        entity_lookat_designator=origin_entity_designator
     )
     return navigation_sm.execute() == "succeeded"
 

From d14b0e0e47819f119ad8291ae654ce3a46e2cde6 Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Sat, 28 Apr 2018 12:20:21 +0200
Subject: [PATCH 23/46] fix(follow): target id

---
 action_server/src/action_server/actions/follow.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/action_server/src/action_server/actions/follow.py b/action_server/src/action_server/actions/follow.py
index e2acb641..09f41dd9 100644
--- a/action_server/src/action_server/actions/follow.py
+++ b/action_server/src/action_server/actions/follow.py
@@ -92,7 +92,7 @@ def _start(self):
                 self._execute_result.message += " But I failed to follow {} ".format(self._target.id)
                 return
         self._robot.speech.speak("Thank you for guiding me.")
-        self._execute_result.message += " I successfully followed {} ".format(self._goal.id)
+        self._execute_result.message += " I successfully followed {} ".format(self._target.id)
         if self._goal:
             self._execute_result.message += " to the {}".format(self._goal.id)
 

From 29c44edb9c29b2aebeb42c4ddfb9a9ec33832612 Mon Sep 17 00:00:00 2001
From: Janno Lunenburg <jannolunenburg@gmail.com>
Date: Sat, 28 Apr 2018 15:04:11 +0200
Subject: [PATCH 24/46] feat(Client)Added send_async_task method

---
 action_server/src/action_server/client.py | 28 +++++++++++++++--------
 1 file changed, 19 insertions(+), 9 deletions(-)

diff --git a/action_server/src/action_server/client.py b/action_server/src/action_server/client.py
index 68cb5047..78c0d1ac 100644
--- a/action_server/src/action_server/client.py
+++ b/action_server/src/action_server/client.py
@@ -4,6 +4,7 @@
 import action_server_msgs.msg
 import action_server_msgs.srv
 
+
 class TaskOutcome(object):
     RESULT_MISSING_INFORMATION = 0
     RESULT_TASK_EXECUTION_FAILED = 1
@@ -28,8 +29,8 @@ def succeeded(self, value):
 
     def __repr__(self):
         return "TaskOutcome(result={}, messages={}, missing_field='{}')".format(self.result,
-                                                                             self.messages,
-                                                                             self.missing_field)
+                                                                                self.messages,
+                                                                                self.missing_field)
 
 
 class Client(object):
@@ -52,7 +53,6 @@ def __init__(self, robot_name):
 
         self.get_actions_proxy = rospy.ServiceProxy('get_actions', action_server_msgs.srv.GetActions)
 
-
     def get_actions(self):
         """
         Query the available actions from the action server.
@@ -67,6 +67,18 @@ def get_actions(self):
 
         return res.actions
 
+    def send_async_task(self, semantics, done_cb, feedback_cb):
+        """ Send a task to the action server and return immediately. A task is composed of one or multiple actions.
+
+        :param semantics: A json string with a list of dicts, every dict in the list has at least an 'action' field,
+        and depending on the type of action, several parameter fields may be required.
+        :param done_cb:	(callable) Callback that gets called on transitions to Done. The callback should take two
+        parameters: the terminal state (as an integer from actionlib_msgs/GoalStatus) and the result.
+        :param feedback_cb: (callable)Callback that gets called whenever feedback for this goal is received. Takes one
+        parameter: the feedback.
+        """
+        goal = action_server_msgs.msg.TaskGoal(recipe=semantics)
+        self._action_client.send_goal(goal=goal, done_cb=done_cb, feedback_cb=feedback_cb)
 
     def send_task(self, semantics):
         """
@@ -77,12 +89,7 @@ def send_task(self, semantics):
         and depending on the type of action, several parameter fields may be required.
         :return: True or false, and a message specifying the outcome of the task
         """
-
-        recipe = semantics
-
-        result = None
-
-        goal = action_server_msgs.msg.TaskGoal(recipe=recipe)
+        goal = action_server_msgs.msg.TaskGoal(recipe=semantics)
         self._action_client.send_goal(goal)
 
         try:
@@ -94,6 +101,7 @@ def send_task(self, semantics):
             self.cancel_all()
             raise KeyboardInterrupt
 
+        # Check result to return the correct outcome
         if result.result == action_server_msgs.msg.TaskResult.RESULT_MISSING_INFORMATION:
 
             to = TaskOutcome(TaskOutcome.RESULT_MISSING_INFORMATION,
@@ -116,6 +124,8 @@ def send_task(self, semantics):
         return TaskOutcome(messages=result.log_messages)
 
     def cancel_all(self):
+        """ Cancels all goals of the action client
+        """
         rospy.logdebug("cancelling all goals...")
         self._action_client.cancel_all_goals()
         self._action_client.wait_for_result()

From 1ef2dbf59910f97bd6892cea28b091b4f10d6403 Mon Sep 17 00:00:00 2001
From: Janno Lunenburg <jannolunenburg@gmail.com>
Date: Sat, 28 Apr 2018 15:17:39 +0200
Subject: [PATCH 25/46] feat(Client)Async done callback is wrapped to use
 TaskOutcome

---
 action_server/src/action_server/client.py | 60 ++++++++++++++---------
 1 file changed, 36 insertions(+), 24 deletions(-)

diff --git a/action_server/src/action_server/client.py b/action_server/src/action_server/client.py
index 78c0d1ac..72443b87 100644
--- a/action_server/src/action_server/client.py
+++ b/action_server/src/action_server/client.py
@@ -33,6 +33,35 @@ def __repr__(self):
                                                                                 self.missing_field)
 
 
+def task_outcome_from_result(result):
+    """ Converts action_server_msgs.msg.TaskResult to TaskOutcome class
+
+    :param result: (action_server_msgs.msg.TaskResult) result input
+    :return: (TaskOutcome) result output
+    """
+    # Check result to return the correct outcome
+    if result.result == action_server_msgs.msg.TaskResult.RESULT_MISSING_INFORMATION:
+
+        to = TaskOutcome(TaskOutcome.RESULT_MISSING_INFORMATION,
+                         result.log_messages)
+        to.missing_field = result.missing_field
+        return to
+
+    elif result.result == action_server_msgs.msg.TaskResult.RESULT_TASK_EXECUTION_FAILED:
+        return TaskOutcome(TaskOutcome.RESULT_TASK_EXECUTION_FAILED,
+                           result.log_messages)
+
+    elif result.result == action_server_msgs.msg.TaskResult.RESULT_UNKNOWN:
+        return TaskOutcome(TaskOutcome.RESULT_UNKNOWN,
+                           result.log_messages)
+
+    elif result.result == action_server_msgs.msg.TaskResult.RESULT_SUCCEEDED:
+        return TaskOutcome(TaskOutcome.RESULT_SUCCEEDED,
+                           result.log_messages)
+
+    return TaskOutcome(messages=result.log_messages)
+
+
 class Client(object):
     """ A client for the action server
 
@@ -72,13 +101,16 @@ def send_async_task(self, semantics, done_cb, feedback_cb):
 
         :param semantics: A json string with a list of dicts, every dict in the list has at least an 'action' field,
         and depending on the type of action, several parameter fields may be required.
-        :param done_cb:	(callable) Callback that gets called on transitions to Done. The callback should take two
-        parameters: the terminal state (as an integer from actionlib_msgs/GoalStatus) and the result.
+        :param done_cb:	(callable) Callback that gets called on transitions to Done. The callback should take one
+        parameter: TaskOutCome
         :param feedback_cb: (callable)Callback that gets called whenever feedback for this goal is received. Takes one
         parameter: the feedback.
         """
+        def _wrapped_done_cb(_, result):
+            taskoutcome = task_outcome_from_result(result=result)
+            return done_cb(taskoutcome)
         goal = action_server_msgs.msg.TaskGoal(recipe=semantics)
-        self._action_client.send_goal(goal=goal, done_cb=done_cb, feedback_cb=feedback_cb)
+        self._action_client.send_goal(goal=goal, done_cb=_wrapped_done_cb, feedback_cb=feedback_cb)
 
     def send_task(self, semantics):
         """
@@ -101,27 +133,7 @@ def send_task(self, semantics):
             self.cancel_all()
             raise KeyboardInterrupt
 
-        # Check result to return the correct outcome
-        if result.result == action_server_msgs.msg.TaskResult.RESULT_MISSING_INFORMATION:
-
-            to = TaskOutcome(TaskOutcome.RESULT_MISSING_INFORMATION,
-                             result.log_messages)
-            to.missing_field = result.missing_field
-            return to
-
-        elif result.result == action_server_msgs.msg.TaskResult.RESULT_TASK_EXECUTION_FAILED:
-            return TaskOutcome(TaskOutcome.RESULT_TASK_EXECUTION_FAILED,
-                               result.log_messages)
-
-        elif result.result == action_server_msgs.msg.TaskResult.RESULT_UNKNOWN:
-            return TaskOutcome(TaskOutcome.RESULT_UNKNOWN,
-                               result.log_messages)
-
-        elif result.result == action_server_msgs.msg.TaskResult.RESULT_SUCCEEDED:
-            return TaskOutcome(TaskOutcome.RESULT_SUCCEEDED,
-                               result.log_messages)
-
-        return TaskOutcome(messages=result.log_messages)
+        return task_outcome_from_result(result=result)
 
     def cancel_all(self):
         """ Cancels all goals of the action client

From 018533b0dd6a87eb737f9fbf4658141391f0ea2a Mon Sep 17 00:00:00 2001
From: Janno Lunenburg <jannolunenburg@gmail.com>
Date: Sat, 28 Apr 2018 15:21:56 +0200
Subject: [PATCH 26/46] feat(Client)Made async callbacks optional

---
 action_server/src/action_server/client.py | 10 ++++++++--
 1 file changed, 8 insertions(+), 2 deletions(-)

diff --git a/action_server/src/action_server/client.py b/action_server/src/action_server/client.py
index 72443b87..0e3dd66e 100644
--- a/action_server/src/action_server/client.py
+++ b/action_server/src/action_server/client.py
@@ -96,7 +96,7 @@ def get_actions(self):
 
         return res.actions
 
-    def send_async_task(self, semantics, done_cb, feedback_cb):
+    def send_async_task(self, semantics, done_cb=None, feedback_cb=None):
         """ Send a task to the action server and return immediately. A task is composed of one or multiple actions.
 
         :param semantics: A json string with a list of dicts, every dict in the list has at least an 'action' field,
@@ -106,11 +106,17 @@ def send_async_task(self, semantics, done_cb, feedback_cb):
         :param feedback_cb: (callable)Callback that gets called whenever feedback for this goal is received. Takes one
         parameter: the feedback.
         """
+        # Define the wrapped done callback
         def _wrapped_done_cb(_, result):
             taskoutcome = task_outcome_from_result(result=result)
             return done_cb(taskoutcome)
+
+        # The wrapped done callback is only used if the provided done callback is callable. Otherwise it's useless
+        _done_cb = _wrapped_done_cb if callable(done_cb) else None
+
+        # Create and send the goal
         goal = action_server_msgs.msg.TaskGoal(recipe=semantics)
-        self._action_client.send_goal(goal=goal, done_cb=_wrapped_done_cb, feedback_cb=feedback_cb)
+        self._action_client.send_goal(goal=goal, done_cb=_done_cb, feedback_cb=feedback_cb)
 
     def send_task(self, semantics):
         """

From e43b0454f3e56b56edf2c662da46483ce1727fac Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Sat, 28 Apr 2018 19:33:33 +0200
Subject: [PATCH 27/46] feat(send_picture): add action

---
 .../src/action_server/actions/__init__.py     | 11 +--
 .../src/action_server/actions/send_picture.py | 89 +++++++++++++++++++
 2 files changed, 95 insertions(+), 5 deletions(-)
 create mode 100644 action_server/src/action_server/actions/send_picture.py

diff --git a/action_server/src/action_server/actions/__init__.py b/action_server/src/action_server/actions/__init__.py
index 700cd86d..b1c596ea 100644
--- a/action_server/src/action_server/actions/__init__.py
+++ b/action_server/src/action_server/actions/__init__.py
@@ -2,17 +2,18 @@
 
 from answer_question import AnswerQuestion
 from arm_goal import ArmGoal
+from demo_presentation import DemoPresentation
+from find import Find
+from follow import Follow
+from guide import Guide
 from gripper_goal import GripperGoal
+from hand_over import HandOver
 from inspect import Inspect
 from look_at import LookAt
 from navigate_to import NavigateTo
 from pick_up import PickUp
 from place import Place
 from reset_wm import ResetWM
-from find import Find
 from say import Say
-from follow import Follow
-from guide import Guide
-from demo_presentation import DemoPresentation
+from send_picture import SendPicture
 from turn_toward_sound import TurnTowardSound
-from hand_over import HandOver
diff --git a/action_server/src/action_server/actions/send_picture.py b/action_server/src/action_server/actions/send_picture.py
new file mode 100644
index 00000000..aefd2e67
--- /dev/null
+++ b/action_server/src/action_server/actions/send_picture.py
@@ -0,0 +1,89 @@
+from action import Action, ConfigurationData
+import rospy
+import threading
+
+import robot_smach_states
+
+from entity_description import resolve_entity_description
+
+
+class SendPicture(Action):
+    """ The Inspect class implements the action to send a picture over a whatsapp client.
+
+    Parameters to pass to the configure() method are:
+     - `target-location` (required): an entity with a segmentation area to inspect
+    """
+    def __init__(self):
+        Action.__init__(self)
+
+    class Semantics:
+        def __init__(self):
+            self.target_location = None
+
+    @staticmethod
+    def _parse_semantics(semantics_dict):
+        semantics = SendPicture.Semantics()
+        semantics.target_location = resolve_entity_description(semantics_dict['target-location'])
+
+        return semantics
+
+    class Context:
+        def __init__(self):
+            self.location_designator = None
+
+    @staticmethod
+    def _parse_context(context_dict):
+        context = SendPicture.Context()
+
+        if 'location-designator' in context_dict:
+            context.location_designator = context_dict['location-designator']
+
+        return context
+
+    def _configure(self, robot, config):
+        self._robot = robot
+
+        self.semantics = self._parse_semantics(config.semantics)
+        self.context = self._parse_context(config.context)
+
+        if self.context.location_designator is None:
+            # Request navigation action
+            self._config_result.required_context = {'action': 'navigate-to'}
+            if self.semantics.target_location is not None:
+                self._config_result.required_context['target-location'] = config.semantics['target-location']
+            return
+        # We can now assume we arrived at the place to take the picture from.
+
+        self._config_result.succeeded = True
+        return
+
+    def _start(self):
+        self._robot.speech.speak("I should be taking a picture and sending it to you now. ")
+        self._execute_result.message = " I took a picture at {} "
+        self._execute_result.succeeded = True
+
+    def _cancel(self):
+        pass
+
+
+if __name__ == "__main__":
+    rospy.init_node('inspect_test')
+
+    import sys
+    robot_name = sys.argv[1]
+    if robot_name == 'amigo':
+        from robot_skills.amigo import Amigo as Robot
+    elif robot_name == 'sergio':
+        from robot_skills.sergio import Sergio as Robot
+    else:
+        from robot_skills.mockbot import Mockbot as Robot
+
+    robot = Robot()
+
+    action = SendPicture()
+
+    config = ConfigurationData({'action': 'inspect',
+                                'target-location': {'id': 'cabinet'}})
+
+    action.configure(robot, config)
+    action.start()

From 1a34ab1a69400ac6a916762b631b87c0b431aac3 Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Sat, 28 Apr 2018 20:55:23 +0200
Subject: [PATCH 28/46] fix(server): set aborted when crashed

---
 action_server/src/action_server/server.py | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/action_server/src/action_server/server.py b/action_server/src/action_server/server.py
index 0480e68c..de2e140c 100644
--- a/action_server/src/action_server/server.py
+++ b/action_server/src/action_server/server.py
@@ -89,6 +89,8 @@ def _add_action_cb(self, goal):
                     return
         except Exception as e:
             rospy.logerr("An error occurred using task recipe: %s\n" % goal.recipe)
+            self._result.log_messages = [' I failed to perform the task. ']
+            self._action_server.set_aborted(self._result)
             raise
 
         rospy.logdebug("Execution of state machine succeeded.")

From feebdf7a9197d23ca048754e274512d16a1c8bf4 Mon Sep 17 00:00:00 2001
From: Loy van Beek <loy.vanbeek@gmail.com>
Date: Sat, 28 Apr 2018 21:37:28 +0200
Subject: [PATCH 29/46] Cancel goals async

---
 action_server/src/action_server/client.py | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/action_server/src/action_server/client.py b/action_server/src/action_server/client.py
index 0e3dd66e..1cbe5ad9 100644
--- a/action_server/src/action_server/client.py
+++ b/action_server/src/action_server/client.py
@@ -148,3 +148,9 @@ def cancel_all(self):
         self._action_client.cancel_all_goals()
         self._action_client.wait_for_result()
         rospy.logdebug("... all goals cancelled!")
+
+    def cancel_all_async(self):
+        """ Cancels all goals of the action client and returns directly without waiting for the result
+        """
+        rospy.logdebug("cancelling all goals async...")
+        self._action_client.cancel_all_goals()

From db109f6c9981cad04ec5bbffae6034107e58861f Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Sat, 28 Apr 2018 21:56:10 +0200
Subject: [PATCH 30/46] fix(guide): guide someone to the bar

---
 .../src/action_server/actions/guide.py        | 41 +++++++++++--------
 1 file changed, 25 insertions(+), 16 deletions(-)

diff --git a/action_server/src/action_server/actions/guide.py b/action_server/src/action_server/actions/guide.py
index 6ecb0d2f..d2080f82 100644
--- a/action_server/src/action_server/actions/guide.py
+++ b/action_server/src/action_server/actions/guide.py
@@ -1,38 +1,47 @@
-from action import ConfigurationData
-from navigate_to import NavigateTo
+from action import Action, ConfigurationData
+import robot_smach_states as states
+import robot_smach_states.util.designators as ds
 
 import rospy
 
 
-class Guide(NavigateTo):
+class Guide(Action):
     ''' The Guide class navigates to a target, telling someone to follow the robot and about arriving at the target.
 
     Parameters to pass to the configure() method are:
      - `object` (required): the id of the entity to navigate to
     '''
     def __init__(self):
-        NavigateTo.__init__(self)
+        Action.__init__(self)
+        self._required_field_prompts = {'object': " What exactly would you like me to find? "}
+        self._required_skills = ['head', 'base', 'rightArm', 'speech']
 
     def _configure(self, robot, config):
         if 'object' in config.semantics and 'id' in config.semantics['object']:
-            self._id = config.semantics['object']['id']
+            self.follower_id = config.semantics['object']['id']
         else:
-            self._id = None
+            # TODO: HUUUUGE Robocup hack!
+            self.follower_id = "lars"
 
-        return NavigateTo._configure(self, robot, config)
-
-    def _start(self):
-        self._robot.speech.speak("Follow me if you want to live", block=True)
+        if 'target-location' in config.semantics and 'id' in config.semantics['target-location']:
+            self.target_location_id = config.semantics['target-location']['id']
+        else:
+            # TODO: HUUUUGE Robocup hack!
+            self.target_location_id = "bar"
 
-        res = NavigateTo._start(self)
+        target_location_designator = ds.EntityByIdDesignator(robot, id=self.target_location_id)
+        follower_designator = ds.EntityByIdDesignator(robot, id=self.follower_id)
 
-        if self._id:
-            self._robot.speech.speak("Here is the {}".format(self._id))
+        self._guide_state_machine = states.Guide(robot=robot, target_location=target_location_designator, follower=follower_designator)
+        self._config_result.succeeded = True
 
-        return res
+    def _start(self):
+        self._guide_state_machine.execute()
+        self._execute_result.succeeded = True
+        self._execute_result.message = "I guided "
 
     def _cancel(self):
-        return NavigateTo._cancel(self)
+        self._guide_state_machine.request_preempt()
 
 
 if __name__ == "__main__":
@@ -49,7 +58,7 @@ def _cancel(self):
 
     robot = Robot()
 
-    action = NavigateTo()
+    action = Guide()
 
     config = ConfigurationData({'action': 'guide',
               'object': {'id': 'cabinet'}})

From 8359d4af63f47e1c69f1aa81d3a77d915cfce435 Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Sat, 28 Apr 2018 21:56:51 +0200
Subject: [PATCH 31/46] fix(send_picture): add sending picture functionality

---
 .../src/action_server/actions/send_picture.py | 27 ++++++++++++++-----
 1 file changed, 20 insertions(+), 7 deletions(-)

diff --git a/action_server/src/action_server/actions/send_picture.py b/action_server/src/action_server/actions/send_picture.py
index aefd2e67..a954f15b 100644
--- a/action_server/src/action_server/actions/send_picture.py
+++ b/action_server/src/action_server/actions/send_picture.py
@@ -1,8 +1,6 @@
 from action import Action, ConfigurationData
 import rospy
-import threading
-
-import robot_smach_states
+import robot_smach_states as states
 
 from entity_description import resolve_entity_description
 
@@ -54,16 +52,31 @@ def _configure(self, robot, config):
             return
         # We can now assume we arrived at the place to take the picture from.
 
+        self.detect_face_state_machine = states.DetectFace(self._robot)
+
         self._config_result.succeeded = True
         return
 
     def _start(self):
-        self._robot.speech.speak("I should be taking a picture and sending it to you now. ")
-        self._execute_result.message = " I took a picture at {} "
-        self._execute_result.succeeded = True
+        self._robot.speech.speak("I will take a picture and send it to my operator now. ")
+        r = rospy.Rate(1.0)
+        i = 0
+        result = None
+        while result is not 'succeeded' and i < 10:
+            self.detect_face_state_machine.execute(self._robot)
+            r.sleep()
+            i += 1
+
+        if result == 'failed':
+            self._execute_result.message = " I didn't see anyone. "
+            self._execute_result.succeeded = False
+        else:
+            self._execute_result.message = " I found someone at the door. "
+            self._execute_result.succeeded = True
 
     def _cancel(self):
-        pass
+        if self.detect_face_state_machine:
+            self.detect_face_state_machine.request_preempt()
 
 
 if __name__ == "__main__":

From 5ef62cf3767d2cb43ace62e24d8e14fcaa53f035 Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Sat, 28 Apr 2018 22:25:41 +0200
Subject: [PATCH 32/46] fix(guide): dont pass location as context

---
 action_server/src/action_server/actions/guide.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/action_server/src/action_server/actions/guide.py b/action_server/src/action_server/actions/guide.py
index d2080f82..7d97ea6f 100644
--- a/action_server/src/action_server/actions/guide.py
+++ b/action_server/src/action_server/actions/guide.py
@@ -33,6 +33,7 @@ def _configure(self, robot, config):
         follower_designator = ds.EntityByIdDesignator(robot, id=self.follower_id)
 
         self._guide_state_machine = states.Guide(robot=robot, target_location=target_location_designator, follower=follower_designator)
+        # self._config_result.context['location-designator'] = target_location_designator
         self._config_result.succeeded = True
 
     def _start(self):

From 4a104b140bba151f856d7f3ae1121c859b9f79fb Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Sun, 29 Apr 2018 08:20:23 +0200
Subject: [PATCH 33/46] fix(find): return true on configuring for find person

---
 action_server/src/action_server/actions/find.py | 12 +++++++++---
 1 file changed, 9 insertions(+), 3 deletions(-)

diff --git a/action_server/src/action_server/actions/find.py b/action_server/src/action_server/actions/find.py
index f44baa45..53daa4c8 100644
--- a/action_server/src/action_server/actions/find.py
+++ b/action_server/src/action_server/actions/find.py
@@ -79,6 +79,9 @@ def _configure(self, robot, config):
             self._config_result.message = " I can't grasp anything from the {}".format(self._location.id)
             return
 
+        # Set up designator to be filled with the found entity
+        self._found_entity_designator = VariableDesignator(resolve_type=Entity)
+
         # Set up designator for areas
         self._areas = {}
         self._nav_areas = {}
@@ -89,8 +92,14 @@ def _configure(self, robot, config):
                     self._areas[location] = self._knowledge.get_inspect_areas(location)
                     self._nav_areas[location] = self._knowledge.get_inspect_position(location)
             else:
+                # person in room
+                self._find_state_machines = [states.FindPersonInRoom(robot, self._location.id, self._object.id)]
                 self._areas[self._location.id] = ["in"]
                 self._nav_areas[self._location.id] = "in"
+                self._config_result.context['object-designator'] = self._found_entity_designator
+                self._config_result.context['location-designator'] = EdEntityDesignator(self._robot, id=self._location.id)
+                self._config_result.succeeded = True
+                return
         elif self._object.type == "person":
             self._areas[self._location.id] = ["near"]
             self._nav_areas[self._location.id] = "near"
@@ -102,9 +111,6 @@ def _configure(self, robot, config):
         entity_description = {'type': self._object.type}
         description_designator = VariableDesignator(entity_description)
 
-        # Set up designator to be filled with the found entity
-        self._found_entity_designator = VariableDesignator(resolve_type=Entity)
-
         self._find_state_machines = []
         for loc, areas in self._areas.iteritems():
             location_designator = EdEntityDesignator(self._robot, id=loc)

From 63298fa85459420e161145d8f74cbcd46e129887 Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Sun, 29 Apr 2018 08:47:53 +0200
Subject: [PATCH 34/46] fix(find): fixes for find person

---
 action_server/src/action_server/actions/find.py | 6 ++----
 1 file changed, 2 insertions(+), 4 deletions(-)

diff --git a/action_server/src/action_server/actions/find.py b/action_server/src/action_server/actions/find.py
index 53daa4c8..6bf2c688 100644
--- a/action_server/src/action_server/actions/find.py
+++ b/action_server/src/action_server/actions/find.py
@@ -144,14 +144,12 @@ def _start(self):
         for fsm in self._find_state_machines:
             res = fsm.execute()
 
-            if res == 'succeeded':
+            if res in ['succeeded', 'found']:
                 self._execute_result.message = " I found {}. ".format(
                     self._object.id if self._object.id else "a " + self._object.type)
                 self._execute_result.succeeded = True
 
-                if self._object.type == "person":
-                    self._point_at_person(self._found_entity_designator.resolve())
-                else:
+                if not self._object.type == "person":
                     self._robot.speech.speak("Hey, I found a {}!".format(self._object.type))
                 return
             elif res == 'not_found':

From b23729caeedd03c33f5c14c9f2f2b92e9b9e46d9 Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Sun, 29 Apr 2018 09:38:11 +0200
Subject: [PATCH 35/46] fix(final): "this guy" needs a location

---
 .../src/action_server/actions/entity_description.py   |  2 ++
 .../src/action_server/actions/navigate_to.py          | 11 +++++++++--
 2 files changed, 11 insertions(+), 2 deletions(-)

diff --git a/action_server/src/action_server/actions/entity_description.py b/action_server/src/action_server/actions/entity_description.py
index 721baa28..fc14f5aa 100644
--- a/action_server/src/action_server/actions/entity_description.py
+++ b/action_server/src/action_server/actions/entity_description.py
@@ -40,5 +40,7 @@ def resolve_entity_description(parameters):
             description.location = resolve_entity_description(parameters["loc"])
         if "designator" in parameters:
             description.designator = parameters["designator"]
+        if "location" in parameters:
+            description.location = EntityDescription(id=parameters["location"])
 
     return description
diff --git a/action_server/src/action_server/actions/navigate_to.py b/action_server/src/action_server/actions/navigate_to.py
index 8d7d79a8..ff35243a 100644
--- a/action_server/src/action_server/actions/navigate_to.py
+++ b/action_server/src/action_server/actions/navigate_to.py
@@ -59,8 +59,15 @@ def _configure(self, robot, config):
         semantics = self._parse_semantics(config.semantics)
         context = self._parse_context(config.context)
 
-        know_target = (semantics.target_location.id and semantics.target_location.type != 'person' or
-                       (semantics.target_location.type == 'reference' and context.object) or
+        if semantics.target_location.id == 'this_guy' and semantics.target_location.location is None:
+            rospy.logerr("Missing required parameter {}".format('target-location.location'))
+            self._config_result.missing_field = 'target-location.location'
+            self._config_result.message = ' Where should I look for this guy? '
+            self._config_result.succeeded = False
+            return
+
+        know_target = (semantics.target_location.id and semantics.target_location.type != 'person' or  # We're talking about some piece of furniture or object
+                       (semantics.target_location.type == 'reference' and context.object) or  # navigate to "it"
                        (semantics.target_location.type == 'person' and context.object))
 
         if not know_target:

From 1c4ff92a3733f1d76ac23df1e9e1cc6a8ae0c7d7 Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Sun, 29 Apr 2018 09:55:53 +0200
Subject: [PATCH 36/46] fix(send_picture): update result

---
 action_server/src/action_server/actions/send_picture.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/action_server/src/action_server/actions/send_picture.py b/action_server/src/action_server/actions/send_picture.py
index a954f15b..188fdbe4 100644
--- a/action_server/src/action_server/actions/send_picture.py
+++ b/action_server/src/action_server/actions/send_picture.py
@@ -63,7 +63,7 @@ def _start(self):
         i = 0
         result = None
         while result is not 'succeeded' and i < 10:
-            self.detect_face_state_machine.execute(self._robot)
+            result = self.detect_face_state_machine.execute(self._robot)
             r.sleep()
             i += 1
 

From 61ab0d315edecab446babaf8bb17f582d7d74342 Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Sun, 29 Apr 2018 10:12:06 +0200
Subject: [PATCH 37/46] hack(navigate_to): lars is known entity

---
 action_server/src/action_server/actions/navigate_to.py | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/action_server/src/action_server/actions/navigate_to.py b/action_server/src/action_server/actions/navigate_to.py
index ff35243a..92de6ac0 100644
--- a/action_server/src/action_server/actions/navigate_to.py
+++ b/action_server/src/action_server/actions/navigate_to.py
@@ -66,7 +66,8 @@ def _configure(self, robot, config):
             self._config_result.succeeded = False
             return
 
-        know_target = (semantics.target_location.id and semantics.target_location.type != 'person' or  # We're talking about some piece of furniture or object
+        know_target = (semantics.target_location.id and
+                       (semantics.target_location.type != 'person' or semantics.target_location.id == 'lars') or  # We're talking about some piece of furniture or object
                        (semantics.target_location.type == 'reference' and context.object) or  # navigate to "it"
                        (semantics.target_location.type == 'person' and context.object))
 
@@ -83,7 +84,8 @@ def _configure(self, robot, config):
             return
         # Now we can assume we know the navigation goal entity!
 
-        if semantics.target_location.id and semantics.target_location.type != 'person':
+        if semantics.target_location.id and \
+            (semantics.target_location.type != 'person' or semantics.target_location.id == 'lars'):
             entity_designator = EntityByIdDesignator(self._robot, id=semantics.target_location.id)
             e = entity_designator.resolve()
 

From ad9169af1488326ea06f3eb1eda92286aad990bd Mon Sep 17 00:00:00 2001
From: Matthijs van der Burgh <matthijs.vander.burgh@live.nl>
Date: Wed, 2 May 2018 17:22:31 +0200
Subject: [PATCH 38/46] Fix branch option for CI

---
 .travis.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.travis.yml b/.travis.yml
index 23d756e0..ee734cbc 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -15,6 +15,6 @@ before_install:
 script:
   - 'for PACKAGE in $PACKAGES;
      do
-     bash install-package.sh --package=$PACKAGE --branch=$TRAVIS_BRANCH --commit=$TRAVIS_COMMIT --pullrequest=$TRAVIS_PULL_REQUEST &&
+     bash install-package.sh --package=$PACKAGE --branch=${TRAVIS_PULL_REQUEST_BRANCH:-$TRAVIS_BRANCH} --commit=$TRAVIS_COMMIT --pullrequest=$TRAVIS_PULL_REQUEST &&
      bash build-package.sh --package=$PACKAGE;
      done'

From 3ee5b12f2f479170538d7e9e038eb03b3cdbf6af Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Sat, 16 Jun 2018 12:55:13 +0200
Subject: [PATCH 39/46] chore(find): remove unused import

---
 action_server/src/action_server/actions/find.py | 1 -
 1 file changed, 1 deletion(-)

diff --git a/action_server/src/action_server/actions/find.py b/action_server/src/action_server/actions/find.py
index 6bf2c688..faa07681 100644
--- a/action_server/src/action_server/actions/find.py
+++ b/action_server/src/action_server/actions/find.py
@@ -1,7 +1,6 @@
 from action import Action, ConfigurationData
 
 from entity_description import resolve_entity_description, EntityDescription
-from util import entities_from_description
 
 import rospy
 import math

From a7e6c86fba2032c945938dff53017c4967504165 Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Sat, 16 Jun 2018 12:59:49 +0200
Subject: [PATCH 40/46] chore(navigate-to): remove robocup hacks

---
 action_server/src/action_server/actions/navigate_to.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/action_server/src/action_server/actions/navigate_to.py b/action_server/src/action_server/actions/navigate_to.py
index 92de6ac0..7db71f7e 100644
--- a/action_server/src/action_server/actions/navigate_to.py
+++ b/action_server/src/action_server/actions/navigate_to.py
@@ -67,7 +67,7 @@ def _configure(self, robot, config):
             return
 
         know_target = (semantics.target_location.id and
-                       (semantics.target_location.type != 'person' or semantics.target_location.id == 'lars') or  # We're talking about some piece of furniture or object
+                       semantics.target_location.type != 'person' or  # We're talking about some piece of furniture or object
                        (semantics.target_location.type == 'reference' and context.object) or  # navigate to "it"
                        (semantics.target_location.type == 'person' and context.object))
 
@@ -85,7 +85,7 @@ def _configure(self, robot, config):
         # Now we can assume we know the navigation goal entity!
 
         if semantics.target_location.id and \
-            (semantics.target_location.type != 'person' or semantics.target_location.id == 'lars'):
+            semantics.target_location.type != 'person':
             entity_designator = EntityByIdDesignator(self._robot, id=semantics.target_location.id)
             e = entity_designator.resolve()
 

From 2235c558c8a3c29cdcd44fd814ee08319c2e7a47 Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Sat, 16 Jun 2018 09:22:16 -0400
Subject: [PATCH 41/46] chore(guide): remove robocup hacks

---
 .../src/action_server/actions/guide.py        | 75 ++++++++++++++-----
 1 file changed, 58 insertions(+), 17 deletions(-)

diff --git a/action_server/src/action_server/actions/guide.py b/action_server/src/action_server/actions/guide.py
index 7d97ea6f..2f0e8116 100644
--- a/action_server/src/action_server/actions/guide.py
+++ b/action_server/src/action_server/actions/guide.py
@@ -1,6 +1,7 @@
 from action import Action, ConfigurationData
 import robot_smach_states as states
 import robot_smach_states.util.designators as ds
+from entity_description import resolve_entity_description
 
 import rospy
 
@@ -15,25 +16,65 @@ def __init__(self):
         Action.__init__(self)
         self._required_field_prompts = {'object': " What exactly would you like me to find? "}
         self._required_skills = ['head', 'base', 'rightArm', 'speech']
+        self._follower_id = None
+        self._target_id = None
+
+    class Semantics:
+        def __init__(self):
+            self.follower = None
+            self.source_location = None
+            self.target_location = None
+
+    @staticmethod
+    def _parse_semantics(semantics_dict):
+        semantics = Guide.Semantics()
+
+        if 'object' in semantics_dict:
+            semantics.follower = resolve_entity_description(semantics_dict['object'])
+
+        if 'source-location' in semantics_dict:
+            semantics.source_location = resolve_entity_description(semantics_dict['source-location'])
+
+        if 'target-location' in semantics_dict:
+            semantics.target_location = resolve_entity_description(semantics_dict['target-location'])
+
+        return semantics
+
+    class Context:
+        def __init__(self):
+            self.person_designator = None
+
+    @staticmethod
+    def _parse_context(context_dict):
+        context = Guide.Context()
+
+        if 'person-designator' in context_dict:
+            context.person_designator = context_dict['person-designator']
+
+        return context
 
     def _configure(self, robot, config):
-        if 'object' in config.semantics and 'id' in config.semantics['object']:
-            self.follower_id = config.semantics['object']['id']
-        else:
-            # TODO: HUUUUGE Robocup hack!
-            self.follower_id = "lars"
-
-        if 'target-location' in config.semantics and 'id' in config.semantics['target-location']:
-            self.target_location_id = config.semantics['target-location']['id']
-        else:
-            # TODO: HUUUUGE Robocup hack!
-            self.target_location_id = "bar"
-
-        target_location_designator = ds.EntityByIdDesignator(robot, id=self.target_location_id)
-        follower_designator = ds.EntityByIdDesignator(robot, id=self.follower_id)
-
-        self._guide_state_machine = states.Guide(robot=robot, target_location=target_location_designator, follower=follower_designator)
-        # self._config_result.context['location-designator'] = target_location_designator
+        # We start by parsing semantics and context
+        self._semantics = Guide._parse_semantics(config.semantics)
+        self._context = Guide._parse_context(config.context)
+
+        # Before we start guiding, we need to know we're at the person we should guide
+        # we check this by checking if we found a person earlier
+        if not self._context.person_designator:
+            self._config_result.required_context = {
+                'action': 'find',
+                'object': config.semantics['object'],
+                'source-location': config.semantics['source-location']
+            }
+            print "I'll need to find someone first!"
+            return
+
+        target_location_designator = ds.EntityByIdDesignator(robot, id=self._semantics.target_location.id)
+
+        self._guide_state_machine = states.Guide(robot=robot,
+                                                 target_location=target_location_designator,
+                                                 follower=self._context.person_designator)
+
         self._config_result.succeeded = True
 
     def _start(self):

From 50d7845b816351760eb05a33482cb9c213b3d7f7 Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Sat, 16 Jun 2018 09:29:03 -0400
Subject: [PATCH 42/46] fix(send-picture): update action and node names

---
 action_server/src/action_server/actions/send_picture.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/action_server/src/action_server/actions/send_picture.py b/action_server/src/action_server/actions/send_picture.py
index 188fdbe4..f7a4eade 100644
--- a/action_server/src/action_server/actions/send_picture.py
+++ b/action_server/src/action_server/actions/send_picture.py
@@ -80,7 +80,7 @@ def _cancel(self):
 
 
 if __name__ == "__main__":
-    rospy.init_node('inspect_test')
+    rospy.init_node('send_picture_test')
 
     import sys
     robot_name = sys.argv[1]
@@ -95,7 +95,7 @@ def _cancel(self):
 
     action = SendPicture()
 
-    config = ConfigurationData({'action': 'inspect',
+    config = ConfigurationData({'action': 'send-picture',
                                 'target-location': {'id': 'cabinet'}})
 
     action.configure(robot, config)

From de62d43c94574d9f74d4131dddd61d3f7e5fe117 Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Sat, 16 Jun 2018 09:43:48 -0400
Subject: [PATCH 43/46] fix(send-picture): remove loop, fixes cancel

---
 action_server/src/action_server/actions/send_picture.py | 9 ++-------
 1 file changed, 2 insertions(+), 7 deletions(-)

diff --git a/action_server/src/action_server/actions/send_picture.py b/action_server/src/action_server/actions/send_picture.py
index f7a4eade..68acefff 100644
--- a/action_server/src/action_server/actions/send_picture.py
+++ b/action_server/src/action_server/actions/send_picture.py
@@ -59,13 +59,8 @@ def _configure(self, robot, config):
 
     def _start(self):
         self._robot.speech.speak("I will take a picture and send it to my operator now. ")
-        r = rospy.Rate(1.0)
-        i = 0
-        result = None
-        while result is not 'succeeded' and i < 10:
-            result = self.detect_face_state_machine.execute(self._robot)
-            r.sleep()
-            i += 1
+
+        result = self.detect_face_state_machine.execute(self._robot)
 
         if result == 'failed':
             self._execute_result.message = " I didn't see anyone. "

From 547a183365abe2091ac7b140b9c6193eabdb534f Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Sat, 16 Jun 2018 09:44:30 -0400
Subject: [PATCH 44/46] fix(send-picture): only request preempt if state
 machine running

---
 action_server/src/action_server/actions/send_picture.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/action_server/src/action_server/actions/send_picture.py b/action_server/src/action_server/actions/send_picture.py
index 68acefff..291d77fc 100644
--- a/action_server/src/action_server/actions/send_picture.py
+++ b/action_server/src/action_server/actions/send_picture.py
@@ -70,7 +70,7 @@ def _start(self):
             self._execute_result.succeeded = True
 
     def _cancel(self):
-        if self.detect_face_state_machine:
+        if self.detect_face_state_machine.is_running:
             self.detect_face_state_machine.request_preempt()
 
 

From 9bf79f72fe5c23822257d2c46acda7a21d67d757 Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Sat, 16 Jun 2018 09:55:38 -0400
Subject: [PATCH 45/46] fix(hand-over): say "bring the action" at the right
 time

---
 action_server/src/action_server/actions/hand_over.py | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/action_server/src/action_server/actions/hand_over.py b/action_server/src/action_server/actions/hand_over.py
index 8bdfc56f..129e1edb 100644
--- a/action_server/src/action_server/actions/hand_over.py
+++ b/action_server/src/action_server/actions/hand_over.py
@@ -100,6 +100,8 @@ def _configure(self, robot, config):
             return
         # We can now assume that we are at the destination for handover!
 
+        self._robot.speech.speak("Bring the action!")
+
         self._config_result.succeeded = True
 
     def _handover(self):
@@ -129,7 +131,6 @@ def _handover(self):
         arm.occupied_by = None
 
     def _start(self):
-        self._robot.speech.speak("Bring the action!")
         # Handover
         self._handover()
 

From 2c5fba4e5f499c2ea3ba585d478f75740eb07cb8 Mon Sep 17 00:00:00 2001
From: Rokus Ottervanger <rokusottervanger@gmail.com>
Date: Sat, 16 Jun 2018 09:56:02 -0400
Subject: [PATCH 46/46] fix(place): dont say "bring the action"

---
 action_server/src/action_server/actions/place.py | 2 --
 1 file changed, 2 deletions(-)

diff --git a/action_server/src/action_server/actions/place.py b/action_server/src/action_server/actions/place.py
index 68cfed90..0f1b285a 100644
--- a/action_server/src/action_server/actions/place.py
+++ b/action_server/src/action_server/actions/place.py
@@ -130,8 +130,6 @@ def _configure(self, robot, config):
         return
 
     def _start(self):
-        self._robot.speech.speak("Bring the action!")
-
         # We either got an arm, or we know which arm to place with
         arm_designator = None
         if self.semantics.arm: