Merge pull request #892 from tue-robotics/rwc2019_challenge_cleanup

Rwc2019 challenge cleanup

challenge_cleanup/CMakeLists.txt

@@ -1,21 +1,20 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(challenge_cleanup)
 
-
 find_package(catkin REQUIRED COMPONENTS
-  robot_skills
-  robot_smach_states
+    rospy
+    robot_skills
+    robot_smach_states
+)
+
+include_directories(
+    ${catkin_INCLUDE_DIRS}
 )
 
 catkin_python_setup()
 
 catkin_package()
 
-
-###########
-## Build ##
-###########
-
-include_directories(
-  ${catkin_INCLUDE_DIRS}
-)
+if (CATKIN_ENABLE_TESTING)
+  catkin_add_nosetests(test)
+endif()

challenge_cleanup/package.xml

@@ -6,20 +6,13 @@
 
   <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>
+  <depend>robocup_knowledge</depend>
+  <depend>robot_skills</depend>
+  <depend>robot_smach_states</depend>
+  <depend>rospy</depend>
 
-  </export>
 </package>

challenge_cleanup/scripts/challenge_cleanup

@@ -1,9 +1,10 @@
 #!/usr/bin/python
 
-# ROS
 import rospy
+from challenge_cleanup.cleanup import setup_statemachine
+from robot_smach_states.util.startup import startup
 
-# TU/e Robotics
-from robot_skills.util.robot_constructor import robot_constructor
 
-# We need more stuff here???
+if __name__ == "__main__":
+    rospy.init_node("challenge_cleanup")
+    startup(setup_statemachine)

challenge_cleanup/src/challenge_cleanup/clean_inspect.py

@@ -1,92 +1,67 @@
 import smach
 
-import robot_smach_states
-from robot_smach_states.util.designators import EdEntityDesignator, VariableDesignator, EntityByIdDesignator, \
-    Designator, check_type
+import robot_smach_states as states
+from robot_smach_states.util.designators import EdEntityDesignator, VariableDesignator, ValueByKeyDesignator
 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
+        navigation_area_des = ValueByKeyDesignator(location_des, key='navigation_area', resolve_type=str,
+                                                   name='navigation_area')
+        location_id_des = ValueByKeyDesignator(location_des, key='name', resolve_type=str, name='location_id')
+
+        room_des = EdEntityDesignator(robot, id_designator=ValueByKeyDesignator(location_des, key="room",
+                                                                                resolve_type=str))
+
         # 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')
+        e_des = EdEntityDesignator(robot, id_designator=location_id_des, name='e_des')
 
         with self:
             smach.StateMachine.add('ITERATE_NEXT_AREA',
-                                    robot_smach_states.IterateDesignator(segment_areas, segment_area.writeable),
-                                    transitions={"next": 'INSPECT',
-                                                 "stop_iteration": "done"})
-
+                                   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"})
+                                   states.NavigateToSymbolic(robot, {e_des: navigation_area_des}, 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"})
+                                   states.Say(robot, "Looking {area} the {entity}", area=segment_area,
+                                              entity=location_id_des, block=False),
+                                   transitions={"spoken": "RISE"})
 
-            smach.StateMachine.add('SEGMENT', robot_smach_states.SegmentObjects(robot,
-                                                                                e_classifications_des.writeable,
-                                                                                e_des,
-                                                                                segment_area),
-                                    transitions={'done': "HANDLE_DETECTED_ENTITIES"})
+            smach.StateMachine.add('RISE', states.RiseForInspect(robot, e_des, segment_area),
+                                   transitions={'succeeded': 'SEGMENT',
+                                                'failed': 'SEGMENT'})
 
-            # 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('SEGMENT', states.SegmentObjects(robot, e_classifications_des.writeable, e_des,
+                                                                    segment_area),
+                                   transitions={'done': "HANDLE_DETECTED_ENTITIES"})
 
-            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("SAY_UNREACHABLE", states.Say(robot, "I failed to inspect the {furn}",
+                                                                 furn=location_id_des, block=True),
+                                   transitions={"spoken": "done"})
 
             smach.StateMachine.add("HANDLE_DETECTED_ENTITIES",
-                                    HandleDetectedEntities(robot, e_classifications_des, location_id, segment_area),
-                                    transitions={"done": "done"})
+                                   HandleDetectedEntities(robot, e_classifications_des, location_id_des, segment_area,
+                                                          room_des),
+                                   transitions={"done": "ITERATE_NEXT_AREA"})