Merge pull request #851 from tue-robotics/rgo2019_challenge_cleanup2

Rgo2019 challenge cleanup2

challenge_cleanup/CMakeLists.txt

@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(challenge_cleanup)
+
+
+find_package(catkin REQUIRED COMPONENTS
+  robot_skills
+  robot_smach_states
+)
+
+catkin_python_setup()
+
+catkin_package()
+
+
+###########
+## Build ##
+###########
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)

challenge_cleanup/README.md

@@ -0,0 +1,30 @@
+Responsible: Henk
+
+# Start-up
+T.b.d.
+
+# Scenario
+### Clean UP [Housekeeper] challenge.
+
+This challenge is described in the 2019 RoboCup@Home Rulebook / Draft version.
+
+### Main goal
+Upon entrance, the robot requests the operator which room shall be cleaned. All misplaced known objects
+found in this room must be taken to their predefined locations and unknown objects thrown in the trash bin.
+
+Number of objects: 5..10
+
+Time limit: 5 minutes
+
+Objects can be anywhere, including the floor, seats, and on furniture. All objects are visible from
+at least 1.0 m distance (no occlusions) and have the following distributions:
+
+Known objects: Any two regular and two alike objects
+
+Unknown objects: One unknown object at grasping distance (i.e. no decorations)
+
+Reward: 1000 pts (100 pts per object), 
+Bonus: max 500 pts
+
+# Possible commands for the robot:
+T.b.d.

challenge_cleanup/Resolution_info.txt

