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 re_init(self, reset_holding_object=True):
if reset_holding_object:
if self.pp_client is not None:
goal = PickPlaceGoal()
goal.operation = goal.RESET
self.pp_client.send_goal(goal)
self.pp_client.wait_for_result()
self.last_pick_instruction_id = None
self.holding_object = None
self.get_ready()
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 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
def place_object(client, x, y):
rospy.loginfo('place_object: ' + str((x, y)))
goal = PickPlaceGoal()
goal.operation = goal.PLACE_TO_POSE
goal.z_axis_angle_increment = 1.57
goal.keep_orientation = False
goal.pose = PoseStamped()
goal.pose.header.frame_id = "marker"
goal.pose.header.stamp = rospy.Time.now()
goal.pose.pose.position.x = x
goal.pose.pose.position.y = y
goal.pose.pose.position.z = 0.1
goal.pose.pose.orientation.w = 1.0
return send_goal(client, goal)
def get_ready(client):
rospy.loginfo('get_ready')
goal = PickPlaceGoal()
goal.operation = goal.GET_READY
return send_goal(client, goal)