Merge pull request #962 from tue-robotics/rwc2019_challenge_where_is_…

…this

Rwc2019 challenge where is this

challenge_where_is_this/CMakeLists.txt

@@ -9,3 +9,7 @@ find_package(catkin REQUIRED COMPONENTS
 catkin_python_setup()
 
 catkin_package()
+
+if (CATKIN_ENABLE_TESTING)
+  catkin_add_nosetests(test)
+endif()

challenge_where_is_this/package.xml

@@ -10,11 +10,9 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
-  <build_depend>robot_skills</build_depend>
-  <build_depend>robot_smach_states</build_depend>
-
   <run_depend>robot_skills</run_depend>
   <run_depend>robot_smach_states</run_depend>
+  <run_depend>robocup_knowledge</run_depend>
   <run_depend>rospy</run_depend>
 
 </package>

challenge_where_is_this/scripts/challenge_where_is_this

@@ -1,7 +1,12 @@
 #!/usr/bin/python
 
+# System
 import rospy
+
+# TU/e Robotics
 from robot_smach_states.util.startup import startup
+
+# Challenge where is this
 from challenge_where_is_this.where_is_this import WhereIsThis
 
 

challenge_where_is_this/scripts/console_people_test.py

@@ -0,0 +1,12 @@
+import sys
+from robot_skills.util import kdl_conversions
+from robot_skills import get_robot
+from robot_smach_states.navigation.guidance import _detect_operator_behind_robot
+
+
+robot = get_robot(sys.argv[1])
+
+a = kdl_conversions.VectorStamped(-1, 0, 1, '/{}/base_link'.format(sys.argv[1]))
+hero.head.look_at_point(a)
+_detect_operator_behind_robot(robot)
+

challenge_where_is_this/src/challenge_where_is_this/simulation.py

@@ -0,0 +1,39 @@
+# System
+import os
+
+# ROS
+import rospy
+import std_srvs.srv
+
+IS_SIMULATING = False
+OPERATOR_AVAILABLE = True
+
+
+def _toggle_operator(_):
+    """
+    Toggles if the operator is following
+    :param _:
+    :return:
+    """
+    global OPERATOR_AVAILABLE
+    OPERATOR_AVAILABLE = not OPERATOR_AVAILABLE
+    rospy.loginfo("Operator available: {}".format(OPERATOR_AVAILABLE))
+    return std_srvs.srv.EmptyResponse()
+
+
+def mock_detect_operator(robot, distance=1.0, radius=0.5):  # noinspection
+    """
+    Mocks the 'detect operator' method. Only returns if the mocked operator is available
+    :param robot: -
+    :param distance: -
+    :param radius: -
+    :return: (bool)
+    """
+    assert IS_SIMULATING, "This 'detect_operator' mock should *only* be used *in simulation*"
+    return OPERATOR_AVAILABLE
+
+
+if os.getenv("ROBOT_REAL", "false").lower() != "true":
+    rospy.Service("toggle_operator", std_srvs.srv.Empty, _toggle_operator)
+    rospy.loginfo('\n\nCall:\nrosservice call /toggle_operator "{}"\n\nto toggle operator availability\n'.format('{}'))
+    IS_SIMULATING = True

challenge_where_is_this/src/challenge_where_is_this/where_is_this.py

@@ -1,63 +1,167 @@
-#!/usr/bin/python
+# System
+import math
 
 # ROS
-import rospy
-import PyKDL as kdl
 import smach
 
 # TU/e Robotics
 import robot_smach_states as states
 import robot_smach_states.util.designators as ds
+from hmi import HMIResult
+from robocup_knowledge import load_knowledge
 from robot_skills.util.kdl_conversions import FrameStamped
 
-# Challenge storing groceries
-from inform_machine import InformMachine
+# Challenge where is this
+from .inform_machine import InformMachine
+
+# Load and extract knowledge here so that stuff fails on startup if not defined
+knowledge = load_knowledge("challenge_where_is_this")
+INFORMATION_POINT_ID = knowledge.information_point_id
+INITIAL_POSE_ID = knowledge.initial_pose_id
+START_GRAMMAR = knowledge.starting_point_grammar
+GRAMMAR = knowledge.location_grammar
+
+START_ROBUST = True  # Set this flag to False if you don"t want to use StartChallengeRobust
 
 
 class WhereIsThis(smach.StateMachine):
     def __init__(self, robot):
-        smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
+        smach.StateMachine.__init__(self, outcomes=["Done", "Aborted"])
+
+        hmi_result_des = ds.VariableDesignator(resolve_type=HMIResult)
+        information_point_id_designator = ds.FuncDesignator(
+            ds.AttrDesignator(hmi_result_des, "semantics", resolve_type=unicode), str, resolve_type=str)
+        information_point_designator = ds.EdEntityDesignator(robot, id_designator=information_point_id_designator)
 
         with self:
             single_item = InformMachine(robot)
 
-            smach.StateMachine.add('INITIALIZE',
-                                   states.Initialize(robot),
-                                   transitions={'initialized': 'STORE_STARTING_POSE',
-                                                'abort': 'Aborted'})
+            if START_ROBUST:
+                smach.StateMachine.add("START_CHALLENGE",
+                                       states.StartChallengeRobust(robot, INITIAL_POSE_ID),
+                                       transitions={"Done": "ASK_WHERE_TO_GO",
+                                                    "Aborted": "Aborted",
+                                                    "Failed": "Aborted"})
+
+                smach.StateMachine.add(
+                    "ASK_WHERE_TO_GO",
+                    states.Say(robot, "Near which furniture object should I go to start guiding people?"),
+                    transitions={"spoken": "WAIT_WHERE_TO_GO"})
+
+                smach.StateMachine.add("WAIT_WHERE_TO_GO",
+                                       states.HearOptionsExtra(
+                                           robot=robot,
+                                           spec_designator=ds.Designator(initial_value=START_GRAMMAR),
+                                           speech_result_designator=hmi_result_des.writeable),
+                                       transitions={"heard": "ASK_CONFIRMATION",
+                                                    "no_result": "ASK_WHERE_TO_GO"})  # ToDo: add fallbacks #option: STORE_STARTING_POSE
+
+                smach.StateMachine.add("ASK_CONFIRMATION",
+                                       states.Say(robot, ["I hear that you would like me to start the tours at"
+                                                                   " the {place}, is this correct?"],
+                                                           place=information_point_id_designator,
+                                                           block=True),
+                                       transitions={"spoken": "CONFIRM_LOCATION"})
+
+                smach.StateMachine.add("CONFIRM_LOCATION",
+                                       states.HearOptions(robot=robot, options=["yes", "no"]),
+                                       transitions={"yes": "MOVE_OUT_OF_MY_WAY",
+                                                    "no": "ASK_WHERE_TO_GO",
+                                                    "no_result": "ASK_WHERE_TO_GO"})
+
+                smach.StateMachine.add("MOVE_OUT_OF_MY_WAY",
+                                       states.Say(robot, "Please move your ass so I can get going!"),
+                                       transitions={"spoken": "TC_MOVE_TIME"})
+
+                smach.StateMachine.add("TC_MOVE_TIME",
+                                       states.WaitTime(robot=robot, waittime=3),
+                                       transitions={"waited": "NAV_TO_START",
+                                                    "preempted": "Aborted"}
+                                       )
 
+                smach.StateMachine.add("NAV_TO_START",
+                                       states.NavigateToSymbolic(
+                                           robot=robot,
+                                           entity_designator_area_name_map={
+                                               information_point_designator: "in_front_of"
+                                           },
+                                           entity_lookat_designator=information_point_designator
+                                       ),
+                                       transitions={"arrived": "TURN_AROUND",
+                                                    "unreachable": "WAIT_NAV_BACKUP",
+                                                    "goal_not_defined": "Aborted"})  # If this happens: never mind
+
+                smach.StateMachine.add("WAIT_NAV_BACKUP",
+                                       states.WaitTime(robot, 3.0),
+                                       transitions={"waited": "NAV_TO_START_BACKUP",
+                                                    "preempted": "Aborted"})
+
+                smach.StateMachine.add("NAV_TO_START_BACKUP",
+                                       states.NavigateToSymbolic(
+                                           robot=robot,
+                                           entity_designator_area_name_map={information_point_designator: "near"},
+                                           entity_lookat_designator=information_point_designator
+                                       ),
+                                       transitions={"arrived": "TURN_AROUND",
+                                                    "unreachable": "SAY_CANNOT_REACH_WAYPOINT",  # Current pose backup
+                                                    "goal_not_defined": "Aborted"})  # If this happens: never mind
+
+                @smach.cb_interface(outcomes=["done"])
+                def _turn_around(userdata=None):
+                    """ Turns the robot approximately 180 degrees around """
+                    v_th = 0.5
+                    robot.base.force_drive(vx=0.0, vy=0.0, vth=v_th, timeout=math.pi / v_th)
+                    return "done"
+
+                smach.StateMachine.add("TURN_AROUND",
+                                       smach.CBState(_turn_around),
+                                       transitions={"done": "STORE_STARTING_POSE"})
+
+                smach.StateMachine.add("SAY_CANNOT_REACH_WAYPOINT",
+                                       states.Say(robot, "I am not able to reach the starting point."
+                                                         "I'll use this as starting point"),
+                                       transitions={"spoken": "STORE_STARTING_POSE"})
+            else:
+                smach.StateMachine.add("INITIALIZE",
+                                       states.Initialize(robot),
+                                       transitions={"initialized": "STORE_STARTING_POSE",
+                                                    "abort": "Aborted"})
+
+
+            ## This is purely for a back up scenario until the range iterator
             @smach.cb_interface(outcomes=["succeeded"])
             def store_pose(userdata=None):
                 base_loc = robot.base.get_location()
                 base_pose = base_loc.frame
-                location_id = "starting_point"
-                robot.ed.update_entity(id=location_id, frame_stamped=FrameStamped(base_pose, "/map"), type="waypoint")
+                location_id = INFORMATION_POINT_ID
+                robot.ed.update_entity(id=location_id,
+                                       frame_stamped=FrameStamped(base_pose, "/map"),
+                                       type="waypoint")
 
                 return "succeeded"
 
             smach.StateMachine.add("STORE_STARTING_POSE",
                                    smach.CBState(store_pose),
-                                   transitions={'succeeded': 'RANGE_ITERATOR'})
+                                   transitions={"succeeded": "RANGE_ITERATOR"})
 
             # Begin setup iterator
             # The exhausted argument should be set to the prefered state machine outcome
