Merge pull request #834 from tue-robotics/rgo2019_challenge_where_is_…

…this

Rgo2019 challenge where is this

challenge_where_is_this/CMakeLists.txt

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

challenge_where_is_this/README.md

@@ -0,0 +1,23 @@
+# Where is this?
+
+Responsible: Matthijs
+
+The robot has to explain and show people where they can find places in the arena (e.g. Where is the TV?).
+The robot has to tell the operator how to get there (de.
+
+## Startup
+
+1. Place the robot near the appointed position (determined by TC on the spot)
+2. Ask who ever stands in front of the robot where they woulf like to be guided to.
+3. Explain, in a summarized fashion, how to get there
+4. Then physically take the operator there in a type of tour guide
+
+amigo1/sergio1/hero1:
+
+    hero-start
+
+amigo2/sergio2/hero2:
+
+    hero-challenge-where-is-this
+
+# Notes

challenge_where_is_this/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<package>
+  <name>challenge_where_is_this</name>
+  <version>0.0.1</version>
+  <description>challenge_where_is_this</description>
+
+  <maintainer email="MatthijsBurgh@outlook.com">Matthijs van der Burgh</maintainer>
+
+  <license>BSD</license>
+
+  <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>rospy</run_depend>
+
+</package>

challenge_where_is_this/scripts/challenge_where_is_this

@@ -0,0 +1,11 @@
+#!/usr/bin/python
+
+import rospy
+from robot_smach_states.util.startup import startup
+from challenge_where_is_this.where_is_this import WhereIsThis
+
+
+if __name__ == '__main__':
+    rospy.init_node('where_is_this')
+
+    startup(WhereIsThis)

challenge_where_is_this/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_where_is_this'],
+    package_dir={'': 'src'}
+)
+
+setup(**d)

challenge_where_is_this/src/challenge_where_is_this/__init__.py

@@ -0,0 +1 @@
+

challenge_where_is_this/src/challenge_where_is_this/inform_machine.py

