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 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