Merge pull request #101 from tue-robotics/robocup

RWC2018

action_server/package.xml

@@ -10,9 +10,10 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
-  <depend>action_server_msgs</depend>
-  <depend>grammar_parser</depend>
-  <depend>robot_skills</depend>
-  <depend>actionlib</depend>
-
+  <exec_depend>action_server_msgs</exec_depend>
+  <exec_depend>grammar_parser</exec_depend>
+  <exec_depend>robot_skills</exec_depend>
+  <exec_depend>robot_smach_states</exec_depend>
+  <exec_depend>actionlib</exec_depend>
+  <exec_depend>challenge_presentation</exec_depend>
 </package>

action_server/src/action_server/actions/__init__.py

@@ -2,18 +2,22 @@
 
 from answer_question import AnswerQuestion
 from arm_goal import ArmGoal
+from count_and_tell import CountAndTell
 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 inspect_action 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 say import Say
 from send_picture import SendPicture
+from tell_name_of_person import TellNameOfPerson
 from turn_toward_sound import TurnTowardSound
+
+# from open_door import OpenDoor

action_server/src/action_server/actions/answer_question.py

@@ -1,5 +1,6 @@
 from action import Action, ConfigurationData
 from find import Find
+from entity_description import resolve_entity_description
 
 import rospy
 from robocup_knowledge import load_knowledge
@@ -18,6 +19,33 @@ class AnswerQuestion(Action):
     def __init__(self):
         Action.__init__(self)
         self._required_skills = ['speech', 'hmi']
+        self._preempt_requested = False
+
+    class Semantics:
+        def __init__(self):
+            self.target_person = None
+
+    @staticmethod
+    def _parse_semantics(semantics_dict):
+        semantics = AnswerQuestion.Semantics()
+
+        if 'target-person' in semantics_dict:
+            semantics.target_person = resolve_entity_description(semantics_dict['target-person'])
+
+        return semantics
+
+    class Context:
+        def __init__(self):
+            self.object = None
+
+    @staticmethod
+    def _parse_context(context_dict):
+        context = AnswerQuestion.Context()
+
+        if 'object' in context_dict:
+            context.object = resolve_entity_description(context_dict['object'])
+
+        return context
 
     def _configure(self, robot, config):
         self._robot = robot
@@ -27,38 +55,29 @@ def _configure(self, robot, config):
             rospy.logerr("Failed to load speech data for 'AnswerQuestion' action")
             return
 
+        semantics = AnswerQuestion._parse_semantics(config.semantics)
+        context = AnswerQuestion._parse_context(config.context)
+
         # If a person is specified in the task description, we need to go and find that person first
-        if 'target-person' in config.semantics:
-            self._find_action = Find()
-            location_id = config.semantics['target-person']['loc']
-            find_semantics = {'object' : config.semantics['target-person'],
-                              'location' : {'id': location_id,
-                                            'type': "furniture" if self._knowledge.is_location(location_id)
-                                            else "room"}}
-            find_config = ConfigurationData(find_semantics, config.knowledge)
-            find_config_result = self._find_action.configure(robot, find_config)
-            if not find_config_result.succeeded:
-                self._config_result = find_config_result
+        if semantics.target_person and not context.object:
+            self._config_result.required_context = {
+                'action': 'find',
+                'object': config.semantics['target-person']
+            }
+            if semantics.target_person.location:
+                self._config_result.required_context['source-location'] = config.semantics['target-person']['location']
                 return
-        else:
-            self._find_action = None
 
         self._config_result.succeeded = True
 
     def _start(self):
-        if self._find_action:
-            find_exec_res = self._find_action.start()
-            if not find_exec_res.succeeded:
-                self._execute_result = find_exec_res
-                return
-
         self._robot.head.look_at_standing_person()
         self._robot.head.wait_for_motion_done()
 
         self._robot.speech.speak("What is your question?")
 
         tries = 0
