Merge pull request #828 from tue-robotics/rgo2019_challenge_serving_d…

…rinks

Rgo2019 challenge serving drinks

challenge_serving_drinks/CMakeLists.txt

@@ -0,0 +1,11 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(challenge_serving_drinks)
+
+find_package(catkin REQUIRED COMPONENTS
+  robot_skills
+  robot_smach_states
+)
+
+catkin_python_setup()
+
+catkin_package()

challenge_serving_drinks/README.md

@@ -0,0 +1,100 @@
+# Responsible: Sam
+
+## CHALLENGE SERVING DRINKS
+
+## Scenario (RGO2019)
+
+I Start:
+
+    The robot starts at a designated starting position <serving_drinks_initial> (in the hallway) and drives to the living room.
+    To qualify for the bonus reward, first go to inspect the bar and then drive to the living room.
+
+II Detection / Get order:
+
+    Setup:
+        There are at least 5 guests in the living room, two standing and three sitting.
+        Each guest has assigned a predefined name and has either a drink or a drink request (choice and alternative).
+    Procedure:
+        The robot detects which are the people without a drink and then gets order.
+        The get order procedure includes:
+            - drive to person
+            - asks for his/her name
+            - learn operator (to recognize him later)
+            - get order for a drink
+                *In case the ordered drink is unavailable, the robot announces that and asks for alternative choice.
+
+III Waiter duty:
+
+    Setup:
+        The bar can be any flat surface where objects can be placed, in any other room but the living room.
+            *2h before test the bar location is specified and announced
+        The Bartender may be standing either behind the bar or next to it, depending on the arena setup.
+        All available beverages are on top of the bar.  
+        One of the drink requests is not available.
+    Procedure:
+        The robot drives to the bar, grabs the correct drink and returns to the living room.
+        The robot finds the person, who requested the drink and hand it over.
+
+        *The robot may either hand-over drinks orhave attached a tray.  
+        *When a tray is used, the robot must be sure that the guest is taking the correct drink (guests may try to take the wrong one).
+        *After giving the order (when the robot is not in the living room), the referees may re-arrange the people.
+
+
+## Scoring sheet of RGO 2019:
+
+	*Partial scoring applies - we get points per served drink
+	*We get bonus rewards only if (at least) one main goal is successfuly executed
+
+	OC instructions:
+		2h before test specify and announce the bar location!
+
+	The maximum time for this test is 5 minutes!
+
+	Main Goal:
+		Deliver a drink to a guest:	                                3 x 250
+		
+		Penalties:
+			Each guest approaching to the robot to place order 	3 × –100
+			Each guest waving or calling the robot to place order 	3 × –50
+			Telling the robot which drink is unavailable 		2 × –100
+
+		Bonus rewards:
+			Inform a guest of drink unavailability upon request 	250
+
+	Total score (excluding penalties and bonuses):				750
+
+	Outstanding performance:						100
+
+
+## Testing / Running
+
+Launch procedure:
+    ```
+    robot-start
+    ```
+    ```
+    robot-challenge-serving-drinks
+    ```
+
+## ToDo:
+
+    Status after RGO2019:
+
+        - After the initial inspection we ask for the unavailable drink.
+        - Should be checked if the passing of the inspected objects is correct and it works.
+        - Waving detection implemented, should be tested.
+        - Detection of people, holding drinks not yet implemented. Is it necessary?
+
+	Enchancements:
+
+        - Storing info for the available drinks (implemented, not tested)
+        - Detect people holding drinks
+
+	Tested successfully:
+
+        - Learn people and their names
+        - Inspecting bar
+        - Grasping a drink (handover from human)
+        - Find specific person in the room stating his/her name and requested drink
+        - Hand over drink
+        - Run full challenge :)

challenge_serving_drinks/package.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>challenge_serving_drinks</name>
+  <version>0.0.0</version>
+  <description>The challenge_serving_drinks package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <maintainer email="alexandrov.samuil@gmail.com">Sam Aleksandrov</maintainer>
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <exec_depend>robot_skills</exec_depend>
+  <exec_depend>robot_smach_states</exec_depend>
+
+</package>

challenge_serving_drinks/scrips/challenge_serving_drinks

@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+
+import rospy
+from robot_smach_states.util.startup import startup
+from challenge_serving_drinks.serving_drinks import ServingDrinks
+
+
+if __name__ == '__main__':
+    rospy.init_node('state_machine')
+
+    startup(ServingDrinks)

