Storing groceries update

challenge_storing_groceries/src/challenge_storing_groceries/inspect_shelves.py

@@ -1,3 +1,5 @@
+from itertools import chain
+
 # ROS
 import rospy
 import smach
@@ -6,7 +8,7 @@
 from ed.entity import Entity
 
 from robot_smach_states.navigation import NavigateToSymbolic, NavigateToObserve
-from robot_smach_states.util.designators import Designator, VariableDesignator, check_type
+from robot_smach_states.util.designators import Designator, VariableDesignator, check_resolve_type, check_type
 from robot_smach_states.world_model import SegmentObjects
 from robot_smach_states.designator_iterator import IterateDesignator
 from robot_smach_states.rise import RiseForInspect
@@ -57,7 +59,7 @@ def __init__(self, robot, entityDes, objectIDsDes=None, roomDes=None, searchArea
 
         check_type(entityDes, Entity)
 
-        if not objectIDsDes:
+        if objectIDsDes is None:
             objectIDsDes = VariableDesignator([], resolve_type=[ClassificationResult])
 
         if searchAreas:
@@ -71,6 +73,8 @@ def __init__(self, robot, entityDes, objectIDsDes=None, roomDes=None, searchArea
         rospy.loginfo("searchAreas: {}".format(searchAreas.resolve()))
         searchArea = VariableDesignator(resolve_type=str)
 
+        found_object_ids = VariableDesignator([], resolve_type=[ClassificationResult])
+
         with self:
             if navigation_area:
                 smach.StateMachine.add('NAVIGATE_TO_INSPECT', NavigateToSymbolic(robot, {entityDes: navigation_area},
@@ -96,10 +100,29 @@ def __init__(self, robot, entityDes, objectIDsDes=None, roomDes=None, searchArea
                                                 'failed': 'SEGMENT'})
 
             smach.StateMachine.add('SEGMENT',
-                                   SegmentObjects(robot, objectIDsDes.writeable, entityDes, searchArea,
+                                   SegmentObjects(robot, found_object_ids.writeable, entityDes, searchArea,
                                                   unknown_threshold=unknown_threshold,
                                                   filter_threshold=filter_threshold),
-                                   transitions={'done': 'ITERATE_AREA'})
+                                   transitions={'done': 'APPEND_OBJECT_IDS'})
+
+            @smach.cb_interface(outcomes=["done"])
+            def append_object_ids(_, stored_objects, segmented_objects):
+                """
+                @param stored_objects: is the new list of objects we have seen,
+                @param segmented_objects: the list of the segmented objects which will be appended in the stored_objects
+                """
+                check_resolve_type(stored_objects, [ClassificationResult])
+                check_resolve_type(segmented_objects, [ClassificationResult])
+                objects_dict = {}
+                for item in chain(stored_objects.resolve(), segmented_objects.resolve()):
+                    objects_dict[item.uuid] = item
+                all_objects = list(objects_dict.values())
+                stored_objects.write(all_objects)
+                return "done"
+
+            smach.StateMachine.add("APPEND_OBJECT_IDS",
+                                   smach.CBState(append_object_ids, cb_args=[objectIDsDes.writeable, found_object_ids]),
+                                   transitions={"done": "ITERATE_AREA"})
 
 
 if __name__ == "__main__":

challenge_storing_groceries/src/challenge_storing_groceries/open_door.py

@@ -186,26 +186,23 @@ def execute(self, userdata=None):
 
 
 class OpenDoorMachine(smach.StateMachine):
-    def __init__(self, robot, shelf_designator, cabinet_navigate_area="in_front_of"):
+    def __init__(self, robot, shelf_designator, cabinet_navigate_area: str = "in_front_of", cabinet_inspect_area: str = "on_top_of"):
         smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"])
 
-        self.shelfDes = shelf_designator
-        cabinet_inspect_area = "Todo"
-
         with self:
             smach.StateMachine.add("NAVIGATE_TO_CABINET",
-                                   states.navigation.NavigateToSymbolic(robot, {self.shelfDes: cabinet_navigate_area}, self.shelfDes),
+                                   states.navigation.NavigateToSymbolic(robot, {shelf_designator: cabinet_navigate_area}, shelf_designator),
                                    transitions={'arrived': 'UPDATE_CABINET_POSE',
                                                 'unreachable': 'failed',
                                                 'goal_not_defined': 'failed'})
 
             smach.StateMachine.add("UPDATE_CABINET_POSE",
-                                   UpdateCabinetPose(robot, self.shelfDes, cabinet_inspect_area),
+                                   UpdateCabinetPose(robot, shelf_designator, cabinet_inspect_area),
                                    transitions={'succeeded': 'OPEN_DOOR',
                                                 'failed': 'failed'})
 
             smach.StateMachine.add("OPEN_DOOR",
-                                   OpenDoor(robot, self.shelfDes),
+                                   OpenDoor(robot, shelf_designator),
                                    transitions={'succeeded': 'succeeded',
                                                 'failed': 'failed'})
 

challenge_storing_groceries/src/challenge_storing_groceries/storing_groceries.py

@@ -23,11 +23,12 @@ def setup_statemachine(robot):
     state_machine = smach.StateMachine(outcomes=['Done', 'Failed', 'Aborted'])
 
     skip_door = rospy.get_param("~skip_door", False)
-    skip_inspect = rospy.get_param("~skip inspect", False)
-    shelfDes = ds.EntityByIdDesignator(robot, uuid=challenge_knowledge.shelf)
-    tableDes = ds.EntityByIdDesignator(robot, uuid=challenge_knowledge.table)
-    objectsDes = ds.VariableDesignator(resolve_type=[ClassificationResult])
-    roomDes = ds.EntityByIdDesignator(robot, challenge_knowledge.room, name="room_designator")
+    skip_inspect = rospy.get_param("~skip_inspect", False)
+    shelf_des = ds.EntityByIdDesignator(robot, uuid=challenge_knowledge.shelf)
+    shelf_room_des = ds.EntityByIdDesignator(robot, challenge_knowledge.shelf_room, name="shelf_room_designator")
+    table_des = ds.EntityByIdDesignator(robot, uuid=challenge_knowledge.table)
+    objects_des = ds.VariableDesignator([], resolve_type=[ClassificationResult])
+    table_room_des = ds.EntityByIdDesignator(robot, challenge_knowledge.table_room, name="table_room_designator")
 
     with state_machine:
 
@@ -44,7 +45,7 @@ def setup_statemachine(robot):
 
         # open the door of the cabinet
         smach.StateMachine.add("OPEN_DOOR",
-                               OpenDoorMachine(robot, shelfDes),
+                               OpenDoorMachine(robot, shelf_des, cabinet_inspect_area=challenge_knowledge.cabinet_inspect_area),
                                transitions={'succeeded': 'SAY_CLOSE_DOOR',
                                             'failed': 'SAY_UNABLE_TO_OPEN_DOOR'})
 
@@ -61,8 +62,8 @@ def setup_statemachine(robot):
         # Inspect shelf
         smach.StateMachine.add("NAV_TO_START",
                                states.navigation.NavigateToSymbolic(robot,
-                                                                    {shelfDes: "in_front_of"},
-                                                                    shelfDes, room=roomDes),
+                                                                    {shelf_des: "in_front_of"},
+                                                                    shelf_des, room=shelf_room_des),
                                transitions={'arrived': 'SKIP_INSPECT',
                                             'unreachable': 'SKIP_INSPECT',
                                             'goal_not_defined': 'SKIP_INSPECT'})
@@ -73,7 +74,7 @@ def setup_statemachine(robot):
                                             'false': 'INSPECT_SHELVES'})
 
         smach.StateMachine.add("INSPECT_SHELVES",
-                               InspectAreas(robot, shelfDes, objectsDes, roomDes, knowledge=challenge_knowledge,
+                               InspectAreas(robot, shelf_des, objects_des, shelf_room_des, knowledge=challenge_knowledge,
                                             navigation_area='in_front_of'),
                                transitions={'done': 'RESET_ARM',
                                             'failed': 'Failed'})
@@ -90,7 +91,7 @@ def reset_arm(userdata=None):
 
         # store items
         smach.StateMachine.add("STORE_GROCERIES",
-                               StoreItems(robot, tableDes, shelfDes, objectsDes, challenge_knowledge, room=roomDes),
+                               StoreItems(robot, table_des, shelf_des, objects_des, challenge_knowledge, room=table_room_des),
                                transitions={'succeeded': 'AT_END',
                                             'preempted': 'Aborted',
                                             'failed': 'Failed'}

robocup_knowledge/src/robocup_knowledge/environments/impuls/challenge_storing_groceries.py

@@ -9,11 +9,15 @@
 
 # Detection
 shelf = "closet"
+shelf_room = common.get_room(shelf)
 default_area = "shelf4"
+cabinet_inspect_area = "shelf2"
 inspect_area = "in_front_of"
 object_shelves = ["shelf3", "shelf4", "shelf5"]  # TODO unused variable?
 object_types = [obj["name"] for obj in common.objects]  # TODO unused variable?
+place_areas = ["shelf2", "shelf3"]
 
 # Grasping
 table = "dinner_table"
-room = common.get_room(table)
+table_room = common.get_room(table)
+