-            range_iterator = smach.Iterator(outcomes=['succeeded', 'failed'],  # Outcomes of the iterator state
+            range_iterator = smach.Iterator(outcomes=["succeeded", "failed"],  # Outcomes of the iterator state
                                             input_keys=[], output_keys=[],
                                             it=lambda: range(1000),
-                                            it_label='index',
-                                            exhausted_outcome='succeeded')
+                                            it_label="index",
+                                            exhausted_outcome="succeeded")
 
             with range_iterator:
-                smach.Iterator.set_contained_state('SINGLE_ITEM',
+                smach.Iterator.set_contained_state("SINGLE_ITEM",
                                                    single_item,
-                                                   loop_outcomes=['succeeded', 'failed'])
+                                                   loop_outcomes=["succeeded", "failed"])
 
-            smach.StateMachine.add('RANGE_ITERATOR', range_iterator,
-                                   {'succeeded': 'AT_END',
-                                    'failed': 'Aborted'})
+            smach.StateMachine.add("RANGE_ITERATOR", range_iterator,
+                                   {"succeeded": "AT_END",
+                                    "failed": "Aborted"})
             # End setup iterator
 
-            smach.StateMachine.add('AT_END',
+            smach.StateMachine.add("AT_END",
                                    states.Say(robot, "Goodbye"),
-                                   transitions={'spoken': 'Done'})
-
+                                   transitions={"spoken": "Done"})