challenge_serving_drinks/setup.py

@@ -0,0 +1,13 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    # #  don't do this unless you want a globally visible script
+    # scripts=['bin/myscript'],
+    packages=['challenge_serving_drinks'],
+    package_dir={'': 'src'}
+)
+
+setup(**d)

challenge_serving_drinks/src/challenge_serving_drinks/drive_in.py

@@ -0,0 +1,88 @@
+# ROS
+import smach
+
+# Robot smach states
+import robot_smach_states as states
+from robocup_knowledge import knowledge_loader
+
+# Serving drinks
+from .sd_states import AskAvailability
+
+# Knowledge
+CHALLENGE_KNOWLEDGE = knowledge_loader.load_knowledge("challenge_serving_drinks")
+
+
+class DriveIn(smach.StateMachine):
+    """ Serves on drink to an operator
+
+    """
+    def __init__(self, robot, bar_designator, room_designator, objects_list_des, unav_drink_des):
+        """ Initialization method
+
+        :param robot: robot api object
+        """
+        smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"])
+
+        # ToDo: check the storing of the available drinks: see inspect_shelves from challenge storing groceries,
+        #  inspections designator is in smach states -> world model
+        #  Classification result is in robot skills -> world model ed
+
+        with self:
+            # Initialize
+            smach.StateMachine.add(
+                'INITIALIZE',
+                states.Initialize(robot),
+                transitions={"initialized": "WAIT_1",
+                             "abort": "aborted"}
+            )
+
+            # Wait for one second so that initialise has time to finish
+            smach.StateMachine.add(
+                    "WAIT_1",
+                    states.WaitTime(
+                        robot=robot,
+                        waittime=1),
+                    transitions={"waited": "SET_INITIAL_POSE",
+                                 "preempted": "SET_INITIAL_POSE"}
+            )
+
+            smach.StateMachine.add(
+                'SET_INITIAL_POSE',
+                states.SetInitialPose(robot, CHALLENGE_KNOWLEDGE.starting_point),
+                transitions={"done": "INSPECT_BAR",
+                             "preempted": "aborted",
+                             "error": "INSPECT_BAR"}
+            )
+
+            # Inspect bar and store the list of available drinks
+            smach.StateMachine.add(
+                "INSPECT_BAR",
+                states.Inspect(
+                    robot=robot,
+                    entityDes=bar_designator,
+                    navigation_area="in_front_of",
+                    objectIDsDes=objects_list_des),
+                transitions={"done": "INSPECT_FALLBACK",  # ToDo: transition to NAVIGATE_TO_ROOM
+                             "failed": "INSPECT_FALLBACK"}
+            )
+
+            # Inspect fallback - ask the bartender which drink is unavailable and store the unavailable drink
+            smach.StateMachine.add(
+                "INSPECT_FALLBACK",
+                AskAvailability(
+                    robot=robot,
+                    unavailable_drink_designator=unav_drink_des.writeable),
+                transitions={"succeeded": "NAVIGATE_TO_ROOM",
+                             "failed": "NAVIGATE_TO_ROOM"},
+            )
+
+            # Navigate to the predefined room
+            smach.StateMachine.add(
+                "NAVIGATE_TO_ROOM",
+                states.NavigateToRoom(
+                    robot=robot,
+                    entity_designator_room=room_designator),
+                transitions={"arrived": "succeeded",
+                             "unreachable": "failed",
+                             "goal_not_defined": "aborted"}
+            )

challenge_serving_drinks/src/challenge_serving_drinks/get_order.py