-        while tries < 3:
+        while tries < 3 and not self._preempt_requested:
             try:
                 res = self._robot.hmi.query(description="",
                                             grammar=self._speech_data.grammar,
@@ -90,9 +109,8 @@ def _start(self):
                     self._execute_result.message = " I did not understand the question. "
                 tries += 1
 
-
     def _cancel(self):
-        pass
+        self._preempt_requested = True
 
 
 if __name__ == "__main__":

action_server/src/action_server/actions/clear_table.py

@@ -0,0 +1,28 @@
+from action import Action, ConfigurationData
+from challenge_dishwasher.dishwasher import NavigateAndPlaceDishwasher, GrabRobust, NavigateAndOpenDishwasher
+
+class ClearTable(Action):
+    def __init__(self):
+        Action.__init__(self)
+
+    def _configure(self, robot, config):
+        self._state_machines = [GrabRobust(robot), NavigateAndOpenDishwasher(robot), NavigateAndPlaceDishwasher(robot)]
+
+        self._config_result.succeeded = True
+        self._active_state_machine = None
+        return
+
+    def _start(self):
+        for sm in self._state_machines:
+            self._active_state_machine = sm
+            self._active_state_machine.execute()
+
+        self._active_state_machine = None
+
+        self._execute_result.message = " I cleaned the table! "
+        self._execute_result.succeeded = True
+        return
+
+    def _cancel(self):
+        if self._active_state_machine:
+            self._active_state_machine.request_preempt()

action_server/src/action_server/actions/count_and_tell.py

@@ -0,0 +1,96 @@
+from action import Action, ConfigurationData
+from entity_description import resolve_entity_description
+
+import rospy
+import robot_smach_states as states
+import robot_smach_states.util.designators as ds
+
+
+
+class CountAndTell(Action):
+    """
+    The CountAndTell class implements the action to count the number of objects in a specific location and tell the
+        operator the amount of objects, the type and the location.
+
+    Parameters to pass to the configure() method are:
+     - `config` (required): the ConfigurationData defines the input data structure for configuration of an action i.e
+                            a JSON string with for this action a "location" and an " object".
+    """
+    def __init__(self):
+        Action.__init__(self)
+        self._required_skills = ['speech']
+
+    class Semantics:
+        def __init__(self):
+            self.location = None
+            self.object = None
+
+    @staticmethod
+    def _parse_semantics(semantics_dict):
+        semantics = CountAndTell.Semantics()
+
+        semantics.location = resolve_entity_description(semantics_dict['location'])
+        semantics.object = resolve_entity_description(semantics_dict['object'])
+
+        return semantics
+
+    def _configure(self, robot, config):
+        self._robot = robot
+
+        semantics = CountAndTell._parse_semantics(config.semantics)
+
+        self._count_designator = ds.VariableDesignator(-1)
+        self._where_to_count_designator = ds.EntityByIdDesignator(robot, id=semantics.location.id)
+        self._what_to_count_designator = ds.Designator(semantics.object.type)
+        self._count_state_machine = states.InspectAndCount(robot,
+                                                           self._where_to_count_designator,
+                                                           self._what_to_count_designator,
+                                                           self._count_designator)
+
+        # Here we set up a message that is formatted further later, in self._start
+        self._execute_result.message = "I counted {{c}} {t}s on the {l}".format(t=semantics.object.type,
+                                                                                l=semantics.location.id)
+        self._config_result.succeeded = True
+        return
+
+    def _start(self):
+        res = self._count_state_machine.execute()
+        if res == 'Aborted':
+            self._execute_result.succeeded = False
+            self._execute_result.message = "I failed to count the objects."
+            return
+
+        self._execute_result.succeeded = True
+
+        # This message is instantiated in _configure but leaves some stuff to be formatted
+        self._execute_result.message = self._execute_result.message.format(c=self._count_designator.resolve())
+        self._robot.speech.speak(self._execute_result.message)
+
+    def _cancel(self):
+        pass
+
+
+if __name__ == "__main__":
+    rospy.init_node('say_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 = CountAndTell()
+
+    config = ConfigurationData({'action': 'count-and-tell',
+              'location': {'id': 'counter'},
+              'object': {'type': 'apple'}})
+
+    action.configure(robot, config)
+    action.start()
+
+    rospy.loginfo(action._execute_result)

action_server/src/action_server/actions/entity_description.py

@@ -36,11 +36,9 @@ def resolve_entity_description(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 "location" in parameters:
+            description.location = resolve_entity_description(parameters["location"])
         if "designator" in parameters:
             description.designator = parameters["designator"]
-        if "location" in parameters:
-            description.location = EntityDescription(id=parameters["location"])
 
     return description