@@ -0,0 +1,133 @@
+import random
+
+# ROS
+import rospy
+import smach
+
+# TU/e
+import robot_skills
+import robot_smach_states as states
+from robot_smach_states.human_interaction.give_directions import GiveDirections
+import robot_smach_states.util.designators as ds
+
+from robocup_knowledge import load_knowledge
+
+from hmi import HMIResult
+
+
+knowledge = load_knowledge("challenge_where_is_this")
+
+
+class EntityFromHmiResults(ds.Designator):
+    """ Designator to pick the closest item on top of the table to grab. This is used for testing
+
+    """
+    def __init__(self, robot, hmi_result_des, parse=True):
+        """ Constructor
+
+        :param robot: robot object
+        :param hmi_result_des:
+        """
+        super(EntityFromHmiResults, self).__init__(resolve_type=robot_skills.util.entity.Entity)
+
+        self._robot = robot
+        self._hmi_result_des = hmi_result_des
+        self.parse = parse
+
+    def _resolve(self):
+        """ Resolves
+
+        :return: entity in the <area_description> of the <surface_designator> that is closest to the robot
+        """
+        sem = self._hmi_result_des.resolve().semantics
+        entity_id = sem["target-location"]["id"]
+
+        entities = self._robot.ed.get_entities(id=entity_id, parse=self.parse)
+        if entities:
+            return entities[0]
+        else:
+            return None
+
+
+class SayWaitDes(smach.StateMachine):
+    def __init__(self, robot, entity_des):
+        """ Constructor
+        :param robot: robot object
+        """
+        smach.StateMachine.__init__(self, outcomes=["succeeded"])
+
+        with self:
+            self.text_des = ds.VariableDesignator(resolve_type=str).writeable
+
+            @smach.cb_interface(outcomes=["succeeded"])
+            def get_text(userdata=None):
+                entity = entity_des.resolve()
+                entity_id = None
+                if entity:
+                    entity_id = entity.id
+                if entity_id:
+                    text = ["Let me think how to get to the {}".format(entity_id),
+                            "I will now determine the best route to the {}".format(entity_id)]
+                else:
+                    text = ["Let me think how to get to there", "I will now determine the best route"]
+
+                self.text_des.write(random.choice(text))
+                return "succeeded"
+
+            smach.StateMachine.add("GET_TEXT",
+                                   smach.CBState(get_text),
+                                   transitions={'succeeded': 'SAY_TEXT'})
+
+            smach.StateMachine.add("SAY_TEXT",
+                                   states.Say(robot, self.text_des, block=True),
+                                   transitions={'spoken': 'succeeded'})
+
+
+class InformMachine(smach.StateMachine):
+    def __init__(self, robot):
+        """ Constructor
+        :param robot: robot object
+        """
+        smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"])
+
+        with self:
+            self.spec_des = ds.Designator(knowledge.location_grammar)
+            self.answer_des = ds.VariableDesignator(resolve_type=HMIResult)
+            self.entity_des = EntityFromHmiResults(robot, self.answer_des)
+
+            smach.StateMachine.add("ANNOUNCE_ITEM",
+                                   states.Say(robot, "Hello, my name is {}. Please call me by my name. Talk loudly into my microphone and wait for the ping".
+                                              format(robot.robot_name), block=True),
+                                   transitions={'spoken': 'WAIT_TO_BE_CALLED'})
+
+            smach.StateMachine.add('WAIT_TO_BE_CALLED',
+                                   states.HearOptions(robot, ["{}".format(robot.robot_name)], rospy.Duration(10)),
+                                   transitions={'{}'.format(robot.robot_name): "INSTRUCT",
+                                                'no_result': 'ANNOUNCE_ITEM'})
+
+            smach.StateMachine.add("INSTRUCT",
+                                   states.Say(robot,
+                                              ["Please tell me where you would like to go. Talk loudly into my microphone and wait for the ping",
+                                               "Where do you want to go? Talk loudly into my microphone and wait for the ping"]
+                                              , block=True),
+                                   transitions={'spoken': 'LISTEN_FOR_LOCATION'})
+
+            smach.StateMachine.add('LISTEN_FOR_LOCATION',
+                                   states.HearOptionsExtra(robot, self.spec_des, self.answer_des.writeable, rospy.Duration(15)),
+                                   transitions={'heard': "INSTRUCT_FOR_WAIT",
+                                                'no_result': 'failed'})
+
+            smach.StateMachine.add("INSTRUCT_FOR_WAIT",
+                                   SayWaitDes(robot, self.entity_des),
+                                   transitions={'succeeded': 'GIVE_DIRECTIONS'})
+
+            smach.StateMachine.add('GIVE_DIRECTIONS',
+                                   GiveDirections(robot, self.entity_des),
+                                   transitions={'succeeded': "SUCCESS",
+                                                'failed': 'failed'})
+
+            smach.StateMachine.add("SUCCESS",
+                                   states.Say(robot,
+                                              ["Good luck with finding your way"]
+                                              , block=True),
+                                   transitions={'spoken': 'succeeded'})

challenge_where_is_this/src/challenge_where_is_this/where_is_this.py

@@ -0,0 +1,63 @@
+#!/usr/bin/python
+
+# 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 robot_skills.util.kdl_conversions import FrameStamped
+
+# Challenge storing groceries
+from inform_machine import InformMachine
+
+
+class WhereIsThis(smach.StateMachine):
+    def __init__(self, robot):
+        smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
+
+        with self:
+            single_item = InformMachine(robot)
+
+            smach.StateMachine.add('INITIALIZE',
+                                   states.Initialize(robot),
+                                   transitions={'initialized': 'STORE_STARTING_POSE',
+                                                'abort': 'Aborted'})
+
+            @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")
+
+                return "succeeded"
+
+            smach.StateMachine.add("STORE_STARTING_POSE",
+                                   smach.CBState(store_pose),
+                                   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
+                                            input_keys=[], output_keys=[],
+                                            it=lambda: range(1000),
+                                            it_label='index',
+                                            exhausted_outcome='succeeded')
+
+            with range_iterator:
+                smach.Iterator.set_contained_state('SINGLE_ITEM',
+                                                   single_item,
+                                                   loop_outcomes=['succeeded', 'failed'])
+
+            smach.StateMachine.add('RANGE_ITERATOR', range_iterator,
+                                   {'succeeded': 'AT_END',
+                                    'failed': 'Aborted'})
+            # End setup iterator
+
+            smach.StateMachine.add('AT_END',
+                                   states.Say(robot, "Goodbye"),
+                                   transitions={'spoken': 'Done'})
+