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)