challenge_where_is_this/test/test_construction.py

@@ -0,0 +1,18 @@
+import os
+import unittest
+
+
+class TestChallengeConstruction(unittest.TestCase):
+    def test_construction(self):
+        """
+        If no exception is raised, this test will succeed
+        """
+        os.environ["ROBOT_ENV"] = "robotics_testlabs"
+        from robot_skills.mockbot import Mockbot
+        from challenge_where_is_this.where_is_this import WhereIsThis
+        robot = Mockbot()
+        WhereIsThis(robot)
+
+
+if __name__ == '__main__':
+    unittest.main()

robocup_knowledge/src/robocup_knowledge/environments/robotics_testlabs/challenge_where_is_this.py

@@ -1,24 +1,45 @@
 # WHERE IS THIS KNOWLEDGE FILE ROBOTICS TESTLABS
 
+# System
+from collections import namedtuple
+
 # TU/e Robotics
 from robocup_knowledge import knowledge_loader
 
+BackupScenario = namedtuple("BackupScenario", ["entity_id", "sentence"])
+
+backup_scenarios = [
+    BackupScenario("fridge", "I will take you to the fridge for a cold beer"),
+    BackupScenario("bed", "You look tired, I will take you to the bed"),
+    BackupScenario("desk", "I'll have you sit down at the desk so you can work on my speech recognition skills"),
+]
+
+information_point_id = "where_is_this_information_point"
+initial_pose_id = "initial_pose"
+
 # Common knowledge
 common = knowledge_loader.load_knowledge("common")
 
 grammar_target = "T"
 