@@ -0,0 +1,47 @@
+29jan2019 - HvR
+RoboCup@Home Rulebook 2019, p46 - 5.1 Clean Up (Housekeeper)
+
+Simplified Challenge description:
+1. Start
+2. Wait for info about the requested room to 'clean'
+3. Move to the room
+4. Visit each piece of furniture, seat, (including the floor!) to look for an object
+5. Segment each camera snapshot and distinghuish between known and unknown objects. ('known' and 'unknown' depend on the
+    set recognition threshold.
+6. Keep a list of the 'known' objects
+7. After all locations have been examined, move to the starting position in the room
+8. Recite the list of objects frm the 'known-object' list
+9. Leave the arena
+10. End
+
+Needed challenge_knowledge:
+1. Layout of all the possible rooms
+2. For each room: Some sort of shortest route to search the room furniture and the floor
+3. Object knowledge (category/name) and required placement location for the objects
+
+
+
+How to make challenge_knowledge?
+- Arena layout (floorplan)                  (In robocup_knowledge-> environments-> <environment> -> common.py)
+- Furniture objects with 'on_top_of' areas  (In ed_object_models-> models-> <environment> -> <object> -> model.yaml)
+- Known objects                             (In cleanup.py)
+- Challenge start/end point location        (In cleanup.py)
+- Waypointlists to get to each room from the start point    (In common.py)
+- Route (waypointlist?) within each room to cover the search area.  (In cleanup.py)
+
+1. Start
+   Load challenge knowledge (CK) from robocup_knowledge
+2. Wait for info about the requested room to 'clean'
+   Ask "Which room needs cleaning?"
+   Loop until operator answer is understood
+3. Move to the room via waypoints (CK) (robot_smach_states->navigation
+   If needed wait for door to open, or let the robot do it
+4. Loop through the list of waypoints for that room (CK)
+    a. Take snapshot
+    b. Analyse/Segment the snapshot
+    c. Update lists of known/unknown objects (threshold dependant)
+   Got next waypoint
+5. If all waypoints are examined goto the starting point in the room
+6. Report known objects to operator
+7. Wait for 'continue'
+8. Leave the arena area.

challenge_cleanup/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>challenge_cleanup</name>
+  <version>0.0.0</version>
+  <description>The challenge_cleanup package</description>
+
+  <maintainer email="hvrooy@todo.todo">hvrooy</maintainer>
+
+
+  <license>TODO</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>robot_skills</build_depend>
+  <build_depend>robot_smach_states</build_depend>
+  <build_export_depend>robot_skills</build_export_depend>
+  <build_export_depend>robot_smach_states</build_export_depend>
+  <exec_depend>robot_skills</exec_depend>
+  <exec_depend>robot_smach_states</exec_depend>
+
+
+  <export>
+
+  </export>
+</package>

challenge_cleanup/scripts/challenge_cleanup

@@ -0,0 +1,9 @@
+#!/usr/bin/python
+
+# ROS
+import rospy
+
+# TU/e Robotics
+from robot_skills.util.robot_constructor import robot_constructor
+
+# We need more stuff here???

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

challenge_cleanup/src/challenge_cleanup/clean_inspect.py

@@ -0,0 +1,92 @@
+import smach
+
+import robot_smach_states
+from robot_smach_states.util.designators import EdEntityDesignator, VariableDesignator, EntityByIdDesignator, \
+    Designator, check_type
+from handle_detected_entities import HandleDetectedEntities
+from robot_skills.classification_result import ClassificationResult
+
+class ValueByKeyDesignator(Designator):
+    def __init__(self, container, key, resolve_type, name=None):
+        """
+        Get a value from a dictionary by it's key
+        :param container: any object with a __getitem__ method or a designator that resolves to it
+        :param name: Name of the designator for introspection purposes
+        """
+        super(ValueByKeyDesignator, self).__init__(resolve_type=resolve_type, name=name)
+        # TODO: Add type checks to make sure that we can do container[key]
+        # OR container.resolve[key]
+        self._container = container
+        self._key = key
+
+    def _resolve(self):
+        container = self._container.resolve()
+        return container[self._key]
+
+
+class CleanInspect(smach.StateMachine):
+#    def __init__(self, robot, location_id, room_id, navigate_area, segment_areas):
+    def __init__(self, robot, location_des):
+        """
+        Visit all selected locations from the list, and handle the found objects
+        :param location_des is a designator resolving to a dictionary with fields ... TODO
+        """
+
+        smach.StateMachine.__init__(self, outcomes=['done'])
+#new
+        # location = location_des.resolve()
+        # segment_areas = location['segment_areas']
+        # navigate_area = location['navigation_area']
+        # location_id = location['name']
+        # room_id = location['room']
+
+        segment_areas = ValueByKeyDesignator(location_des, "segment_areas", [str], name='segment_areas')
+        segment_area = VariableDesignator(resolve_type=str, name='segment_area')
+
+        navigate_area = ValueByKeyDesignator(location_des, 'navigation_area', str, name='navigate_area')
+        location_id = ValueByKeyDesignator(location_des, 'name', str, name='location_id')
+        room_id = ValueByKeyDesignator(location_des, 'room', str, name='room_id')
+#/new
+        # Set up the designators for this machine
+        e_classifications_des = VariableDesignator([], resolve_type=[ClassificationResult], name='e_classifications_des')
+        e_des = EdEntityDesignator(robot, id=location_id, name='e_des')
+        room_des = EntityByIdDesignator(robot, id=room_id, name='room_des')
+
+        with self:
+            smach.StateMachine.add('ITERATE_NEXT_AREA',
+                                    robot_smach_states.IterateDesignator(segment_areas, segment_area.writeable),
+                                    transitions={"next": 'INSPECT',
+                                                 "stop_iteration": "done"})
+
+
+            # Segment the area and handle segmented objects for the specified navigation area
+            # for i, segment_area in enumerate(segment_areas):
+
+            smach.StateMachine.add('INSPECT',
+                                    robot_smach_states.NavigateToSymbolic(robot, {e_des: navigate_area, room_des: "in"},
+                                                                             e_des),
+                                    transitions={'arrived': 'SEGMENT_SAY',
+                                                 'unreachable': "SAY_UNREACHABLE",
+                                                 'goal_not_defined': "SAY_UNREACHABLE"})
+
+            smach.StateMachine.add("SEGMENT_SAY",
+                                    robot_smach_states.Say(robot, ["Looking %s the %s"
+                                                                      % (segment_area, location_id)], block=False),
+                                    transitions={"spoken": "SEGMENT"})
+
+            smach.StateMachine.add('SEGMENT', robot_smach_states.SegmentObjects(robot,
+                                                                                e_classifications_des.writeable,
+                                                                                e_des,
+                                                                                segment_area),
+                                    transitions={'done': "HANDLE_DETECTED_ENTITIES"})
+
+            # Determine the next state, either it is the next iter or done
+            #next_state = "NAVIGATE_%d" % (i + 1) if i + 1 < len(segment_areas) else "done"
+
+            smach.StateMachine.add("SAY_UNREACHABLE",
+                                    robot_smach_states.Say(robot, ["I failed to inspect the %s" % location_id], block=True),
+                                    transitions={"spoken": "done"})
+
+            smach.StateMachine.add("HANDLE_DETECTED_ENTITIES",
+                                    HandleDetectedEntities(robot, e_classifications_des, location_id, segment_area),
+                                    transitions={"done": "done"})