class GraspActionServer:
def __init__(self):
self.server = actionlib.SimpleActionServer('object_pick', pickAction,
self.execute, False)
self.result = pickResult()
self.moveit = MoveIt()
self.object_size = {
"evergreen": [0.215, 0.05, 0.095],
"tomatosoup": [0.1, 0.03, 0.13],
"basetech": [0.16, 0.045, 0.17],
"eraserbox": [0.155, 0.05, 0.055],
"usbhub": [0.13, 0.07, 0.075]
}
self.server.start()
def execute(self, goal):
print('+++++++++++++++++++++++++++++++++++++++++++++++++')
rospy.loginfo('Picking request received')
self.moveit.close_fingers()
x, y, z = goal.request.position
z_max = goal.request.z_max
rospy.loginfo("Received z_max from behav: {}".format(z_max))
rospy.loginfo("Received z from behav: {}".format(z))
success = self.moveit.clear_object(
x, y, z, z_max, goal.request.yaw,
self.object_size[goal.request.object_label])
self.result = pickResult() # Reset object array
if success:
self.server.set_succeeded(self.result)
else:
self.moveit.rtb()
self.server.set_aborted(self.result)
move_head(move_head_req)
model = load_model(os.environ["HOME"] + "/DATA/model.h5")
model_classification = load_model(os.environ["HOME"] + "/DATA/model_classification.h5")
clear_octomap() # Reseting the Octomap because we moved before
alice_object = AliceObject() # Class for interfacing with alice_object node
moveit = MoveIt() # Class for interfacing with MoveIt (Needs to be implemented!)
# For deleting the model from Gazebo (Pretending we dropped in a bin on Alice)
delete_model = rospy.ServiceProxy("gazebo/delete_model", DeleteModel)
# you can change the cropping in GetObjectInformation function
all_object_data = alice_object.GetObjectInformation() # Gather RGB and Depth cropping data and x,y,z information
moveit.close_fingers() # Close the fingers before planning!
zero_image = np.zeros((28,28))
one_image = np.ones((28,28))
for objects in all_object_data:
x = 0
y = 0
z = 0
z_min = 0
z_max = 0
predictions = []
classifications = []
for object in objects: