(storing_groceries) re-add cabinet_inspect_area

Author: MatthijsBurgh

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

@@ -44,7 +44,7 @@ def setup_statemachine(robot):
 
         # open the door of the cabinet
         smach.StateMachine.add("OPEN_DOOR",
-                               OpenDoorMachine(robot, shelf_des),
+                               OpenDoorMachine(robot, shelf_des, cabinet_inspect_area=challenge_knowledge.cabinet_inspect_area),
                                transitions={'succeeded': 'SAY_CLOSE_DOOR',
                                             'failed': 'SAY_UNABLE_TO_OPEN_DOOR'})
 

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

@@ -10,6 +10,7 @@
 # Detection
 shelf = "closet"
 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?