def plot_output(depth_img,
grasp_q_img,
grasp_angle_img,
no_grasps=1,
grasp_width_img=None):
"""
Plot the output of a GG-CNN
:param rgb_img: RGB Image
:param depth_img: Depth Image
:param grasp_q_img: Q output of GG-CNN
:param grasp_angle_img: Angle output of GG-CNN
:param no_grasps: Maximum number of grasps to plot
:param grasp_width_img: (optional) Width output of GG-CNN
:return:
"""
gs = detect_grasps(grasp_q_img,
grasp_angle_img,
width_img=grasp_width_img,
no_grasps=1)
fig = plt.figure(figsize=(10, 10))
ax = fig.add_subplot(1, 1, 1)
ax.imshow(depth_img, cmap='gray')
for g in gs:
g.plot(ax)
ax.set_title('Depth')
ax.axis('off')
return gs
def generate(self):
# Get RGB-D image from camera
image_bundle = self.camera.get_image_bundle()
rgb = image_bundle['rgb']
depth = image_bundle['aligned_depth']
x, depth_img, rgb_img = self.cam_data.get_data(rgb=rgb, depth=depth)
# Predict the grasp pose using the saved model
with torch.no_grad():
xc = x.to(self.device)
pred = self.model.predict(xc)
q_img, ang_img, width_img = post_process_output(
pred['pos'], pred['cos'], pred['sin'], pred['width'])
grasps = detect_grasps(q_img, ang_img, width_img)
# Get grasp position from model output
pos_z = depth[grasps[0].center[0] + self.cam_data.top_left[0],
grasps[0].center[1] +
self.cam_data.top_left[1]] * self.cam_depth_scale - 0.04
pos_x = np.multiply(
grasps[0].center[1] + self.cam_data.top_left[1] -
self.camera.intrinsics.ppx, pos_z / self.camera.intrinsics.fx)
pos_y = np.multiply(
grasps[0].center[0] + self.cam_data.top_left[0] -
self.camera.intrinsics.ppy, pos_z / self.camera.intrinsics.fy)
if pos_z == 0:
return
target = np.asarray([pos_x, pos_y, pos_z])
target.shape = (3, 1)
print('target: ', target)
# Convert camera to robot coordinates
camera2robot = self.cam_pose
target_position = np.dot(camera2robot[0:3, 0:3],
target) + camera2robot[0:3, 3:]
target_position = target_position[0:3, 0]
# Convert camera to robot angle
angle = np.asarray([0, 0, grasps[0].angle])
angle.shape = (3, 1)
target_angle = np.dot(camera2robot[0:3, 0:3], angle)
# Concatenate grasp pose with grasp angle
grasp_pose = np.append(target_position, target_angle[2])
print('grasp_pose: ', grasp_pose)
np.save(self.grasp_pose, grasp_pose)
if self.fig:
plot_grasp(fig=self.fig,
rgb_img=self.cam_data.get_rgb(rgb, False),
grasps=grasps,
save=True)
def plot_grasp(fig,
grasps=None,
save=False,
rgb_img=None,
grasp_q_img=None,
grasp_angle_img=None,
no_grasps=1,
grasp_width_img=None):
"""
Plot the output grasp of a network
:param fig: Figure to plot the output
:param grasps: grasp pose(s)
:param save: Bool for saving the plot
:param rgb_img: RGB Image
:param grasp_q_img: Q output of network
:param grasp_angle_img: Angle output of network
:param no_grasps: Maximum number of grasps to plot
:param grasp_width_img: (optional) Width output of network
:return:
"""
if grasps is None:
grasps = detect_grasps(grasp_q_img,
grasp_angle_img,
width_img=grasp_width_img,
no_grasps=no_grasps)
plt.ion()
plt.clf()
ax = plt.subplot(111)
ax.imshow(rgb_img)
for g in grasps:
g.plot(ax)
ax.set_title('Grasp')
ax.axis('off')
plt.pause(0.1)
fig.canvas.draw()
if save:
time = datetime.now().strftime('%Y-%m-%d %H:%M:%S')
fig.savefig('results/{}.png'.format(time))
def __init__(self, args):
self.index = 0
if len(sys.argv) > 1:
self.index = int(sys.argv[1])
rospy.init_node('save_img')
bridge = CvBridge()
#while not rospy.is_shutdown():
#raw_input()
#color_sub = rospy.Subscriber("/camera/color/image_raw",Image)
cmd_pub = rospy.Publisher('ggcnn/out/command', Float32MultiArray, queue_size=1)
print "depth"
rgbo = rospy.wait_for_message('/camera/color/image_raw', Image)
print "depth"
deptho = rospy.wait_for_message('/camera/depth/image_raw', Image)
# rgbo = rospy.wait_for_message('/camera/rgb/image_color', Image)
depthfin = bridge.imgmsg_to_cv2(deptho)
rgbfin = bridge.imgmsg_to_cv2(rgbo)
depthfin1 = depthfin/2
#rgbfin1= cv2.cvtColor(rgbfin, cv2.COLOR_BGR2RGB)
#cv2.imwrite('rgb.png', rgbfin1)
#MODEL_FILE = 'training2_084'
points_out, ang_out, width_out, depth = predict(depthfin1)
grasps = grasp.detect_grasps(points_out, ang_out, 0.7, width_img=width_out, no_grasps=5)
m = evaluation1.plot_output(width_out, depth, points_out, ang_out, grasps, rgbfin, crop_size, y_off, x_off)
x, y, z, ang, rwidth = graspdata(points_out, depthfin, grasps, m)
punto=gmsg.Pose()
#invertidos porque si
punto.position.x=x
punto.position.y=y
punto.position.z=0
print punto
print convert_pose(punto,"cam","world")
def plan(self, image, width):
random.seed(0)
np.random.seed(0)
im = image.copy()
try_num = 5
qs = []
gs = []
for _ in range(try_num):
try:
points_out, ang_out, width_out, depth_out = self.ggcnn.predict(
im, 300)
ggs = detect_grasps(points_out,
ang_out,
width_img=width_out,
no_grasps=1)
if len(ggs) == 0:
continue
g = Grasp2D.from_jaq(ggs[0].to_jacquard(scale=1))
g.width_px = width
q = points_out[int(g.center[1]), int(g.center[0])]
except Exception as e:
print('--------------------出错了----------------------')
print(e)
else:
qs.append(q)
gs.append(g)
# if q > 0.9:
# break
if len(gs) == 0:
return None
g = gs[np.argmax(qs)]
q = qs[np.argmax(qs)]
p0, p1 = g.endpoints
print('-------------------------')
print([p0, p1, g.depth, g.depth, q])
return [p0, p1, g.depth, g.depth, q]
if args.iou_eval:
s = evaluation.calculate_iou_match(
q_img,
ang_img,
test_data.dataset.get_gtbb(didx, rot, zoom),
no_grasps=args.n_grasps,
grasp_width=width_img,
)
if s:
results['correct'] += 1
else:
results['failed'] += 1
if args.jacquard_output:
grasps = grasp.detect_grasps(q_img,
ang_img,
width_img=width_img,
no_grasps=1)
with open(jo_fn, 'a') as f:
for g in grasps:
f.write(test_data.dataset.get_jname(didx) + '\n')
f.write(g.to_jacquard(scale=1024 / 300) + '\n')
if args.vis:
evaluation.plot_output(
test_data.dataset.get_rgb(didx, rot, zoom,
normalise=False),
test_data.dataset.get_depth(didx, rot, zoom),
q_img,
ang_img,
no_grasps=args.n_grasps,
grasp_width_img=width_img)
def find_pose():
rospy.sleep(0.1)
rgbo = rospy.wait_for_message('/camera/color/image_raw', Image)
print("depth")
deptho = rospy.wait_for_message('/camera/depth/image_raw', Image)
# rgbo = rospy.wait_for_message('/camera/rgb/image_color', Image)
depthfin = bridge.imgmsg_to_cv2(deptho)
rgbfin = bridge.imgmsg_to_cv2(rgbo)
rgbfin1= cv2.cvtColor(rgbfin, cv2.COLOR_BGR2RGB)
#cv2.imwrite('rgb.png', rgbfin1)
#print(rgbfin.shape)
#img_p = np.average(rgbfin.astype(np.float64),axis=2)
#print(img_p.shape)
#img_p = np.tile(img_p[:,:,np.newaxis],(1,1,3))
#img_p = np.tile(img_p,(1,1,3))
#k = 10;
#img_hc = k*(rgbfin-img_p) # Remueve el nivel de gris
#print(img_hc.dtype)
#fig = plt.figure(figsize=(10, 10))
#ax = fig.add_subplot(1, 1, 1)
#plot = ax.imshow(img_hc)
#ax.set_title('hc')
#ax.axis('off')
#plt.show()
#print(img_hc.shape)
#print(np.amin(img_hc,axis=2).shape)
#img_hc = k*(img_hc - np.tile(np.amin(img_hc,axis=2)[:,:,np.newaxis],(1,1,3))) # Manda el menor canal a 0 y multiplica
#print(img_hc.dtype)
#fig = plt.figure(figsize=(10, 10))
#ax = fig.add_subplot(1, 2, 1)
#plot = ax.imshow(img_hc)
#ax.set_title('hc')
#ax.axis('off')
#ax = fig.add_subplot(1, 2, 2)
#plot = ax.imshow(rgbfin)
#ax.set_title('rgb')
#ax.axis('off')
#plt.show()
#img_hc = cv2.cvtColor(img_hc.astype(np.uint8),cv2.COLOR_BGR2HSV)
#fig = plt.figure(figsize=(10, 10))
#ax = fig.add_subplot(1, 1, 1)
#plot = ax.imshow(img_hc, cmap='hsv')
#ax.set_title('hc')
#ax.axis('off')
#plt.show()
#lim0 = [0, 0, 0]
#lim1 = [120, 100, 100]
#img_b = cv2.inRange(img_hc,lim0,lim1) # Aqui ya deberias tener SOLO el perfil del canasto
#iy, ix, Dy, Dx, widthbinx, widthbiny, iy1, ix1, Dy1, Dx1 = calibracion.calibracion(depthfin, rgbfin)
iy, ix, Dy, Dx, widthbinx, widthbiny, iy1, ix1, Dy1, Dx1 =[129, 192, 222, 329, 379, 252, 8, 6, 206, 317 ]
#raw_input('done')
depthfin1 = depthfin[iy:Dy+iy, ix:Dx+ix]
#fig = plt.figure(figsize=(10, 10))
#ax = fig.add_subplot(1, 2, 1)
#ax.imshow(rgbfin)
#ax.set_title('rgb')
#ax.axis('off')
#ax = fig.add_subplot(1, 1, 1)
#ax.imshow(depthfin1, cmap='gray')
#ax.set_title('Depth')
#ax.axis('off')
#plt.show()
depthfin1 = depthfin1
#rgbfin1= cv2.cvtColor(rgbfin, cv2.COLOR_BGR2RGB)
#cv2.imwrite('rgb.png', rgbfin1)
#MODEL_FILE = 'training2_084'
points_out, ang_out, width_out, depth = predict(depthfin1)
grasps = grasp.detect_grasps(points_out, ang_out, 0.7, width_img=width_out, no_grasps=5)
if grasps:
maxgrasps = evaluation1.plot_output(width_out, depth, points_out, ang_out, grasps, rgbfin, crop_size, y_off, x_off)
#fig.savefig('plot1.png')
#ENCONTRAR LA PROFUNDIDAD EN LA IMAGEN ORIGINAL
#PIXEL CON VALOR MAXIMO
pushlist, grasps = pushing(grasps, ix1, iy1, Dx1, Dy1, Dx, Dy)
#print('pushlist:', pushlist)
grasps = graspdata(points_out, depthfin, grasps, ix, iy, Dx, Dy)
punto1 = geometry_msgs.msg.Pose()
arr=[]
if not grasps:
arr=[0,0,0,0,0,6]
elif grasps:
for i in range(len(grasps)):
gqmax = maxgrasps[i][1]
mov = pushlist[gqmax]
x, y, z, ang, rwidth =rvalues(grasps[gqmax], depthfin, Dx, Dy,widthbinx, widthbiny)
arr.append(x)
arr.append(y)
arr.append(z)
arr.append(ang)
arr.append(rwidth)
arr.append(mov)
punto1.position.x = z
punto1.position.y = -x
punto1.position.z = -y
q = tft.quaternion_from_euler(0, 1.5, ang)
punto1.orientation.x =q[0]
punto1.orientation.y =q[1]
punto1.orientation.z =q[2]
punto1.orientation.w =q[3]
punto2 = convert_pose(punto1,"cam","world")
#print('punto 1: ', punto1)
print('punto 2: ', punto2)
#punto.position.x=z
#punto.position.y=-x
#punto.position.z=-y
#print punto
#x =0.5
#y=-0.1
#z=0.6
#w = 1
cmd_msg = Float32MultiArray()
cmd_msg.data = arr #print('publicado lol: ', cmd_msg)
cmd_pub.publish(cmd_msg)
print('arr: ', arr)
#d1= 0.352
#a1=0.07
#a2= 0.36
#d4 = 0.38
#d6= 0.065
#th1=math.atan2(punto2.position.y, punto2.position.x)
#print('puntof', punto2)
#print current_robot_pose("tcp_link","world")
#pose_commander.main()
#print convert_pose(punto2,"tcp_link","world")
cont=0
fig = plt.figure(figsize=(10, 10))
ax = fig.add_subplot(1, 2, 1)
ax.imshow(depthfin, cmap='gray')
if grasps:
for g in grasps:
g.plot(ax)
if pushlist[cont] == 1:
ax.arrow(g.center[1], g.center[0], 50, 0, head_width=8, head_length=10, fc='lightblue', ec='red')
if pushlist[cont] == 2:
ax.arrow(g.center[1], g.center[0], -50, 0, head_width=8, head_length=10, fc='lightblue', ec='red')
if pushlist[cont] == 3:
ax.arrow(g.center[1], g.center[0], 0, 50, head_width=8, head_length=10, fc='lightblue', ec='red')
if pushlist[cont] == 4:
ax.arrow(g.center[1], g.center[0], 0, -50, head_width=8, head_length=10, fc='lightblue', ec='red')
cont = cont+1
ax.set_title('Depth')
ax.axis('off')
ax = fig.add_subplot(1, 2, 2)
plot = ax.imshow(points_out, cmap='jet', vmin=0, vmax=1)
ax.set_title('quality')
ax.axis('off')
plt.show()
#fig = plt.figure(figsize=(10, 10))
#ax = fig.add_subplot(1, 2, 1)
#plot = ax.imshow(ang_out, cmap='hsv', vmin=-np.pi / 2, vmax=np.pi / 2)
#ax.set_title('Angle')
#ax.axis('off')
#ax = fig.add_subplot(1, 2, 2)
#plot = ax.imshow(width_out, cmap='hsv', vmin=0, vmax=150)
#ax.set_title('width')
#ax.axis('off')
#plt.colorbar(plot)
#ax = fig.add_subplot(2, 2, 4)
#ax.imshow(rgbfin)
#ax.set_title('rgb')
#ax.axis('off')
plt.show()
def __init__(self, args):
self.index = 0
if len(sys.argv) > 1:
self.index = int(sys.argv[1])
rospy.init_node('save_img')
bridge = CvBridge()
crop_size = 350
#while not rospy.is_shutdown():
raw_input()
# color_sub = rospy.Subscriber("/camera/color/image_raw",Image)
#depth_sub = rospy.Subscriber("/camera/depth/image_raw",Image)
print "depth"
cmd_pub = rospy.Publisher('ggcnn/out/command',
Float32MultiArray,
queue_size=1)
#rgbo = rospy.wait_for_message('/camera/color/image_raw', Image)
deptho = rospy.wait_for_message('/camera/depth/image_rect', Image)
rgbo = rospy.wait_for_message('/camera/rgb/image_color', Image)
depthfin = bridge.imgmsg_to_cv2(deptho)
depthfin = depthfin
#try:
# cv_image = self.bridge.imgmsg_to_cv2(data, "passthrough")
#except CvBridgeError as e:
# print(e)
#print "Saved to: ", base_path+str(self.index)+".jpg"
#cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB, cv_image)
#cv2.imwrite('/home/mateo/catkin_ws/src/grasping/src/img1.png', cv_image)#*255)
#self.index += 1
ROBOT_Z = 0
fx = 458.455478616934780
cx = 343.645038678435410
fy = 458.199272745572390
cy = 229.805975111304460
#MODEL_FILE = 'training2_084'
points_out, ang_out, width_out, depth = predict(depthfin)
grasps = grasp.detect_grasps(points_out,
ang_out,
width_img=width_out,
no_grasps=5)
#if grasps == []:
# print('es')
#imagen RGB
#fileim=fname+'.png'
#print(fileim)
#rgb_img = image.Image.from_file(fileim)
#rgb_img.img=cv2.resize(rgb_img.img, (640, 480), cv2.INTER_AREA)
rgbfin = bridge.imgmsg_to_cv2(rgbo)
y_off = 0
x_off = 0
crop_size = crop_size
imh, imw = [480, 640]
rgbcrop = rgbfin[(imh - crop_size) // 2 +
y_off:(imh - crop_size) // 2 + crop_size + y_off,
(imw - crop_size) // 2 +
x_off:(imw - crop_size) // 2 + crop_size + x_off]
fig = plt.figure(figsize=(10, 10))
ax = fig.add_subplot(2, 3, 1)
ax.imshow(depth, cmap='gray')
for g in grasps:
g.plot(ax)
ax.set_title('Depth')
ax.axis('off')
ax = fig.add_subplot(2, 3, 2)
plot = ax.imshow(points_out, cmap='jet', vmin=0, vmax=1)
#grasps[0].plot(ax)
#grasps[1].plot(ax)
ax.set_title('quality')
ax.axis('off')
plt.colorbar(plot)
ax = fig.add_subplot(2, 3, 3)
plot = ax.imshow(width_out, cmap='hsv', vmin=0, vmax=150)
grasps[0].plot(ax)
ax.set_title('width')
ax.axis('off')
plt.colorbar(plot)
ax = fig.add_subplot(2, 3, 5)
plot = ax.imshow(ang_out, cmap='hsv', vmin=-np.pi / 2, vmax=np.pi / 2)
grasps[0].plot(ax)
ax.set_title('ang')
ax.axis('off')
plt.colorbar(plot)
ax = fig.add_subplot(2, 3, 4)
plot = ax.imshow(rgbcrop)
for g in grasps:
g.plot(ax)
ax.set_title('RGB')
ax.axis('off')
#fig.savefig('plot1.png')
#gr=GraspRectangles.load_from_array(grasps)
#evaluation1.plot_output(width_out, depth, points_out , ang_out, no_grasps=1, rgb_img=None)
#ENCONTRAR LA PROFUNDIDAD EN LA IMAGEN ORIGINAL
for i in range(len(grasps)):
print('q', i, ': ', points_out[grasps[i].center[0],
grasps[i].center[1]])
print('pix', i, ': ', grasps[i].center[0], grasps[i].center[1])
print(
'ang', i, ': ',
ang_out[grasps[i].center[0], grasps[i].center[1]] * 180 /
math.pi)
print('width', i, ': ', width_out[grasps[i].center[0],
grasps[i].center[1]])
print(len(grasps))
#PIXEL CON VALOR MAXIMO
#max_pixel = np.array(np.unravel_index(np.argmax(points_out), points_out.shape))
px, py = grasps[0].center
max_pixel = np.array([px, py])
ang = ang_out[max_pixel[0], max_pixel[1]]
width = width_out[max_pixel[0], max_pixel[1]]
print('qf: ', points_out[max_pixel[0], max_pixel[1]])
print('viejopixel: ', max_pixel[0], max_pixel[1])
crop_size = 300
y_off = 0
x_off = 0
#max_pixel = ((np.array(max_pixel) / 440.0 * crop_size) + np.array([(480 - crop_size)//2+y_off, (640 - crop_size) // 2+x_off]))
max_pixel = ((np.array(max_pixel)) +
np.array([(480 - crop_size) // 2 + y_off,
(640 - crop_size) // 2 + x_off]))
max_pixel = np.round(max_pixel).astype(np.int)
#scale=1024/480
scale = 1
#grasps[0].center=[max_pixel[0]*1024/480, max_pixel[1]*1024/640]
grasps[0].center = [max_pixel[0], max_pixel[1]]
print('Nuevopixel: ', max_pixel[0], max_pixel[1])
grasps[0].angle = ang
grasps[0].length = width * scale
grasps[0].width = width * scale / 2
#pfinal = [x, y, z, ang, width, depth_center]
ax = fig.add_subplot(2, 3, 6)
plot = ax.imshow(rgbfin)
grasps[0].plot(ax)
ax.set_title('RGBo')
ax.axis('off')
#ENCONTRAR VALORES REALES DE IMAGEN
#point_depth = depthfin[max_pixel[0], max_pixel[1]]
point_depth = depthfin[px, py]
x = (grasps[0].center[0] - cx) / (fx) * point_depth
y = (grasps[0].center[1] - cy) / (fy) * point_depth
z = point_depth
x1 = (grasps[0].center[0] + width * math.cos(ang) / 2 -
cx) / (fx) * point_depth
y1 = (grasps[0].center[1] + width * math.sin(ang) / 2 -
cy) / (fy) * point_depth
x2 = (grasps[0].center[0] - width * math.cos(ang) / 2 -
cx) / (fx) * point_depth
y2 = (grasps[0].center[1] - width * math.sin(ang) / 2 -
cy) / (fy) * point_depth
rwidth = math.sqrt(math.pow((x1 - x2), 2) + math.pow((y1 - y2), 2))
print('x: ', x)
print('y: ', y)
print('z: ', z)
print('ang: ', ang * 180 / math.pi)
print('width: ', width)
print('rwidth: ', rwidth)
cmd_msg = Float32MultiArray()
cmd_msg.data = [x, y, z, ang, rwidth, point_depth]
cmd_pub.publish(cmd_msg)
plt.show()
def save_results(rgb_img,
grasp_q_img,
grasp_angle_img,
depth_img=None,
no_grasps=1,
grasp_width_img=None,
offset=None,
idx=0):
"""
Plot the output of a network
:param rgb_img: RGB Image
:param depth_img: Depth Image
:param grasp_q_img: Q output of network
:param grasp_angle_img: Angle output of network
:param no_grasps: Maximum number of grasps to plot
:param grasp_width_img: (optional) Width output of network
:return:
"""
gs = detect_grasps(grasp_q_img,
grasp_angle_img,
width_img=grasp_width_img,
no_grasps=no_grasps)
if offset is None:
offset = [0, 0]
# save images for open3D
if depth_img is not None:
out_img = np.concatenate(
[rgb_img, np.expand_dims(depth_img, axis=-1)], axis=-1)
np.save("results/rgbd_{}.npy".format(idx), out_img)
# save the grasp pose
grasps = {}
for i, g in enumerate(gs):
center = [int(g.center[1] + offset[1]), int(g.center[0] + offset[0])]
grasps[i] = {
"center": center,
"angle": float(-g.angle),
"width": float(g.length)
}
with open("results/grasps_{}.json".format(idx), "w") as f:
json.dump(grasps, f)
# fig = plt.figure(figsize=(10, 10))
# plt.ion()
# plt.clf()
# ax = plt.subplot(111)
# ax.imshow(rgb_img)
# ax.set_title('RGB')
# ax.axis('off')
# fig.savefig('results/rgb_{}.png'.format(idx))
# if depth_img.any():
# fig = plt.figure(figsize=(10, 10))
# plt.ion()
# plt.clf()
# ax = plt.subplot(111)
# ax.imshow(depth_img, cmap='gray')
# for g in gs:
# g.plot(ax)
# ax.set_title('Depth')
# ax.axis('off')
# fig.savefig('results/depth_{}.png'.format(idx))
fig = plt.figure(figsize=(10, 10))
plt.ion()
plt.clf()
ax = plt.subplot(111)
ax.imshow(rgb_img)
for g in gs:
g.plot(ax)
ax.set_title('Grasp')
ax.axis('off')
fig.savefig('results/grasp_{}.png'.format(idx))
# fig = plt.figure(figsize=(10, 10))
# plt.ion()
# plt.clf()
# ax = plt.subplot(111)
# plot = ax.imshow(grasp_q_img, cmap='jet', vmin=0, vmax=1)
# ax.set_title('Q')
# ax.axis('off')
# plt.colorbar(plot)
# fig.savefig('results/quality_{}.png'.format(idx))
# fig = plt.figure(figsize=(10, 10))
# plt.ion()
# plt.clf()
# ax = plt.subplot(111)
# plot = ax.imshow(grasp_angle_img, cmap='hsv', vmin=-np.pi / 2, vmax=np.pi / 2)
# ax.set_title('Angle')
# ax.axis('off')
# plt.colorbar(plot)
# fig.savefig('results/angle_{}.png'.format(idx))
# fig = plt.figure(figsize=(10, 10))
# plt.ion()
# plt.clf()
# ax = plt.subplot(111)
# plot = ax.imshow(grasp_width_img, cmap='jet', vmin=0, vmax=100)
# ax.set_title('Width')
# ax.axis('off')
# plt.colorbar(plot)
# fig.savefig('results/width_{}.png'.format(idx))
fig.canvas.draw()
plt.close(fig)
def plot_results(fig,
rgb_img,
grasp_q_img,
grasp_angle_img,
depth_img=None,
no_grasps=1,
grasp_width_img=None):
"""
Plot the output of a network
:param fig: Figure to plot the output
:param rgb_img: RGB Image
:param depth_img: Depth Image
:param grasp_q_img: Q output of network
:param grasp_angle_img: Angle output of network
:param no_grasps: Maximum number of grasps to plot
:param grasp_width_img: (optional) Width output of network
:return:
"""
gs = detect_grasps(grasp_q_img,
grasp_angle_img,
width_img=grasp_width_img,
no_grasps=no_grasps)
plt.ion()
plt.clf()
ax = fig.add_subplot(2, 3, 1)
ax.imshow(rgb_img)
ax.set_title('RGB')
ax.axis('off')
if depth_img is not None:
ax = fig.add_subplot(2, 3, 2)
ax.imshow(depth_img, cmap='gray')
ax.set_title('Depth')
ax.axis('off')
ax = fig.add_subplot(2, 3, 3)
ax.imshow(rgb_img)
for g in gs:
g.plot(ax)
ax.set_title('Grasp')
ax.axis('off')
ax = fig.add_subplot(2, 3, 4)
plot = ax.imshow(grasp_q_img, cmap='jet', vmin=0, vmax=1)
ax.set_title('Q')
ax.axis('off')
plt.colorbar(plot)
ax = fig.add_subplot(2, 3, 5)
plot = ax.imshow(grasp_angle_img,
cmap='hsv',
vmin=-np.pi / 2,
vmax=np.pi / 2)
ax.set_title('Angle')
ax.axis('off')
plt.colorbar(plot)
ax = fig.add_subplot(2, 3, 6)
plot = ax.imshow(grasp_width_img, cmap='jet', vmin=0, vmax=100)
ax.set_title('Width')
ax.axis('off')
plt.colorbar(plot)
plt.pause(0.1)
fig.canvas.draw()
#np.savetxt("array1.txt", depthfin, fmt="%s")
#depthfin=cv2.GaussianBlur(depthfin,(5,5),0)
#depthn=cv2.blur(depth1,(5,5))
#print(depthfin.shape)
#depthfin=image.DepthImage.from_tiff(depthorig).img
#here = path.dirname(path.abspath(__file__))
#sys.path.append(here)
#print(path.join(path.dirname(__file__), MODEL_FILE))
#model = torch.load(path.join(path.dirname(__file__), MODEL_FILE))
model = torch.load(MODEL_FILE, map_location='cpu')
device = torch.device('cpu')
points_out, ang_out, width_out, depth = predict(depthfin)
#print(points_out)
#print(ang_out.shape)
#print(width_out.shape)
grasps = grasp.detect_grasps(points_out, ang_out, width_img=width_out, no_grasps=5)
#if grasps == []:
# print('es')
#imagen RGB
fileim=fname+'.png'
print(fileim)
rgb_img = image.Image.from_file(fileim)
#rgb_img.img=cv2.resize(rgb_img.img, (640, 480), cv2.INTER_AREA)
y_off=0
x_off=0
rgb_img.img = rgb_img.img[20+y_off:460+y_off,100+x_off:540+x_off]
fig = plt.figure(figsize=(10, 10))
ax = fig.add_subplot(2, 3, 1)
ax.imshow(depth, cmap='gray')
for g in grasps:
def __init__(self, args):
print "aquip"
self.index = 0
if len(sys.argv) > 1:
self.index = int(sys.argv[1])
rospy.init_node('save_img')
bridge = CvBridge()
crop_size = 400
#while not rospy.is_shutdown():
raw_input()
#color_sub = rospy.Subscriber("/camera/color/image_raw",Image)
print "depth"
rgbo = rospy.wait_for_message('/camera/color/image_raw', Image)
#depth_sub = rospy.Subscriber("/camera/depth/image_raw",Image)
print "depth"
cmd_pub = rospy.Publisher('ggcnn/out/command1',
Float32MultiArray,
queue_size=1)
#rgbo = rospy.wait_for_message('/camera/color/image_raw1', Image)
deptho = rospy.wait_for_message('/camera/depth/image_raw', Image)
# rgbo = rospy.wait_for_message('/camera/rgb/image_color', Image)
rgbfin = bridge.imgmsg_to_cv2(rgbo)
depthfin = bridge.imgmsg_to_cv2(deptho)
rgbfin1 = cv2.cvtColor(rgbfin, cv2.COLOR_BGR2RGB)
cv2.imwrite('rgb.png', rgbfin1)
ROBOT_Z = 0
#distancia focal
fx = 458.455478616934780
fy = 458.199272745572390
#centro de camara
cx = 343.645038678435410
cy = 229.805975111304460
#MODEL_FILE = 'training2_084'
points_out, ang_out, width_out, depth = predict(depthfin)
grasps = grasp.detect_grasps(points_out,
ang_out,
0.7,
width_img=width_out,
no_grasps=5)
y_off = 0
x_off = 0
crop_size = 400
imh, imw = [480, 640]
rgbcrop = rgbfin[(imh - crop_size) // 2 +
y_off:(imh - crop_size) // 2 + crop_size + y_off,
(imw - crop_size) // 2 +
x_off:(imw - crop_size) // 2 + crop_size + x_off]
fig = plt.figure(figsize=(10, 10))
ax = fig.add_subplot(2, 2, 1)
ax.imshow(depth, cmap='gray')
for g in grasps:
g.plot(ax)
ax.set_title('Depth')
ax.axis('off')
ax = fig.add_subplot(2, 2, 2)
plot = ax.imshow(points_out, cmap='jet', vmin=0, vmax=1)
#grasps[0].plot(ax)
#grasps[1].plot(ax)
ax.set_title('quality')
ax.axis('off')
#plt.colorbar(plot)
ax = fig.add_subplot(2, 2, 3)
plot = ax.imshow(width_out, cmap='hsv', vmin=0, vmax=150)
grasps[0].plot(ax)
ax.set_title('width')
ax.axis('off')
plt.colorbar(plot)
# image = cv2.circle(rgbcrop, (120, 50), 20, (255, 0, 0), 2)
# ax = fig.add_subplot(2, 2, 4)
# plot = ax.imshow(rgbcrop)
# #for g in grasps:
# # g.plot(ax)
#
# ax.set_title('RGB')
# ax.axis('off')
#fig.savefig('plot1.png')
#gr=GraspRectangles.load_from_array(grasps)
#evaluation1.plot_output(width_out, depth, points_out , ang_out, no_grasps=1, rgb_img=None)
#ENCONTRAR LA PROFUNDIDAD EN LA IMAGEN ORIGINAL
for i in range(len(grasps)):
print('q', i, ': ', points_out[grasps[i].center[0],
grasps[i].center[1]])
print('pix', i, ': ', grasps[i].center[0], grasps[i].center[1])
print(
'ang', i, ': ',
ang_out[grasps[i].center[0], grasps[i].center[1]] * 180 /
math.pi)
print('width', i, ': ', width_out[grasps[i].center[0],
grasps[i].center[1]])
print(len(grasps))
#PIXEL CON VALOR MAXIMO
#max_pixel = np.array(np.unravel_index(np.argmax(points_out), points_out.shape))
px, py = grasps[0].center
max_pixel = np.array([px, py])
ang = ang_out[max_pixel[0], max_pixel[1]]
width = width_out[max_pixel[0], max_pixel[1]]
# print('qf: ',points_out[max_pixel[0], max_pixel[1]])
# print('viejopixel: ',max_pixel[0], max_pixel[1])
crop_size = 400
y_off = 0
x_off = 0
#max_pixel = ((np.array(max_pixel) / 440.0 * crop_size) + np.array([(480 - crop_size)//2+y_off, (640 - crop_size) // 2+x_off]))
max_pixel = ((np.array(max_pixel)) +
np.array([(480 - crop_size) // 2 + y_off,
(640 - crop_size) // 2 + x_off]))
max_pixel = np.round(max_pixel).astype(np.int)
#scale=1024/480
scale = 1
#grasps[0].center=[max_pixel[0]*1024/480, max_pixel[1]*1024/640]
grasps[0].center = [max_pixel[0], max_pixel[1]]
# print('Nuevopixel: ',max_pixel[0], max_pixel[1])
grasps[0].angle = ang
grasps[0].length = width * scale
grasps[0].width = width * scale / 2
#pfinal = [x, y, z, ang, width, depth_center]
point_depth = depthfin[px, py]
x = (grasps[0].center[0] - cx) / (fx) * point_depth
y = (grasps[0].center[1] - cy) / (fy) * point_depth
z = point_depth
x1 = (grasps[0].center[0] + width * math.cos(ang) / 2 -
cx) / (fx) * point_depth
y1 = (grasps[0].center[1] + width * math.sin(ang) / 2 -
cy) / (fy) * point_depth
x2 = (grasps[0].center[0] - width * math.cos(ang) / 2 -
cx) / (fx) * point_depth
y2 = (grasps[0].center[1] - width * math.sin(ang) / 2 -
cy) / (fy) * point_depth
rwidth = math.sqrt(math.pow((x1 - x2), 2) + math.pow((y1 - y2), 2))
print('x: ', x)
print('y: ', y)
print('z: ', z)
print('ang: ', ang * 180 / math.pi)
print('width: ', width)
print('rwidth: ', rwidth)
punto = gmsg.Pose()
#invertidos porque si
punto.position.x = y
punto.position.y = x
punto.position.z = 0
print punto
print convert_pose(punto, "cam", "world")
image = cv2.circle(rgbcrop, (144, 199), 20, (255, 0, 0), 2)
ax = fig.add_subplot(2, 2, 4)
plot = ax.imshow(rgbcrop)
#for g in grasps:
# g.plot(ax)
ax.set_title('RGB')
ax.axis('off')
plt.show()
def find_pose():
print('hola')
#color_sub = rospy.Subscriber("/camera/color/image_raw",Image)
rgbo = rospy.wait_for_message('/camera/rgb/image_rect_color', Image)
print "depth"
deptho = rospy.wait_for_message('/camera/depth/image_raw', Image)
# rgbo = rospy.wait_for_message('/camera/rgb/image_color', Image)
depthfin = bridge.imgmsg_to_cv2(deptho)
rgbfin = bridge.imgmsg_to_cv2(rgbo)
rgb1 = cv2.cvtColor(rgbfin, cv2.COLOR_BGR2RGB)
#(y, x) = np.where(out2 == 255)
#(topy, topx) = (np.min(y), np.min(x))
#(bottomy, bottomx) = (np.max(y), np.max(x))
#iy1=topy-13
#ix1=topx-16
#Dy1=bottomy-iy1+13
#Dx1=bottomx-ix1+16
#print('Calibracion, iy1, ix1, Dy1, Dx1:',iy1 , ix1, Dy1, Dx1 )
#iy, ix, Dy, Dx, widthbinx, widthbiny, iy1, ix1, Dy1, Dx1 = calibracionKinect.calibracion(depthfin, rgbfin)
#print(xx)
iy, ix, Dy, Dx, widthbinx, widthbiny, iy1, ix1, Dy1, Dx1 = [
110, 208, 180, 286, 286, 180, 0, 0, 180, 286
]
#depthfin1= depthfin.copy()
depthfin1 = depthfin[iy:Dy + iy, ix:Dx + ix]
depthfin1 = depthfin1 / 2
#fig = plt.figure(figsize=(10, 10))
#ax = fig.add_subplot(1, 1, 1)
#ax.imshow(depthfin1, cmap='gray')
#ax.set_title('Depth')
#ax.axis('off')
#plt.show()
#cv2.imwrite('rgb.png', rgbfin1)
#MODEL_FILE = 'training2_084'
points_out, ang_out, width_out, depth = predict(depthfin1)
grasps = grasp.detect_grasps(points_out,
ang_out,
0.7,
width_img=width_out,
no_grasps=5)
if grasps:
maxgrasps = evaluation1.plot_output(width_out, depth, points_out,
ang_out, grasps, rgbfin, crop_size,
y_off, x_off)
#m = evaluation1.plot_output(width_out, depth, points_out, ang_out, grasps, rgbfin, crop_size, y_off, x_off)
#fig.savefig('plot1.png')
#ENCONTRAR LA PROFUNDIDAD EN LA IMAGEN ORIGINAL
#PIXEL CON VALOR MAXIMO
pushlist, grasps = pushing(grasps, ix1, iy1, Dx1, Dy1, Dx, Dy)
grasps = graspdata(points_out, depthfin, grasps, ix, iy, Dx, Dy)
#x, y, z, ang, rwidth =rvalues(grasps[maxgrasps[0][1]], depthfin, Dx, Dy)
punto1 = geometry_msgs.msg.Pose()
arr = []
if not grasps:
arr = [0, 0, 0, 0, 0, 6]
elif grasps:
for i in range(len(grasps)):
gqmax = maxgrasps[i][1]
mov = pushlist[gqmax]
x, y, z, ang, rwidth = rvalues(grasps[gqmax], depthfin, Dx, Dy)
arr.append(x)
arr.append(y)
arr.append(z)
arr.append(ang)
arr.append(rwidth)
arr.append(mov)
punto1.position.x = x
punto1.position.y = y
punto1.position.z = z
q = tft.quaternion_from_euler(0, 1.5, ang)
punto1.orientation.x = q[0]
punto1.orientation.y = q[1]
punto1.orientation.z = q[2]
punto1.orientation.w = q[3]
punto2 = convert_pose(punto1, "cam", "world")
#print('punto 1: ', punto1)
print('punto 2: ', punto2)
#print('arr', arr)
cmd_msg = Float32MultiArray()
cmd_msg.data = arr #print('publicado lol: ', cmd_msg)
cmd_pub.publish(cmd_msg)
print(arr)
# t = tf2.Transformer(True, rospy.Duration(10.0))
#t.setTransform(m)
#print( t.lookupTransform('cam', 'world', rospy.Time(0)))
cont = 0
fig = plt.figure(figsize=(10, 10))
ax = fig.add_subplot(1, 2, 1)
ax.imshow(depthfin, cmap='gray')
if grasps:
for g in grasps:
g.plot(ax)
if pushlist[cont] == 1:
ax.arrow(g.center[1],
g.center[0],
50,
0,
head_width=8,
head_length=10,
fc='lightblue',
ec='red')
if pushlist[cont] == 2:
ax.arrow(g.center[1],
g.center[0],
-50,
0,
head_width=8,
head_length=10,
fc='lightblue',
ec='red')
if pushlist[cont] == 3:
ax.arrow(g.center[1],
g.center[0],
0,
50,
head_width=8,
head_length=10,
fc='lightblue',
ec='red')
if pushlist[cont] == 4:
ax.arrow(g.center[1],
g.center[0],
0,
-50,
head_width=8,
head_length=10,
fc='lightblue',
ec='red')
cont = cont + 1
ax.set_title('Depth')
ax.axis('off')
#ax = fig.add_subplot(1, 2, 2)
#plot = ax.imshow(depthfin1, cmap='gray')
#ax.set_title('quality')
#ax.axis('off')
ax = fig.add_subplot(1, 2, 2)
plot = ax.imshow(points_out, cmap='jet', vmin=0, vmax=1)
ax.set_title('quality')
ax.axis('off')
plt.show()
#fig = plt.figure(figsize=(10, 10))
#ax = fig.add_subplot(1, 1, 1)
#plot = ax.imshow(width_out, cmap='hsv', vmin=0, vmax=150)
#ax.set_title('width')
#ax.axis('off')
#plt.colorbar(plot)
#plt.show()
#rospy.spin()
#fig = plt.figure(figsize=(10, 10))
#ax = fig.add_subplot(1, 1, 1)
#plot = ax.imshow(ang_out, cmap='hsv', vmin=-np.pi / 2, vmax=np.pi / 2)
#ax.set_title('Angle')
#ax.axis('off')
#plt.colorbar(plot)
plt.show()
def save_results(rgb_img,
grasp_q_img,
grasp_angle_img,
depth_img=None,
no_grasps=1,
grasp_width_img=None):
"""
Plot the output of a network
:param rgb_img: RGB Image
:param depth_img: Depth Image
:param grasp_q_img: Q output of network
:param grasp_angle_img: Angle output of network
:param no_grasps: Maximum number of grasps to plot
:param grasp_width_img: (optional) Width output of network
:return:
"""
gs = detect_grasps(grasp_q_img,
grasp_angle_img,
width_img=grasp_width_img,
no_grasps=no_grasps)
fig = plt.figure(figsize=(10, 10))
plt.ion()
plt.clf()
ax = plt.subplot(111)
ax.imshow(rgb_img)
ax.set_title('RGB')
ax.axis('off')
fig.savefig('results/rgb.png')
if depth_img.any():
fig = plt.figure(figsize=(10, 10))
plt.ion()
plt.clf()
ax = plt.subplot(111)
ax.imshow(depth_img, cmap='gray')
for g in gs:
g.plot(ax)
ax.set_title('Depth')
ax.axis('off')
fig.savefig('results/depth.png')
fig = plt.figure(figsize=(10, 10))
plt.ion()
plt.clf()
ax = plt.subplot(111)
ax.imshow(rgb_img)
for g in gs:
g.plot(ax)
ax.set_title('Grasp')
ax.axis('off')
fig.savefig('results/grasp.png')
fig = plt.figure(figsize=(10, 10))
plt.ion()
plt.clf()
ax = plt.subplot(111)
plot = ax.imshow(grasp_q_img, cmap='jet', vmin=0, vmax=1)
ax.set_title('Q')
ax.axis('off')
plt.colorbar(plot)
fig.savefig('results/quality.png')
fig = plt.figure(figsize=(10, 10))
plt.ion()
plt.clf()
ax = plt.subplot(111)
plot = ax.imshow(grasp_angle_img,
cmap='hsv',
vmin=-np.pi / 2,
vmax=np.pi / 2)
ax.set_title('Angle')
ax.axis('off')
plt.colorbar(plot)
fig.savefig('results/angle.png')
fig = plt.figure(figsize=(10, 10))
plt.ion()
plt.clf()
ax = plt.subplot(111)
plot = ax.imshow(grasp_width_img, cmap='jet', vmin=0, vmax=100)
ax.set_title('Width')
ax.axis('off')
plt.colorbar(plot)
fig.savefig('results/width.png')
fig.canvas.draw()
plt.close(fig)