+starting_point_grammar = """
+T[A] -> LOCATION[A]
+"""
+
+for loc in common.location_names:
+    starting_point_grammar += '\nLOCATION[%s] -> %s' % (loc, loc)
+
 location_grammar = """
 T[A] -> COURTESY_PREFIX VP[A] | VP[A]
 
 COURTESY_PREFIX -> robot please | could you | could you please | please
 """
 
 for loc in common.location_rooms:
-    location_grammar += '\nLOCATION[{"id": "%s"}] -> %s' % (loc, loc)
+    location_grammar += '\nLOCATION[%s] -> %s' % (loc, loc)
 
 for loc in common.location_names:
-    location_grammar += '\nLOCATION[{"id": "%s"}] -> %s' % (loc, loc)
+    location_grammar += '\nLOCATION[%s] -> %s' % (loc, loc)
 
 for loc in common.object_names:
     category = common.get_object_category(loc)
@@ -27,14 +48,14 @@
 
     entity_id = common.get_object_category_location(category)[0]
 
-    location_grammar += '\nLOCATION[{"id": "%s"}] -> %s' % (entity_id, loc)
+    location_grammar += '\nLOCATION[%s] -> %s' % (entity_id, loc)
 
 location_grammar += '\nDET_LOCATION[Y] -> LOCATION[Y] | the LOCATION[Y]'
 
 location_grammar += """
 V_GUIDE -> guide | bring | lead
 
-VP[{"target-location": Y}] -> DET_LOCATION[Y] | V_GUIDE me to DET_LOCATION[Y] | i want to go to DET_LOCATION[Y] | i would like to go to DET_LOCATION[Y] | i like to go to DET_LOCATION[Y] | tell me how to go to DET_LOCATION[Y] | tell me how to reach DET_LOCATION[Y]
+VP[Y] -> DET_LOCATION[Y] | V_GUIDE me to DET_LOCATION[Y] | i want to go to DET_LOCATION[Y] | i would like to go to DET_LOCATION[Y] | i like to go to DET_LOCATION[Y] | tell me how to go to DET_LOCATION[Y] | tell me how to reach DET_LOCATION[Y]
 
 """
 
@@ -43,6 +64,22 @@
 
     from grammar_parser.cfgparser import CFGParser
 
+    print("Location Grammar")
     grammar_parser = CFGParser.fromstring(location_grammar)
     grammar_parser.verify()
     grammar_parser.check_rules()
+    print("Random Sentence:")
+    sentence = grammar_parser.get_random_sentence("T")
+    print(sentence)
+    print("Resulting Semantics:")
+    print(grammar_parser.parse("T", sentence))
+
+    print("\n\nStarting Point Grammar")
+    grammar_parser = CFGParser.fromstring(starting_point_grammar)
+    grammar_parser.verify()
+    grammar_parser.check_rules()
+    print("Random Sentence:")
+    sentence = grammar_parser.get_random_sentence("T")
+    print(sentence)
+    print("Resulting Semantics:")
+    print(grammar_parser.parse("T", sentence))