diff --git a/challenge_storing_groceries/src/challenge_storing_groceries/inspect_shelves.py b/challenge_storing_groceries/src/challenge_storing_groceries/inspect_shelves.py index 63b30758a..2eacd33bf 100644 --- a/challenge_storing_groceries/src/challenge_storing_groceries/inspect_shelves.py +++ b/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__": diff --git a/challenge_storing_groceries/src/challenge_storing_groceries/open_door.py b/challenge_storing_groceries/src/challenge_storing_groceries/open_door.py index 363712d50..5a96f1266 100644 --- a/challenge_storing_groceries/src/challenge_storing_groceries/open_door.py +++ b/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'}) diff --git a/challenge_storing_groceries/src/challenge_storing_groceries/storing_groceries.py b/challenge_storing_groceries/src/challenge_storing_groceries/storing_groceries.py index fa4d3ba7c..af2f8aa5e 100644 --- a/challenge_storing_groceries/src/challenge_storing_groceries/storing_groceries.py +++ b/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'} diff --git a/robocup_knowledge/src/robocup_knowledge/environments/impuls/challenge_storing_groceries.py b/robocup_knowledge/src/robocup_knowledge/environments/impuls/challenge_storing_groceries.py index 024920a0f..453d1de24 100644 --- a/robocup_knowledge/src/robocup_knowledge/environments/impuls/challenge_storing_groceries.py +++ b/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) +