def pick_object(self, obj, pick_instruction_id, arm_id=None, pick_only_y_axis=False, from_feeder=False): if arm_id is None: return ArtBrainErrorSeverities.ERROR, ArtBrainErrors.ERROR_GRIPPER_NOT_DEFINED, None arm = self.get_arm_by_id(arm_id) severity, error = self.check_arm_for_pick(arm) if error is not None: return severity, error, arm_id if obj is None or obj.object_id is None or obj.object_id == "": return ArtBrainErrorSeverities.WARNING, ArtBrainErrors.ERROR_OBJECT_NOT_DEFINED, arm_id goal = PickPlaceGoal() goal.object = obj.object_id goal.operation = goal.PICK_OBJECT_ID if not from_feeder else goal.PICK_FROM_FEEDER goal.keep_orientation = False goal.pick_only_y_axis = pick_only_y_axis rospy.logdebug("Picking object with ID: " + str(obj.object_id)) arm.pp_client.send_goal(goal) # arm.pp_client.wait_for_result() severity, error, _ = self.wait_for_action_result(arm.pp_client) if error is not None: # print "returning 2" return severity, error, arm_id # TODO: make some error msg etc rospy.logdebug('Results from p&p server') rospy.logdebug("result: " + str(arm.pp_client.get_result())) rospy.logdebug("status: " + arm.pp_client.get_goal_status_text()) rospy.logdebug("state: " + str(arm.pp_client.get_state())) if arm.pp_client.get_result().result == 0: arm.last_pick_instruction_id = pick_instruction_id return None, None, arm_id else: return ArtBrainErrorSeverities.WARNING, ArtBrainErrors.ERROR_PICK_FAILED, arm_id
def pick_object_id(client, object_id): rospy.loginfo('pick_object_id: ' + str(object_id)) goal = PickPlaceGoal() goal.object = object_id goal.operation = goal.PICK_OBJECT_ID return send_goal(client, goal)
def place_object_to_pose(self, place_pose, arm_id, objects_frame_id="marker", pick_only_y_axis=False): if arm_id is None: return ArtBrainErrorSeverities.ERROR, ArtBrainErrors.ERROR_GRIPPER_NOT_DEFINED, None if place_pose is None: return ArtBrainErrorSeverities.ERROR, ArtBrainErrors.ERROR_PLACE_POSE_NOT_DEFINED, None arm = self.get_arm_by_id(arm_id) # type: ArtGripper if arm.holding_object is None or arm.holding_object.object_type is None or arm.holding_object.object_type == "": return ArtBrainErrorSeverities.WARNING, ArtBrainErrors.ERROR_NO_OBJECT_IN_GRIPPER, arm_id goal = PickPlaceGoal() goal.operation = goal.PLACE_TO_POSE goal.object = arm.holding_object.object_id # TODO (Kapi) check if it is safe to place the object # if not self.check_place_pose(place_pose, obj): # return False # TODO how to decide between 180 and 90 deg? # allow object to be rotated by 90 deg around z axis # goal.z_axis_angle_increment = 3.14 goal.pose = place_pose goal.pose.header.stamp = rospy.Time.now() goal.pose.header.frame_id = objects_frame_id # no need to alter place pose z - contacts between attached object and support surface (table) are allowed # goal.pose.pose.position.z += 0.01 rospy.logdebug("Placing object with ID: " + str(arm.holding_object.object_id)) arm.pp_client.send_goal(goal) severity, error, _ = self.wait_for_action_result(arm.pp_client) if error is not None: return severity, error, arm_id if arm.pp_client.get_result().result == 0: return None, None, arm_id else: return ArtBrainErrorSeverities.WARNING, ArtBrainErrors.ERROR_PLACE_FAILED, None