update knowledge to correctly be able to plan after the NavToCabinet state

Author: IasonTheodorou

challenge_storing_groceries/src/challenge_storing_groceries/storing_groceries.py

@@ -25,9 +25,10 @@ def setup_statemachine(robot):
     skip_door = rospy.get_param("~skip_door", False)
     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])
-    room_des = ds.EntityByIdDesignator(robot, challenge_knowledge.room, name="room_designator")
+    table_room_des = ds.EntityByIdDesignator(robot, challenge_knowledge.table_room, name="table_room_designator")
 
     with state_machine:
 
@@ -62,7 +63,7 @@ def setup_statemachine(robot):
         smach.StateMachine.add("NAV_TO_START",
                                states.navigation.NavigateToSymbolic(robot,
                                                                     {shelf_des: "in_front_of"},
-                                                                    shelf_des, room=room_des),
+                                                                    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, shelf_des, objects_des, room_des, 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, table_des, shelf_des, objects_des, challenge_knowledge, room=room_des),
+                               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,12 +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)
+