@@ -0,0 +1,148 @@
+# ROS
+import smach
+
+# TU/e Robotics
+import robot_smach_states as states
+import robot_smach_states.util.designators as ds
+
+from robocup_knowledge import knowledge_loader
+from robot_skills.robot import Robot
+
+# Serving drinks
+from .sd_states import AskDrink, DetectWaving
+
+# Knowledge
+COMMON_KNOWLEDGE = knowledge_loader.load_knowledge("common")
+
+
+class GetOrder(smach.StateMachine):
+    """ Gets an order. If succeeded, the person_designator and drink_designator are filled and can be used in subsequent
+    states.
+
+    """
+    def __init__(self, robot, operator_name, drink_designator,
+                 available_drinks_designator, unavailable_drink_designator):
+        # type: (Robot, str, VariableDesignator) -> None
+        """ Initialization method
+
+        :param robot: robot api object
+        :param operator_name: name with which the operator will be stored in image recognition module
+        :param drink_designator: (VariableDesignator) in which the drink to fetch is stored.
+        """
+        smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"])
+
+        with self:
+
+            # Operator id
+            caller_id = "operator"
+            caller_designator = ds.EdEntityDesignator(robot=robot, id=caller_id, name='caller_des')
+
+            # Detect - people holding drinks and people without drinks  #ToDo: implement!
+
+            # Detect fallback - detect waving people
+            smach.StateMachine.add(
+                "ASK_FOR_WAVING",
+                states.Say(
+                    robot=robot,
+                    sentence="Looks like everyone has a drink",
+                    look_at_standing_person=True),
+                transitions={"spoken": "ASK_STEP_IN_FRONT"}  # ToDo: transition to WAIT_FOR_WAVING
+            )
+
+            smach.StateMachine.add(
+                "WAIT_FOR_WAVING",
+                DetectWaving(
+                    robot=robot,
+                    caller_id=caller_id),
+                transitions={'succeeded': 'SAY_I_HAVE_SEEN',
+                             'aborted': 'ASK_STEP_IN_FRONT'}
+            )
+
+            smach.StateMachine.add(
+                'SAY_I_HAVE_SEEN',
+                states.Say(
+                    robot=robot,
+                    sentence='I have seen a waving person, I will be there shortly!',
+                    look_at_standing_person=True),
+                transitions={"spoken": 'NAVIGATE_TO_WAVING'}
+            )
+
+            # Navigate to waving people
+            smach.StateMachine.add(
+                'NAVIGATE_TO_WAVING',
+                states.NavigateToObserve(
+                    robot=robot,
+                    entity_designator=caller_designator,
+                    radius=1.1),
+                transitions={'arrived': 'LEARN_NAME',
+                             'unreachable': 'ASK_STEP_IN_FRONT',
+                             'goal_not_defined': 'WAIT_FOR_WAVING'}
+            )
+
+            # Detect waving people fallback - ask operator in front
+            smach.StateMachine.add(
+                "ASK_STEP_IN_FRONT",
+                states.Say(
+                    robot=robot,
+                    sentence="Please step in front of me to give your order",
+                    look_at_standing_person=True),
+                transitions={"spoken": "LEARN_NAME"}
+            )
+
+            # Ask operator for his name
+            smach.StateMachine.add(
+                "LEARN_NAME",
+                states.AskPersonName(
+                    robot=robot,
+                    person_name_des=operator_name.writeable,
+                    name_options=COMMON_KNOWLEDGE.names,
+                    default_name='john',
+                    nr_tries=2),
+                transitions={"succeeded": "LEARN_OPERATOR",
+                             "failed": "LEARN_NAME_FALLBACK",
+                             "timeout": "LEARN_NAME_FALLBACK"}
+            )
+
+            # Ask operator for his name fallback
+            smach.StateMachine.add(
+                "LEARN_NAME_FALLBACK",
+                states.Say(
+                    robot=robot,
+                    sentence="Sorry, I did not get your name, I'll just call you john",
+                    look_at_standing_person=True),
+                transitions={"spoken": "LEARN_OPERATOR"}
+            )
+
+            # Learn operator
+            smach.StateMachine.add(
+                "LEARN_OPERATOR",
+                states.LearnPerson(
+                    robot=robot,
+                    name_designator=operator_name,
+                    nr_tries=5),
+                transitions={"succeeded": "ASK_DRINK",
+                             "failed": "LEARN_OPERATOR_FALLBACK"}
+            )
+
+            # Learn operator fallback
+            smach.StateMachine.add(
+                "LEARN_OPERATOR_FALLBACK",
+                states.Say(
+                    robot=robot,
+                    sentence="Something went wrong but I will call you by name when I'm back",
+                    look_at_standing_person=True),
+                transitions={"spoken": "ASK_DRINK"}
+            )
+
+            # Ask for preferred beverage
+            smach.StateMachine.add(
+                "ASK_DRINK",
+                AskDrink(
+                    robot=robot,
+                    operator_name=operator_name,
+                    drink_designator=drink_designator.writeable,
+                    available_drinks_designator=available_drinks_designator,
+                    unavailable_drink_designator=unavailable_drink_designator),
+                transitions={"succeeded": "succeeded",
+                             "failed": "failed"},
+            )