From 62f72f47f0f67007ba336c14165e6ae70c2f6059 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Mon, 14 Dec 2020 15:56:43 +0100 Subject: [PATCH 01/15] changed gripper interface --- .../src/challenge_dishwasher/custom_place.py | 2 +- .../challenge_dishwasher/open_dishwasher.py | 4 ++-- .../src/challenge_dishwasher/simple_grab.py | 10 +++++----- .../navigate_to_and_grab_rack.py | 2 +- .../src/challenge_presentation/presentation.py | 12 ++++++------ .../presentation_hero.py | 6 +++--- .../manipulate_machine.py | 2 +- .../navigate_to_and_close_cupboard_drawer.py | 4 ++-- .../navigate_to_and_open_cupboard_drawer.py | 4 ++-- ...te_to_and_pick_item_from_cupboard_drawer.py | 2 +- .../navigate_to_and_place_item_on_table.py | 2 +- .../manipulate_machine.py | 2 +- .../drop_down.py | 4 ++-- .../challenge_take_out_the_garbage/pick_up.py | 10 +++++----- .../robot_smach_states/manipulation/grab.py | 14 +++++++------- .../manipulation/manipulation.py | 18 +++++++++--------- .../robot_smach_states/manipulation/place.py | 10 +++++----- 17 files changed, 54 insertions(+), 54 deletions(-) diff --git a/challenge_dishwasher/src/challenge_dishwasher/custom_place.py b/challenge_dishwasher/src/challenge_dishwasher/custom_place.py index 2e0e95440c..4224263214 100644 --- a/challenge_dishwasher/src/challenge_dishwasher/custom_place.py +++ b/challenge_dishwasher/src/challenge_dishwasher/custom_place.py @@ -57,7 +57,7 @@ def place_pose(ud): @cb_interface(outcomes=['done']) def open_gripper(ud): - arm.resolve().send_gripper_goal("open", timeout=0) + arm.resolve().gripper.send_goal("open", timeout=0) rospy.sleep(3.0) robot.speech.speak("This is where I usually place my cup") return 'done' diff --git a/challenge_dishwasher/src/challenge_dishwasher/open_dishwasher.py b/challenge_dishwasher/src/challenge_dishwasher/open_dishwasher.py index debb9cc507..e978133867 100644 --- a/challenge_dishwasher/src/challenge_dishwasher/open_dishwasher.py +++ b/challenge_dishwasher/src/challenge_dishwasher/open_dishwasher.py @@ -105,7 +105,7 @@ def __init__(self, robot, dishwasher_id): @cb_interface(outcomes=['done']) def _pre_grab_handle(ud): - robot.rightArm.send_gripper_goal("open", timeout=0) + robot.rightArm.gripper.send_goal("open", timeout=0) robot.rightArm._send_joint_trajectory([[0, 0.2519052373022729913, 0.7746500794619434, 1.3944848321343395, -1.829999276180074, 0.6947045024700284, 0.1889253710114966]], timeout=rospy.Duration(0)) @@ -118,7 +118,7 @@ def _grab_handle(ud): robot.speech.speak('I hope this goes right!', block=False) fs = frame_stamped("dishwasher", 0.42, 0, 0.8, roll=math.pi / 2, pitch=0, yaw=math.pi) robot.rightArm.send_goal(fs.projectToFrame(robot.robot_name + "/base_link", robot.tf_listener)) - robot.rightArm.send_gripper_goal("close") + robot.rightArm.gripper.send_goal("close") return 'done' @cb_interface(outcomes=['done']) diff --git a/challenge_dishwasher/src/challenge_dishwasher/simple_grab.py b/challenge_dishwasher/src/challenge_dishwasher/simple_grab.py index 4fc6cba050..517c487a2d 100644 --- a/challenge_dishwasher/src/challenge_dishwasher/simple_grab.py +++ b/challenge_dishwasher/src/challenge_dishwasher/simple_grab.py @@ -86,13 +86,13 @@ def execute(self, ud): self.robot.speech.speak('I am sorry but I cannot move my arm to the object position', block=False) rospy.logerr('Grasp failed') arm.reset() - arm.send_gripper_goal('close', timeout=0.0) + arm.gripper.send_goal('close', timeout=0.0) return 'failed' # Close gripper - arm.send_gripper_goal('close') + arm.gripper.send_goal('close') - arm.occupied_by = None # TODO: what to do if we don't have an entity + arm.gripper.occupied_by = None # TODO: what to do if we don't have an entity # Lift rospy.loginfo('Start lifting') @@ -142,7 +142,7 @@ def __init__(self, robot, arm): @cb_interface(outcomes=['succeeded']) def prepare_grasp(ud): # Open gripper (non-blocking) - arm.resolve().send_gripper_goal('open', timeout=0) + arm.resolve().gripper.send_goal('open', timeout=0) # Torso up (non-blocking) robot.torso.high() @@ -151,7 +151,7 @@ def prepare_grasp(ud): arm.resolve().send_joint_trajectory('prepare_grasp', timeout=0) # Open gripper - arm.resolve().send_gripper_goal('open', timeout=0.0) + arm.resolve().gripper.send_goal('open', timeout=0.0) return 'succeeded' with self: diff --git a/challenge_final/src/challenge_final/navigate_to_and_grab_rack.py b/challenge_final/src/challenge_final/navigate_to_and_grab_rack.py index 2dac8ae49d..0a0bde067b 100644 --- a/challenge_final/src/challenge_final/navigate_to_and_grab_rack.py +++ b/challenge_final/src/challenge_final/navigate_to_and_grab_rack.py @@ -35,7 +35,7 @@ def send_joint_goal(position_array, wait_for_motion_done=True): arm.wait_for_motion_done() def send_gripper_goal(open_close_string, max_torque=1.0): - arm.send_gripper_goal(open_close_string, max_torque=max_torque) + arm.gripper.send_goal(open_close_string, max_torque=max_torque) rospy.sleep(1.0) # Does not work with motion_done apparently def show_image(package_name, path_to_image_in_package): diff --git a/challenge_presentation/src/challenge_presentation/presentation.py b/challenge_presentation/src/challenge_presentation/presentation.py index f16b43ba64..6631c9dfad 100644 --- a/challenge_presentation/src/challenge_presentation/presentation.py +++ b/challenge_presentation/src/challenge_presentation/presentation.py @@ -110,11 +110,11 @@ def execute(self, userdata=None): if self.right_arm is not None: function_list.append(partial(self.right_arm.send_joint_trajectory, "show_gripper")) if self.left_arm is not None: - function_list.append(partial(self.left_arm.send_gripper_goal, "open")) - function_list.append(partial(self.left_arm.send_gripper_goal, "close")) + function_list.append(partial(self.left_arm.gripper.send_goal, "open")) + function_list.append(partial(self.left_arm.gripper.send_goal, "close")) if self.right_arm is not None: - function_list.append(partial(self.right_arm.send_gripper_goal, "open")) - function_list.append(partial(self.right_arm.send_gripper_goal, "close")) + function_list.append(partial(self.right_arm.gripper.send_goal, "open")) + function_list.append(partial(self.right_arm.gripper.send_goal, "close")) if self.left_arm is not None: function_list.append(partial(self.left_arm.reset)) if self.right_arm is not None: @@ -165,9 +165,9 @@ def execute(self, userdata=None): if self.right_arm is not None: self.right_arm.reset() if self.left_arm is not None: - self.left_arm.send_gripper_goal("close") + self.left_arm.gripper.send_goal("close") if self.right_arm is not None: - self.right_arm.send_gripper_goal("close") + self.right_arm.gripper.send_goal("close") self.robot.torso.reset() self.robot.head.reset() diff --git a/challenge_presentation/src/challenge_presentation/presentation_hero.py b/challenge_presentation/src/challenge_presentation/presentation_hero.py index e49bb59b51..976815a766 100755 --- a/challenge_presentation/src/challenge_presentation/presentation_hero.py +++ b/challenge_presentation/src/challenge_presentation/presentation_hero.py @@ -120,8 +120,8 @@ def execute(self, userdata=None): function_list.append(partial(self.robot.speech.speak, self.trans.GRIPPER, language=self.language, voice=self.voice, block=False)) function_list.append(partial(self.arm.send_joint_goal, "show_gripper")) - function_list.append(partial(self.arm.send_gripper_goal, "open")) - function_list.append(partial(self.arm.send_gripper_goal, "close")) + function_list.append(partial(self.arm.gripper.send_goal, "open")) + function_list.append(partial(self.arm.gripper.send_goal, "close")) function_list.append(partial(self.robot.speech.speak, self.trans.GRIPPER_CAMERA, language=self.language, voice=self.voice, block=True)) function_list.append(partial(self.arm.reset)) @@ -179,7 +179,7 @@ def execute(self, userdata=None): if self.preempt_requested(): self.robot.speech.speak("Sorry, but I have to stop my introduction") self.arm.reset() - self.arm.send_gripper_goal("close") + self.arm.gripper.send_goal("close") self.robot.torso.reset() self.robot.head.reset() self.arm.wait_for_motion_done() diff --git a/challenge_set_a_table/src/challenge_set_a_table_states/manipulate_machine.py b/challenge_set_a_table/src/challenge_set_a_table_states/manipulate_machine.py index 771c1deb25..6ae5c2503a 100644 --- a/challenge_set_a_table/src/challenge_set_a_table_states/manipulate_machine.py +++ b/challenge_set_a_table/src/challenge_set_a_table_states/manipulate_machine.py @@ -136,7 +136,7 @@ def __init__(self, robot, place_designator): def execute(self, userdata=None): # Try to place the object - item = ds.EdEntityDesignator(robot=self._robot, id=arm.occupied_by.id) + item = ds.EdEntityDesignator(robot=self._robot, id=arm.gripper.occupied_by.id) arm_designator = ds.OccupiedArmDesignator(self._robot, arm_properties={ "required_trajectories": ["prepare_place"], diff --git a/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_close_cupboard_drawer.py b/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_close_cupboard_drawer.py index f21904cd6d..94132c8963 100644 --- a/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_close_cupboard_drawer.py +++ b/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_close_cupboard_drawer.py @@ -28,12 +28,12 @@ def send_joint_goal(position_array, wait_for_motion_done=True): arm.wait_for_motion_done() def send_gripper_goal(open_close_string): - arm.send_gripper_goal(open_close_string) + arm.gripper.send_goal(open_close_string) rospy.sleep(1.0) # Does not work with motion_done apparently @cb_interface(outcomes=['done']) def _pre_grab_handle(_): - arm.send_gripper_goal("open", timeout=0) + arm.gripper.send_goal("open", timeout=0) send_joint_goal([0, -0, 0, -1.57, 1.57]) return 'done' diff --git a/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_open_cupboard_drawer.py b/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_open_cupboard_drawer.py index 3df5a720fa..7a232945f8 100644 --- a/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_open_cupboard_drawer.py +++ b/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_open_cupboard_drawer.py @@ -28,12 +28,12 @@ def send_joint_goal(position_array, wait_for_motion_done=True): arm.wait_for_motion_done() def send_gripper_goal(open_close_string): - arm.send_gripper_goal(open_close_string) + arm.gripper.send_goal(open_close_string) rospy.sleep(1.0) # Does not work with motion_done apparently @cb_interface(outcomes=['done']) def _pre_grab_handle(_): - arm.send_gripper_goal("open", timeout=0) + arm.gripper.send_goal("open", timeout=0) send_joint_goal([0, -0, 0, -1.57, 1.57]) return 'done' diff --git a/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_pick_item_from_cupboard_drawer.py b/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_pick_item_from_cupboard_drawer.py index af2882e44b..b1d6e48d44 100644 --- a/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_pick_item_from_cupboard_drawer.py +++ b/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_pick_item_from_cupboard_drawer.py @@ -37,7 +37,7 @@ def send_joint_goal(position_array, wait_for_motion_done=True): arm.wait_for_motion_done() def send_gripper_goal(open_close_string, max_torque=0.1): - arm.send_gripper_goal(open_close_string, max_torque=max_torque) + arm.gripper.send_goal(open_close_string, max_torque=max_torque) rospy.sleep(1.0) # Does not work with motion_done apparently def show_image(package_name, path_to_image_in_package): diff --git a/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_place_item_on_table.py b/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_place_item_on_table.py index cfc343ccaf..13c0bf164e 100644 --- a/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_place_item_on_table.py +++ b/challenge_set_the_table/src/challenge_set_the_table/navigate_to_and_place_item_on_table.py @@ -91,7 +91,7 @@ def send_joint_trajectory(goal_array, wait_for_motion_done=True): arm.wait_for_motion_done() def send_gripper_goal(open_close_string): - arm.send_gripper_goal(open_close_string) + arm.gripper.send_goal(open_close_string) rospy.sleep(1.0) # Does not work with motion_done apparently @cb_interface(outcomes=['done'], input_keys=["item_picked"]) diff --git a/challenge_storing_groceries/src/challenge_storing_groceries/manipulate_machine.py b/challenge_storing_groceries/src/challenge_storing_groceries/manipulate_machine.py index 46281e081f..5d2e8bb25f 100644 --- a/challenge_storing_groceries/src/challenge_storing_groceries/manipulate_machine.py +++ b/challenge_storing_groceries/src/challenge_storing_groceries/manipulate_machine.py @@ -158,7 +158,7 @@ def __init__(self, robot, place_designator): def execute(self, userdata=None): # Try to place the object - item = ds.EdEntityDesignator(robot=self._robot, id=arm.occupied_by.id) + item = ds.EdEntityDesignator(robot=self._robot, id=arm.gripper.occupied_by.id) arm_designator = ds.OccupiedArmDesignator(self._robot, {"required_goals": ["reset", "handover_to_human"], "required_gripper_types": [arms.GripperTypes.GRASPING]}) resolved_arm = arm_designator.resolve() diff --git a/challenge_take_out_the_garbage/src/challenge_take_out_the_garbage/drop_down.py b/challenge_take_out_the_garbage/src/challenge_take_out_the_garbage/drop_down.py index f43fd948b9..85dc878130 100644 --- a/challenge_take_out_the_garbage/src/challenge_take_out_the_garbage/drop_down.py +++ b/challenge_take_out_the_garbage/src/challenge_take_out_the_garbage/drop_down.py @@ -36,7 +36,7 @@ def execute(self, userdata=None): # Arm to position in a safe way arm.send_joint_goal('handover') arm.wait_for_motion_done() - arm.send_gripper_goal('open') + arm.gripper.send_goal('open') arm.wait_for_motion_done() arm._arm._send_joint_trajectory( [[0.4, -1.0, 0.0, -1.0, 0.0],[0.4, -1.0, 0.0, -1.57, 0.0], [0.4, -1.0, 0.0, -1.0, 0.0], @@ -44,7 +44,7 @@ def execute(self, userdata=None): arm.wait_for_motion_done() arm.send_joint_goal('reset') arm.wait_for_motion_done() - # arm.send_gripper_goal('close') + # arm.gripper.send_goal('close') # arm.wait_for_motion_done() return "succeeded" diff --git a/challenge_take_out_the_garbage/src/challenge_take_out_the_garbage/pick_up.py b/challenge_take_out_the_garbage/src/challenge_take_out_the_garbage/pick_up.py index 9cf83768db..b8f74ce586 100644 --- a/challenge_take_out_the_garbage/src/challenge_take_out_the_garbage/pick_up.py +++ b/challenge_take_out_the_garbage/src/challenge_take_out_the_garbage/pick_up.py @@ -132,16 +132,16 @@ def execute(self, userdata=None): # This opening and closing is to make sure that the gripper is empty and closed before measuring the forces # It is necessary to close the gripper since the gripper is also closed at the final measurement - # arm.send_gripper_goal('open') + # arm.gripper.send_goal('open') # arm.wait_for_motion_done() - # arm.send_gripper_goal('close', max_torque=1.0) + # arm.gripper.send_goal('close', max_torque=1.0) # arm.wait_for_motion_done() arm_weight = measure_force.get_force() rospy.loginfo("Empty weight %s", arm_weight) # Open gripper - arm.send_gripper_goal('open') + arm.gripper.send_goal('open') arm.wait_for_motion_done() # Go down and grab @@ -154,7 +154,7 @@ def execute(self, userdata=None): # self._robot.torso.send_goal("grab_trash_down") # self._robot.torso.wait_for_motion_done() # rospy.sleep(3) - arm.send_gripper_goal('close', max_torque=1.0) + arm.gripper.send_goal('close', max_torque=1.0) arm.wait_for_motion_done() # Go up and back to pre grasp position @@ -182,7 +182,7 @@ def execute(self, userdata=None): self._robot.speech.speak("Look at this I can pick up the trash!") handed_entity = EntityInfo(id="trash") - arm.occupied_by = handed_entity + arm.gripper.occupied_by = handed_entity return "succeeded" diff --git a/robot_smach_states/src/robot_smach_states/manipulation/grab.py b/robot_smach_states/src/robot_smach_states/manipulation/grab.py index b92ab79abd..8d7c157bc2 100644 --- a/robot_smach_states/src/robot_smach_states/manipulation/grab.py +++ b/robot_smach_states/src/robot_smach_states/manipulation/grab.py @@ -63,7 +63,7 @@ def execute(self, userdata=None): arm.wait_for_motion_done() # Open gripper - arm.send_gripper_goal('open', timeout=0.0) + arm.gripper.send_goal('open', timeout=0.0) arm.wait_for_motion_done() # Make sure the head looks at the entity @@ -176,7 +176,7 @@ def execute(self, userdata=None): # ): # rospy.logerr('Pre-grasp failed:') # arm.reset() - # arm.send_gripper_goal('close', timeout=None) + # arm.gripper.send_goal('close', timeout=None) # return 'failed' # Grasp @@ -185,13 +185,13 @@ def execute(self, userdata=None): self.robot.speech.speak('I am sorry but I cannot move my arm to the object position', block=False) rospy.logerr('Grasp failed') arm.reset() - arm.send_gripper_goal('close', timeout=0.0) + arm.gripper.send_goal('close', timeout=0.0) return 'failed' # Close gripper - arm.send_gripper_goal('close') + arm.gripper.send_goal('close') - arm.occupied_by = grab_entity + arm.gripper.occupied_by = grab_entity # Lift goal_bl = grasp_framestamped.projectToFrame(self.robot.robot_name + "/base_link", @@ -237,7 +237,7 @@ def execute(self, userdata=None): result = "failed" rospy.logerr("Gripper is not holding an object") self.robot.speech.speak("Whoops, something went terribly wrong") - arm.occupied_by = None # Set the object the arm is holding to None + arm.gripper.occupied_by = None # Set the object the arm is holding to None else: # State is holding, grasp succeeded. # If unknown: sensor not there, assume gripper is holding and hope for the best @@ -301,7 +301,7 @@ def execute(self, userdata=None): if self._robot.robot_name == "amigo": self._robot.torso.reset() # Move up to make resetting of the arm safer. if arm is not None: - arm.send_gripper_goal('close') + arm.gripper.send_goal('close') self._robot.head.reset() # Sends a goal self._robot.head.cancel_goal() # And cancels it... if arm is not None: diff --git a/robot_smach_states/src/robot_smach_states/manipulation/manipulation.py b/robot_smach_states/src/robot_smach_states/manipulation/manipulation.py index ca2f5f2dac..476dfb909c 100644 --- a/robot_smach_states/src/robot_smach_states/manipulation/manipulation.py +++ b/robot_smach_states/src/robot_smach_states/manipulation/manipulation.py @@ -62,7 +62,7 @@ def execute(self, userdata=None): rospy.logerr("Could not resolve arm") return "failed" - object_in_arm = arm.occupied_by + object_in_arm = arm.gripper.occupied_by if not object_in_arm: rospy.logerr("Unoccupied arm sent to HandOverToHuman, arm should be occupied when entering this state.") @@ -78,11 +78,11 @@ def execute(self, userdata=None): self.robot.ed.update_entity(id=object_in_arm.id, action='remove') self.robot.speech.speak("I will open my gripper now.", block=False) - arm.send_gripper_goal('open') + arm.gripper.send_goal('open') arm.reset() arm.wait_for_motion_done() - arm.occupied_by = None + arm.gripper.occupied_by = None return 'succeeded' @@ -213,22 +213,22 @@ def execute(self, userdata=None): return "failed" if self.item_designator: - arm.occupied_by = self.item_designator.resolve() + arm.gripper.occupied_by = self.item_designator.resolve() else: if self.item_label != "": handed_entity = EntityInfo(id=self.item_label) - arm.occupied_by = handed_entity + arm.gripper.occupied_by = handed_entity else: rospy.logerr("No grabbed entity designator and no label for dummy entity given") return "failed" if arm.handover_to_robot(self.timeout): - arm.send_gripper_goal('close') + arm.gripper.send_goal('close') rospy.sleep(2.0) return "succeeded" else: rospy.logwarn("Eventhough no force is felt the gripper will close") - arm.send_gripper_goal('close') + arm.gripper.send_goal('close') rospy.sleep(2.0) return "timeout" @@ -266,9 +266,9 @@ def execute(self, userdata=None): rospy.logerr("Could not resolve {0}".format(self.grab_entity_designator)) if self.gripperstate != GripperState.OPEN: - arm.occupied_by = entity + arm.gripper.occupied_by = entity - if arm.send_gripper_goal(self.gripperstate, timeout=self.timeout): + if arm.gripper.send_goal(self.gripperstate, timeout=self.timeout): return 'succeeded' else: return 'failed' diff --git a/robot_smach_states/src/robot_smach_states/manipulation/place.py b/robot_smach_states/src/robot_smach_states/manipulation/place.py index 73b71ba1b3..50c2fd3c87 100644 --- a/robot_smach_states/src/robot_smach_states/manipulation/place.py +++ b/robot_smach_states/src/robot_smach_states/manipulation/place.py @@ -149,18 +149,18 @@ def execute(self, userdata=None): timeout=10, pre_grasp=False): rospy.logwarn("Cannot place the object, dropping it...") - place_entity = arm.occupied_by + place_entity = arm.gripper.occupied_by if not place_entity: rospy.logerr("Arm not holding an entity to place. This should never happen") else: self._robot.ed.update_entity(place_entity.id, frame_stamped=placement_fs) - arm.occupied_by = None + arm.gripper.occupied_by = None # Open gripper # Since we cannot reliably wait for the gripper, just set this timeout - arm.send_gripper_goal('open', timeout=2.0) + arm.gripper.send_goal('open', timeout=2.0) - arm.occupied_by = None + arm.gripper.occupied_by = None # Retract place_pose_bl = placement_fs.projectToFrame(self._robot.robot_name+'/base_link', @@ -179,7 +179,7 @@ def execute(self, userdata=None): arm.cancel_goals() # Close gripper - arm.send_gripper_goal('close', timeout=0.0) + arm.gripper.send_goal('close', timeout=0.0) arm.reset() arm.wait_for_motion_done() From eb9a69f7cb6a1e694f441eff45e4936752c4bfb7 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Tue, 29 Dec 2020 14:22:38 +0100 Subject: [PATCH 02/15] changed handover interface --- .../src/robot_smach_states/manipulation/manipulation.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/robot_smach_states/src/robot_smach_states/manipulation/manipulation.py b/robot_smach_states/src/robot_smach_states/manipulation/manipulation.py index 476dfb909c..684e534507 100644 --- a/robot_smach_states/src/robot_smach_states/manipulation/manipulation.py +++ b/robot_smach_states/src/robot_smach_states/manipulation/manipulation.py @@ -70,7 +70,7 @@ def execute(self, userdata=None): attempt = 0 - while not arm.handover_to_human(timeout=10) and attempt < 2: + while not arm.handover_detector.handover_to_human(timeout=10) and attempt < 2: self.robot.speech.speak("Please take it from my gripper.", block=False) attempt += 1 @@ -222,7 +222,7 @@ def execute(self, userdata=None): rospy.logerr("No grabbed entity designator and no label for dummy entity given") return "failed" - if arm.handover_to_robot(self.timeout): + if arm.handover_detector.handover_to_robot(self.timeout): arm.gripper.send_goal('close') rospy.sleep(2.0) return "succeeded" From b61dcbfa1e87d5f979ba59a3b9b79ca1e9695b00 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Tue, 15 Dec 2020 20:55:01 +0100 Subject: [PATCH 03/15] change grasp sensor interface --- .../src/robot_smach_states/manipulation/grab.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/robot_smach_states/src/robot_smach_states/manipulation/grab.py b/robot_smach_states/src/robot_smach_states/manipulation/grab.py index 8d7c157bc2..e64f09f3b9 100644 --- a/robot_smach_states/src/robot_smach_states/manipulation/grab.py +++ b/robot_smach_states/src/robot_smach_states/manipulation/grab.py @@ -230,9 +230,9 @@ def execute(self, userdata=None): arm.send_joint_goal('carrying_pose', timeout=0.0) result = 'succeeded' - if self._check_occupancy: + if self._check_occupancy and hasattr(arm.gripper, 'grasp_sensor'): # Check if the object is present in the gripper - if arm.object_in_gripper_measurement.is_empty: + if arm.gripper.grasp_sensor.object_in_gripper_measurement.is_empty: # If state is empty, grasp has failed result = "failed" rospy.logerr("Gripper is not holding an object") @@ -242,7 +242,7 @@ def execute(self, userdata=None): # State is holding, grasp succeeded. # If unknown: sensor not there, assume gripper is holding and hope for the best result = "succeeded" - if arm.object_in_gripper_measurement.is_unknown: + if arm.gripper.grasp_sensor.object_in_gripper_measurement.is_unknown: rospy.logwarn("GripperMeasurement unknown") # Reset head From 7dbf052308a8bce70548e0883eaa97134bc22d0e Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Tue, 9 Feb 2021 20:57:03 +0100 Subject: [PATCH 04/15] create custom check to ignore other arguments --- .../test/manipulation/test_manipulation.py | 21 ++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/robot_smach_states/test/manipulation/test_manipulation.py b/robot_smach_states/test/manipulation/test_manipulation.py index 72bce9ec39..59d1ea76ac 100644 --- a/robot_smach_states/test/manipulation/test_manipulation.py +++ b/robot_smach_states/test/manipulation/test_manipulation.py @@ -11,6 +11,17 @@ from robot_smach_states.util import designators as ds +def assert_any_first_argument(unit_test_case, calls, argument): + for call in calls: + if call[0][0] == argument: + return + raise AssertionError("co call in {} has first argument {}".format(calls, argument)) + + +def assert_first_argument(unit_test_case, call, argument): + unit_test_case.assertEqual(call[0][0], argument) + + class TestHandOverToHuman(unittest.TestCase): @classmethod @@ -34,7 +45,7 @@ def test_handover_to_human(self): self.robot.arms["leftArm"].send_joint_goal.assert_any_call('handover_to_human', max_joint_vel=mock.ANY, timeout=mock.ANY) self.robot.arms["leftArm"].send_joint_goal.assert_any_call('reset', max_joint_vel=mock.ANY, timeout=mock.ANY) - self.robot.arms["leftArm"].gripper.send_goal.assert_any_call('open', mock.ANY, max_torque=mock.ANY) + assert_any_first_argument(self, self.robot.arms["leftArm"].gripper.send_goal.call_args_list, 'open') self.assertIsNone(self.robot.arms["leftArm"].gripper.occupied_by) @@ -67,8 +78,8 @@ def test_handover_from_human(self): self.robot.arms["rightArm"].handover_detector.handover_to_robot.assert_called_once() - self.robot.arms["rightArm"].gripper.send_goal.assert_any_call('open', mock.ANY, max_torque=mock.ANY) - self.robot.arms["rightArm"].gripper.send_goal.assert_called_with('close', mock.ANY, max_torque=mock.ANY) + assert_any_first_argument(self, self.robot.arms["rightArm"].gripper.send_goal.call_args_list, 'open') + assert_first_argument(self, self.robot.arms["rightArm"].gripper.send_goal.call_args, 'close') self.assertEqual(self.robot.arms["rightArm"].gripper.occupied_by, self.entity) @@ -84,7 +95,7 @@ def setUp(self): def test_set_open(self): state = SetGripper(self.robot, self.arm_ds, 'open') self.assertEqual(state.execute(), "succeeded") - self.robot.arms["leftArm"].gripper.send_goal.assert_called_once_with('open', mock.ANY, max_torque=mock.ANY) + assert_first_argument(self, self.robot.arms["leftArm"].gripper.send_goal.call_args, 'open') class TestSetGripperClose(unittest.TestCase): @@ -101,7 +112,7 @@ def setUp(self): def test_set_close(self): state = SetGripper(self.robot, self.arm_ds, 'close', self.entity_ds) self.assertEqual(state.execute(), "succeeded") - self.robot.arms["leftArm"].gripper.send_goal.assert_called_once_with('close', mock.ANY, max_torque=mock.ANY) + assert_first_argument(self, self.robot.arms["leftArm"].gripper.send_goal.call_args, 'close') self.assertEqual(self.robot.arms["leftArm"].gripper.occupied_by, self.entity) From 56c4249c948dccaffdccd3db81ccd4b407af13f7 Mon Sep 17 00:00:00 2001 From: PetervDooren <19806646+PetervDooren@users.noreply.github.com> Date: Wed, 10 Feb 2021 10:41:59 +0100 Subject: [PATCH 05/15] spelling typo --- robot_smach_states/test/manipulation/test_manipulation.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robot_smach_states/test/manipulation/test_manipulation.py b/robot_smach_states/test/manipulation/test_manipulation.py index 59d1ea76ac..3873ec7c59 100644 --- a/robot_smach_states/test/manipulation/test_manipulation.py +++ b/robot_smach_states/test/manipulation/test_manipulation.py @@ -15,7 +15,7 @@ def assert_any_first_argument(unit_test_case, calls, argument): for call in calls: if call[0][0] == argument: return - raise AssertionError("co call in {} has first argument {}".format(calls, argument)) + raise AssertionError("no call in {} has first argument {}".format(calls, argument)) def assert_first_argument(unit_test_case, call, argument): From f043d0272c6da4487d8715229c575da0df4e14ab Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Wed, 10 Feb 2021 21:37:47 +0100 Subject: [PATCH 06/15] made custom magicmock --- robot_skills/src/robot_skills/mockbot.py | 58 ++++++++++++++++++++++++ 1 file changed, 58 insertions(+) diff --git a/robot_skills/src/robot_skills/mockbot.py b/robot_skills/src/robot_skills/mockbot.py index 06d8d1a45d..bb5642cf55 100755 --- a/robot_skills/src/robot_skills/mockbot.py +++ b/robot_skills/src/robot_skills/mockbot.py @@ -7,6 +7,7 @@ import mock import random import os +import six # ROS import geometry_msgs @@ -45,6 +46,63 @@ def old_query(spec, choices, timeout=10): raise Exception('robot.ears.recognize IS REMOVED. Use `robot.hmi.query`') +class AlteredMagicMock(mock.MagicMock): + def assert_called_with(_mock_self, *args, **kwargs): + """assert that the mock was called with the specified arguments. + + Raises an AssertionError if the args and keyword args passed in are + different to the last call to the mock.""" + self = _mock_self + if self.call_args is None: + expected = self._format_mock_call_signature(args, kwargs) + raise AssertionError('Expected call: %s\nNot called' % (expected,)) + + def _error_message(cause): + msg = self._format_mock_failure_message(args, kwargs) + if six.PY2 and cause is not None: + # Tack on some diagnostics for Python without __cause__ + msg = '%s\n%s' % (msg, str(cause)) + return msg + + expected = self._call_matcher((args, kwargs)) + actual = self._call_matcher(self.call_args) + if not self.equal_or_more_arguments(expected, actual): + cause = expected if isinstance(expected, Exception) else None + six.raise_from(AssertionError(_error_message(cause)), cause) + + def assert_any_call(self, *args, **kwargs): + """assert the mock has been called with the specified arguments. + + The assert passes if the mock has *ever* been called, unlike + `assert_called_with` and `assert_called_once_with` that only pass if + the call is the most recent one.""" + expected = self._call_matcher((args, kwargs)) + actual = [self._call_matcher(c) for c in self.call_args_list] + if not any([self.equal_or_more_arguments(expected, c) for c in actual]): + cause = expected if isinstance(expected, Exception) else None + expected_string = self._format_mock_call_signature(args, kwargs) + six.raise_from(AssertionError( + '%s call not found' % expected_string + ), cause) + + @staticmethod + def equal_or_more_arguments(expected, actual): + """ check whether the actual call contains at least the arguments and keyword arguments of the expected call. + But still return true if the actual call contains more (keyword) arguments than expected + """ + # check args + for i in range(len(expected[0])): + if expected[0][i] != actual[0][i]: + return False + # check kwargs + for k in expected[1]: + if k not in actual[1]: + return False + if expected[1][k] != actual[1][k]: + return False + return True + + class MockedRobotPart(object): def __init__(self, robot_name, tf_listener, *args, **kwargs): self.robot_name = robot_name From faa747c2e31a0b0d3c73e0e360e27d8a2af2dc14 Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Wed, 10 Feb 2021 21:39:38 +0100 Subject: [PATCH 07/15] changed mockbot --- robot_skills/src/robot_skills/mockbot.py | 202 +++++++++++------------ 1 file changed, 101 insertions(+), 101 deletions(-) diff --git a/robot_skills/src/robot_skills/mockbot.py b/robot_skills/src/robot_skills/mockbot.py index bb5642cf55..ad81b79b2c 100755 --- a/robot_skills/src/robot_skills/mockbot.py +++ b/robot_skills/src/robot_skills/mockbot.py @@ -108,17 +108,17 @@ def __init__(self, robot_name, tf_listener, *args, **kwargs): self.robot_name = robot_name self.tf_listener = tf_listener - self.load_param = mock.MagicMock() - self.wait_for_connections = mock.MagicMock() - self.create_simple_action_client = mock.MagicMock() - self.create_service_client = mock.MagicMock() - self.create_subscriber = mock.MagicMock() - self._add_connection = mock.MagicMock() - self.operational = mock.MagicMock() - self.subscribe_hardware_status = mock.MagicMock() - self.unsubscribe_hardware_status = mock.MagicMock() - self.process_hardware_status = mock.MagicMock() - self.reset = mock.MagicMock() + self.load_param = AlteredMagicMock() + self.wait_for_connections = AlteredMagicMock() + self.create_simple_action_client = AlteredMagicMock() + self.create_service_client = AlteredMagicMock() + self.create_subscriber = AlteredMagicMock() + self._add_connection = AlteredMagicMock() + self.operational = AlteredMagicMock() + self.subscribe_hardware_status = AlteredMagicMock() + self.unsubscribe_hardware_status = AlteredMagicMock() + self.process_hardware_status = AlteredMagicMock() + self.reset = AlteredMagicMock() class Arm(MockedRobotPart): @@ -132,19 +132,19 @@ def __init__(self, robot_name, tf_listener, get_joint_states, side): self._base_offset = random_kdl_vector() - self.default_configurations = mock.MagicMock() - self.default_trajectories = mock.MagicMock() - self.has_joint_goal = mock.MagicMock() - self.has_joint_trajectory = mock.MagicMock() - self.cancel_goals = mock.MagicMock() - self.close = mock.MagicMock() - self.send_goal = mock.MagicMock() - self.send_joint_goal = mock.MagicMock() - self.send_joint_trajectory = mock.MagicMock() - self.reset = mock.MagicMock() - self._send_joint_trajectory = mock.MagicMock() - self._publish_marker = mock.MagicMock() - self.wait_for_motion_done = mock.MagicMock() + self.default_configurations = AlteredMagicMock() + self.default_trajectories = AlteredMagicMock() + self.has_joint_goal = AlteredMagicMock() + self.has_joint_trajectory = AlteredMagicMock() + self.cancel_goals = AlteredMagicMock() + self.close = AlteredMagicMock() + self.send_goal = AlteredMagicMock() + self.send_joint_goal = AlteredMagicMock() + self.send_joint_trajectory = AlteredMagicMock() + self.reset = AlteredMagicMock() + self._send_joint_trajectory = AlteredMagicMock() + self._publish_marker = AlteredMagicMock() + self.wait_for_motion_done = AlteredMagicMock() # add parts self.gripper = Gripper(robot_name, tf_listener) @@ -162,29 +162,29 @@ class Gripper(MockedRobotPart): def __init__(self, robot_name, tf_listener, *args, **kwargs): super(Gripper, self).__init__(robot_name, tf_listener) self.occupied_by = None - self.send_goal = mock.MagicMock() + self.send_goal = AlteredMagicMock() class HandoverDetector(MockedRobotPart): def __init__(self, robot_name, tf_listener, *args, **kwargs): super(HandoverDetector, self).__init__(robot_name, tf_listener) - self.handover_to_human = mock.MagicMock() - self.handover_to_robot = mock.MagicMock() + self.handover_to_human = AlteredMagicMock() + self.handover_to_robot = AlteredMagicMock() class Base(MockedRobotPart): def __init__(self, robot_name, tf_listener, *args, **kwargs): super(Base, self).__init__(robot_name, tf_listener) - self.move = mock.MagicMock() - self.turn_towards = mock.MagicMock() - self.force_drive = mock.MagicMock() + self.move = AlteredMagicMock() + self.turn_towards = AlteredMagicMock() + self.force_drive = AlteredMagicMock() self.get_location = lambda: FrameStamped(random_kdl_frame(), "/map") - self.set_initial_pose = mock.MagicMock() - self.wait_for_motion_done = mock.MagicMock() - self.go = mock.MagicMock() - self.reset_costmap = mock.MagicMock() - self.cancel_goal = mock.MagicMock() - self.analyzer = mock.MagicMock() - self.global_planner = mock.MagicMock() - self.local_planner = mock.MagicMock() + self.set_initial_pose = AlteredMagicMock() + self.wait_for_motion_done = AlteredMagicMock() + self.go = AlteredMagicMock() + self.reset_costmap = AlteredMagicMock() + self.cancel_goal = AlteredMagicMock() + self.analyzer = AlteredMagicMock() + self.global_planner = AlteredMagicMock() + self.local_planner = AlteredMagicMock() self.local_planner.getStatus = mock.MagicMock(return_value="arrived") # always arrive for now self.global_planner.getPlan = mock.MagicMock(return_value=["dummy_plan"]) # always arrive for now @@ -194,73 +194,73 @@ def __init__(self, robot_name, tf_listener, *args, **kwargs): super(Hmi, self).__init__(robot_name, tf_listener) self.query = mock_query - self.show_image = mock.MagicMock() - self.show_image_from_msg = mock.MagicMock() + self.show_image = AlteredMagicMock() + self.show_image_from_msg = AlteredMagicMock() self.old_query = old_query - self.reset = mock.MagicMock() - self.restart_dragonfly = mock.MagicMock() + self.reset = AlteredMagicMock() + self.restart_dragonfly = AlteredMagicMock() class EButton(MockedRobotPart): def __init__(self, robot_name, tf_listener, *args, **kwargs): super(EButton, self).__init__(robot_name, tf_listener) - self.close = mock.MagicMock() - self._listen = mock.MagicMock() - self.read_ebutton = mock.MagicMock() + self.close = AlteredMagicMock() + self._listen = AlteredMagicMock() + self.read_ebutton = AlteredMagicMock() class Head(MockedRobotPart): def __init__(self, robot_name, tf_listener, *args, **kwargs): super(Head, self).__init__(robot_name, tf_listener) - self.reset = mock.MagicMock() - self.close = mock.MagicMock() - self.set_pan_tilt = mock.MagicMock() - self.send_goal = mock.MagicMock() - self.cancel_goal = mock.MagicMock() - self.reset = mock.MagicMock() - self.look_at_hand = mock.MagicMock() - self.wait = mock.MagicMock() - self.getGoal = mock.MagicMock() - self.atGoal = mock.MagicMock() - self.look_at_standing_person = mock.MagicMock() - self.look_at_point = mock.MagicMock() - self.look_at_ground_in_front_of_robot = mock.MagicMock() # TODO: Must return a EntityInfo - self.setPanTiltGoal = mock.MagicMock() - self.setLookAtGoal = mock.MagicMock() - self.cancelGoal = mock.MagicMock() - self.wait_for_motion_done = mock.MagicMock() - self._setHeadReferenceGoal = mock.MagicMock() - self.__feedbackCallback = mock.MagicMock() - self.__doneCallback = mock.MagicMock() + self.reset = AlteredMagicMock() + self.close = AlteredMagicMock() + self.set_pan_tilt = AlteredMagicMock() + self.send_goal = AlteredMagicMock() + self.cancel_goal = AlteredMagicMock() + self.reset = AlteredMagicMock() + self.look_at_hand = AlteredMagicMock() + self.wait = AlteredMagicMock() + self.getGoal = AlteredMagicMock() + self.atGoal = AlteredMagicMock() + self.look_at_standing_person = AlteredMagicMock() + self.look_at_point = AlteredMagicMock() + self.look_at_ground_in_front_of_robot = AlteredMagicMock() # TODO: Must return a EntityInfo + self.setPanTiltGoal = AlteredMagicMock() + self.setLookAtGoal = AlteredMagicMock() + self.cancelGoal = AlteredMagicMock() + self.wait_for_motion_done = AlteredMagicMock() + self._setHeadReferenceGoal = AlteredMagicMock() + self.__feedbackCallback = AlteredMagicMock() + self.__doneCallback = AlteredMagicMock() class Perception(MockedRobotPart): def __init__(self, robot_name, tf_listener, *args, **kwargs): super(Perception, self).__init__(robot_name, tf_listener) - self.reset = mock.MagicMock() - self.close = mock.MagicMock() - self.learn_person = mock.MagicMock() - self.detect_faces = mock.MagicMock() - self.get_best_face_recognition = mock.MagicMock() - self.get_rgb_depth_caminfo = mock.MagicMock() + self.reset = AlteredMagicMock() + self.close = AlteredMagicMock() + self.learn_person = AlteredMagicMock() + self.detect_faces = AlteredMagicMock() + self.get_best_face_recognition = AlteredMagicMock() + self.get_rgb_depth_caminfo = AlteredMagicMock() self.project_roi = lambda *args, **kwargs: VectorStamped(random.random(), random.random(), random.random(), "/map") class Lights(MockedRobotPart): def __init__(self, robot_name, tf_listener, *args, **kwargs): super(Lights, self).__init__(robot_name, tf_listener) - self.close = mock.MagicMock() - self.set_color = mock.MagicMock() - self.set_color_colorRGBA = mock.MagicMock() - self.on = mock.MagicMock() - self.off = mock.MagicMock() + self.close = AlteredMagicMock() + self.set_color = AlteredMagicMock() + self.set_color_colorRGBA = AlteredMagicMock() + self.on = AlteredMagicMock() + self.off = AlteredMagicMock() class Speech(MockedRobotPart): def __init__(self, robot_name, tf_listener, *args, **kwargs): super(Speech, self).__init__(robot_name, tf_listener) - self.close = mock.MagicMock() - self.speak = mock.MagicMock() + self.close = AlteredMagicMock() + self.speak = AlteredMagicMock() class Torso(MockedRobotPart): @@ -269,18 +269,18 @@ def __init__(self, robot_name, tf_listener, get_joint_states, *args, **kwargs): self.get_joint_states = get_joint_states - self.close = mock.MagicMock() - self.send_goal = mock.MagicMock() - self._send_goal = mock.MagicMock() - self.high = mock.MagicMock() - self.medium = mock.MagicMock() - self.low = mock.MagicMock() - self.reset = mock.MagicMock() - self.wait = mock.MagicMock() - self.cancel_goal = mock.MagicMock() - self._receive_torso_measurement = mock.MagicMock() - self.get_position = mock.MagicMock() - self.wait_for_motion_done = mock.MagicMock() + self.close = AlteredMagicMock() + self.send_goal = AlteredMagicMock() + self._send_goal = AlteredMagicMock() + self.high = AlteredMagicMock() + self.medium = AlteredMagicMock() + self.low = AlteredMagicMock() + self.reset = AlteredMagicMock() + self.wait = AlteredMagicMock() + self.cancel_goal = AlteredMagicMock() + self._receive_torso_measurement = AlteredMagicMock() + self.get_position = AlteredMagicMock() + self.wait_for_motion_done = AlteredMagicMock() class ED(MockedRobotPart): @@ -293,7 +293,7 @@ def generate_random_entity(id=None, type=None): else: entity_info.id = id entity_info.type = random.choice(["random_from_magicmock", "human", "coke", "fanta"]) - # entity.data = mock.MagicMock() + # entity.data = AlteredMagicMock() entity_info.data = "" entity = from_entity_info(entity_info) @@ -316,13 +316,13 @@ def __init__(self, robot_name, tf_listener, *args, **kwargs): self.get_closest_entity = lambda *args, **kwargs: random.choice(self._entities.values()) self.get_entity = lambda id=None: self._entities[id] self.reset = lambda *args, **kwargs: self._dynamic_entities.clear() - self.navigation = mock.MagicMock() - self.navigation.get_position_constraint = mock.MagicMock() - self.update_entity = mock.MagicMock() + self.navigation = AlteredMagicMock() + self.navigation.get_position_constraint = AlteredMagicMock() + self.update_entity = AlteredMagicMock() self.get_closest_possible_person_entity = lambda *args, **kwargs: self.generate_random_entity() self.get_closest_laser_entity = lambda *args, **kwargs: self.generate_random_entity() - self.get_entity_info = mock.MagicMock() - self.wait_for_connections = mock.MagicMock() + self.get_entity_info = AlteredMagicMock() + self.wait_for_connections = AlteredMagicMock() self._person_names = [] @@ -399,10 +399,10 @@ def __init__(self, *args, **kwargs): super(Mockbot, self).__init__(robot_name=robot_name, wait_services=False, tf_listener=MockedTfListener) - self.publish_target = mock.MagicMock() - self.tf_transform_pose = mock.MagicMock() - self.close = mock.MagicMock() - self.get_joint_states = mock.MagicMock() + self.publish_target = AlteredMagicMock() + self.tf_transform_pose = AlteredMagicMock() + self.close = AlteredMagicMock() + self.get_joint_states = AlteredMagicMock() self.add_body_part('hmi', Hmi(self.robot_name, self.tf_listener)) @@ -425,7 +425,7 @@ def __init__(self, *args, **kwargs): self.add_body_part('perception', Perception(self.robot_name, self.tf_listener)) # Miscellaneous - self.pub_target = mock.MagicMock() + self.pub_target = AlteredMagicMock() # Grasp offsets # TODO: Don't hardcode, load from parameter server to make robot independent. From 18f2bc00cb93b3780955cd23d8e6facf0c39417b Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Wed, 10 Feb 2021 21:40:31 +0100 Subject: [PATCH 08/15] Revert "create custom check to ignore other arguments" This reverts commit 59420f533d62b1e43bf8423e7d683d81a592f9f3. --- .../test/manipulation/test_manipulation.py | 21 +++++-------------- 1 file changed, 5 insertions(+), 16 deletions(-) diff --git a/robot_smach_states/test/manipulation/test_manipulation.py b/robot_smach_states/test/manipulation/test_manipulation.py index 3873ec7c59..72bce9ec39 100644 --- a/robot_smach_states/test/manipulation/test_manipulation.py +++ b/robot_smach_states/test/manipulation/test_manipulation.py @@ -11,17 +11,6 @@ from robot_smach_states.util import designators as ds -def assert_any_first_argument(unit_test_case, calls, argument): - for call in calls: - if call[0][0] == argument: - return - raise AssertionError("no call in {} has first argument {}".format(calls, argument)) - - -def assert_first_argument(unit_test_case, call, argument): - unit_test_case.assertEqual(call[0][0], argument) - - class TestHandOverToHuman(unittest.TestCase): @classmethod @@ -45,7 +34,7 @@ def test_handover_to_human(self): self.robot.arms["leftArm"].send_joint_goal.assert_any_call('handover_to_human', max_joint_vel=mock.ANY, timeout=mock.ANY) self.robot.arms["leftArm"].send_joint_goal.assert_any_call('reset', max_joint_vel=mock.ANY, timeout=mock.ANY) - assert_any_first_argument(self, self.robot.arms["leftArm"].gripper.send_goal.call_args_list, 'open') + self.robot.arms["leftArm"].gripper.send_goal.assert_any_call('open', mock.ANY, max_torque=mock.ANY) self.assertIsNone(self.robot.arms["leftArm"].gripper.occupied_by) @@ -78,8 +67,8 @@ def test_handover_from_human(self): self.robot.arms["rightArm"].handover_detector.handover_to_robot.assert_called_once() - assert_any_first_argument(self, self.robot.arms["rightArm"].gripper.send_goal.call_args_list, 'open') - assert_first_argument(self, self.robot.arms["rightArm"].gripper.send_goal.call_args, 'close') + self.robot.arms["rightArm"].gripper.send_goal.assert_any_call('open', mock.ANY, max_torque=mock.ANY) + self.robot.arms["rightArm"].gripper.send_goal.assert_called_with('close', mock.ANY, max_torque=mock.ANY) self.assertEqual(self.robot.arms["rightArm"].gripper.occupied_by, self.entity) @@ -95,7 +84,7 @@ def setUp(self): def test_set_open(self): state = SetGripper(self.robot, self.arm_ds, 'open') self.assertEqual(state.execute(), "succeeded") - assert_first_argument(self, self.robot.arms["leftArm"].gripper.send_goal.call_args, 'open') + self.robot.arms["leftArm"].gripper.send_goal.assert_called_once_with('open', mock.ANY, max_torque=mock.ANY) class TestSetGripperClose(unittest.TestCase): @@ -112,7 +101,7 @@ def setUp(self): def test_set_close(self): state = SetGripper(self.robot, self.arm_ds, 'close', self.entity_ds) self.assertEqual(state.execute(), "succeeded") - assert_first_argument(self, self.robot.arms["leftArm"].gripper.send_goal.call_args, 'close') + self.robot.arms["leftArm"].gripper.send_goal.assert_called_once_with('close', mock.ANY, max_torque=mock.ANY) self.assertEqual(self.robot.arms["leftArm"].gripper.occupied_by, self.entity) From b25493f897b08a23404e6d858362eda2459276c2 Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Wed, 10 Feb 2021 22:01:20 +0100 Subject: [PATCH 09/15] catch different size args list --- robot_skills/src/robot_skills/mockbot.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/robot_skills/src/robot_skills/mockbot.py b/robot_skills/src/robot_skills/mockbot.py index ad81b79b2c..6cd0b3a30f 100755 --- a/robot_skills/src/robot_skills/mockbot.py +++ b/robot_skills/src/robot_skills/mockbot.py @@ -92,8 +92,10 @@ def equal_or_more_arguments(expected, actual): """ # check args for i in range(len(expected[0])): - if expected[0][i] != actual[0][i]: - return False + if i >= len(actual[0]): + return False + if expected[0][i] != actual[0][i]: + return False # check kwargs for k in expected[1]: if k not in actual[1]: From 75f85d8c08c3b4ce76495609385fcf4c0016477d Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Wed, 10 Feb 2021 22:01:45 +0100 Subject: [PATCH 10/15] made gripper tests less strict --- .../test/manipulation/test_manipulation.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/robot_smach_states/test/manipulation/test_manipulation.py b/robot_smach_states/test/manipulation/test_manipulation.py index 72bce9ec39..cd7cd310bd 100644 --- a/robot_smach_states/test/manipulation/test_manipulation.py +++ b/robot_smach_states/test/manipulation/test_manipulation.py @@ -34,7 +34,7 @@ def test_handover_to_human(self): self.robot.arms["leftArm"].send_joint_goal.assert_any_call('handover_to_human', max_joint_vel=mock.ANY, timeout=mock.ANY) self.robot.arms["leftArm"].send_joint_goal.assert_any_call('reset', max_joint_vel=mock.ANY, timeout=mock.ANY) - self.robot.arms["leftArm"].gripper.send_goal.assert_any_call('open', mock.ANY, max_torque=mock.ANY) + self.robot.arms["leftArm"].gripper.send_goal.assert_any_call('open') self.assertIsNone(self.robot.arms["leftArm"].gripper.occupied_by) @@ -67,8 +67,8 @@ def test_handover_from_human(self): self.robot.arms["rightArm"].handover_detector.handover_to_robot.assert_called_once() - self.robot.arms["rightArm"].gripper.send_goal.assert_any_call('open', mock.ANY, max_torque=mock.ANY) - self.robot.arms["rightArm"].gripper.send_goal.assert_called_with('close', mock.ANY, max_torque=mock.ANY) + self.robot.arms["rightArm"].gripper.send_goal.assert_any_call('open') + self.robot.arms["rightArm"].gripper.send_goal.assert_called_with('close') self.assertEqual(self.robot.arms["rightArm"].gripper.occupied_by, self.entity) @@ -84,7 +84,7 @@ def setUp(self): def test_set_open(self): state = SetGripper(self.robot, self.arm_ds, 'open') self.assertEqual(state.execute(), "succeeded") - self.robot.arms["leftArm"].gripper.send_goal.assert_called_once_with('open', mock.ANY, max_torque=mock.ANY) + self.robot.arms["leftArm"].gripper.send_goal.assert_called_once_with('open') class TestSetGripperClose(unittest.TestCase): @@ -101,7 +101,7 @@ def setUp(self): def test_set_close(self): state = SetGripper(self.robot, self.arm_ds, 'close', self.entity_ds) self.assertEqual(state.execute(), "succeeded") - self.robot.arms["leftArm"].gripper.send_goal.assert_called_once_with('close', mock.ANY, max_torque=mock.ANY) + self.robot.arms["leftArm"].gripper.send_goal.assert_called_once_with('close') self.assertEqual(self.robot.arms["leftArm"].gripper.occupied_by, self.entity) From 84816016081402c53cfed558e9c5b1bb58c247f8 Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Thu, 11 Feb 2021 11:50:48 +0100 Subject: [PATCH 11/15] add six dependency --- robot_skills/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/robot_skills/package.xml b/robot_skills/package.xml index 224aa46e24..5ea9ab7726 100644 --- a/robot_skills/package.xml +++ b/robot_skills/package.xml @@ -40,6 +40,7 @@ python-mock test_tools + python-six python-sphinx python-sphinx-autoapi-pip From ddc01774aed28fd4e257f88e361cc3acf4caa8bf Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Thu, 11 Feb 2021 17:33:09 +0100 Subject: [PATCH 12/15] added missing __ne__ to vectorstamped --- robot_skills/src/robot_skills/util/kdl_conversions.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/robot_skills/src/robot_skills/util/kdl_conversions.py b/robot_skills/src/robot_skills/util/kdl_conversions.py index b680054f1e..978e6d889b 100644 --- a/robot_skills/src/robot_skills/util/kdl_conversions.py +++ b/robot_skills/src/robot_skills/util/kdl_conversions.py @@ -87,6 +87,9 @@ def __eq__(self, other): else: return False + def __ne__(self, other): + return not self.__eq__(other) + def point_msg_to_kdl_vector(point): """ From 7de4d5e249bbb9b144ecd638cbd9406bfa75294a Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 13 Feb 2021 13:51:07 +0100 Subject: [PATCH 13/15] (robot_skills) update docstrings styling --- robot_skills/src/robot_skills/mockbot.py | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/robot_skills/src/robot_skills/mockbot.py b/robot_skills/src/robot_skills/mockbot.py index 6cd0b3a30f..1e2ffaf344 100755 --- a/robot_skills/src/robot_skills/mockbot.py +++ b/robot_skills/src/robot_skills/mockbot.py @@ -48,10 +48,12 @@ def old_query(spec, choices, timeout=10): class AlteredMagicMock(mock.MagicMock): def assert_called_with(_mock_self, *args, **kwargs): - """assert that the mock was called with the specified arguments. + """ + Assert that the mock was called with the specified arguments. Raises an AssertionError if the args and keyword args passed in are - different to the last call to the mock.""" + different to the last call to the mock. + """ self = _mock_self if self.call_args is None: expected = self._format_mock_call_signature(args, kwargs) @@ -71,11 +73,13 @@ def _error_message(cause): six.raise_from(AssertionError(_error_message(cause)), cause) def assert_any_call(self, *args, **kwargs): - """assert the mock has been called with the specified arguments. + """ + Assert the mock has been called with the specified arguments. The assert passes if the mock has *ever* been called, unlike `assert_called_with` and `assert_called_once_with` that only pass if - the call is the most recent one.""" + the call is the most recent one. + """ expected = self._call_matcher((args, kwargs)) actual = [self._call_matcher(c) for c in self.call_args_list] if not any([self.equal_or_more_arguments(expected, c) for c in actual]): @@ -87,7 +91,8 @@ def assert_any_call(self, *args, **kwargs): @staticmethod def equal_or_more_arguments(expected, actual): - """ check whether the actual call contains at least the arguments and keyword arguments of the expected call. + """ + Check whether the actual call contains at least the arguments and keyword arguments of the expected call. But still return true if the actual call contains more (keyword) arguments than expected """ # check args @@ -166,12 +171,14 @@ def __init__(self, robot_name, tf_listener, *args, **kwargs): self.occupied_by = None self.send_goal = AlteredMagicMock() + class HandoverDetector(MockedRobotPart): def __init__(self, robot_name, tf_listener, *args, **kwargs): super(HandoverDetector, self).__init__(robot_name, tf_listener) self.handover_to_human = AlteredMagicMock() self.handover_to_robot = AlteredMagicMock() + class Base(MockedRobotPart): def __init__(self, robot_name, tf_listener, *args, **kwargs): super(Base, self).__init__(robot_name, tf_listener) From 29d907193dd0a58ab31b8d8f6dc50962eb731bda Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 13 Feb 2021 13:52:58 +0100 Subject: [PATCH 14/15] (robot_skills) don't use custom self --- robot_skills/src/robot_skills/mockbot.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/robot_skills/src/robot_skills/mockbot.py b/robot_skills/src/robot_skills/mockbot.py index 1e2ffaf344..a5b055145b 100755 --- a/robot_skills/src/robot_skills/mockbot.py +++ b/robot_skills/src/robot_skills/mockbot.py @@ -47,14 +47,13 @@ def old_query(spec, choices, timeout=10): class AlteredMagicMock(mock.MagicMock): - def assert_called_with(_mock_self, *args, **kwargs): + def assert_called_with(self, *args, **kwargs): """ Assert that the mock was called with the specified arguments. Raises an AssertionError if the args and keyword args passed in are different to the last call to the mock. """ - self = _mock_self if self.call_args is None: expected = self._format_mock_call_signature(args, kwargs) raise AssertionError('Expected call: %s\nNot called' % (expected,)) From e64ee2bacf19fefce4df777570d8c4140eb4d907 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 13 Feb 2021 13:56:07 +0100 Subject: [PATCH 15/15] (robot_skills) reduce statement to one line --- robot_skills/src/robot_skills/mockbot.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/robot_skills/src/robot_skills/mockbot.py b/robot_skills/src/robot_skills/mockbot.py index a5b055145b..b70a1c9859 100755 --- a/robot_skills/src/robot_skills/mockbot.py +++ b/robot_skills/src/robot_skills/mockbot.py @@ -84,9 +84,7 @@ def assert_any_call(self, *args, **kwargs): if not any([self.equal_or_more_arguments(expected, c) for c in actual]): cause = expected if isinstance(expected, Exception) else None expected_string = self._format_mock_call_signature(args, kwargs) - six.raise_from(AssertionError( - '%s call not found' % expected_string - ), cause) + six.raise_from(AssertionError("'%s' call not found" % expected_string), cause) @staticmethod def equal_or_more_arguments(expected, actual):