def callback(msg_1): global i global j global outlist j = j + 1 if j == 1: i = i + 1 # resize and crop image cv2_msg_img = bridge.imgmsg_to_cv2(msg_1) cv_imgc = bridge.cv2_to_imgmsg(cv2_msg_img, 'rgb8') pil_img = encode(cv_imgc) fg_img = PILImage.new('RGBA', pil_img.size, (0, 0, 0, 255)) draw = ImageDraw.Draw(fg_img) draw.ellipse(XYc, fill=(0, 0, 0, 0)) # 中心を抜く pil_img.paste(fg_img, (0, 0), fg_img.split()[3]) img_msg = decode(pil_img) cv2_imgd = bridge.imgmsg_to_cv2(img_msg, 'rgb8') cv_cutimg = cv2_imgd[yc - xyoffset:yc + xyoffset, xc - xyoffset:xc + xyoffset] cv_cutimg = cv2.transpose(cv_cutimg) cv_cutimg = cv2.flip(cv_cutimg, 1) # cv_resize1 = cv2.resize(cv_cutimg, (rsizex, rsizey)) cv_resizex = cv_resize1.transpose(2, 0, 1) in_imgcc1 = np.array([cv_resizex], dtype=np.float32) in_img1 = (in_imgcc1 - 128) / 128 # # in_imgg = cuda.to_gpu(in_img1) # to gpu img_real = Variable(in_imgg) # with chainer.using_config('train', False), chainer.no_backprop_mode(): img_gen = gen(invg(img_real)) dis_real = dis(img_real) dis_gen = dis(img_gen) output = fl(img_real - img_gen, dis_real - dis_gen, dis_real) #traversable probablity out_gonogoc = cuda.to_cpu(output.data) # to cpu gonet_out.publish(out_gonogoc) #publish j = 0
def preprocess_image(msg): cv2_msg_img_L = bridge.imgmsg_to_cv2(msg) cv_imgc_L = bridge.cv2_to_imgmsg(cv2_msg_img_L, 'rgb8') pil_img_L = encode(cv_imgc_L) fg_img = PILImage.new('RGBA', pil_img_L.size, (0, 0, 0, 255)) draw = ImageDraw.Draw(fg_img) draw.ellipse(XYf, fill=(0, 0, 0, 0)) draw.ellipse(XYb, fill=(0, 0, 0, 0)) pil_img_L.paste(fg_img, (0, 0), fg_img.split()[3]) img_msg_L = decode(pil_img_L) cv2_img_L = bridge.imgmsg_to_cv2(img_msg_L, 'rgb8') cv_cutimg_F = cv2_img_L[yc - xyoffset:yc + xyoffset, xc - xyoffset:xc + xyoffset] cv_cutimg_B = cv2_img_L[yc - xyoffset:yc + xyoffset, xc + xplus - xyoffset:xc + xplus + xyoffset] cv_cutimg_FF = cv2.transpose(cv_cutimg_F) cv_cutimg_F = cv2.flip(cv_cutimg_FF, 1) cv_cutimg_Bt = cv2.transpose(cv_cutimg_B) cv_cutimg_B = cv2.flip(cv_cutimg_Bt, 0) cv_cutimg_BF = cv2.flip(cv_cutimg_Bt, -1) #resize image cv_resize_F = cv2.resize(cv_cutimg_F, (rsizex, rsizey)) #front image cv_resize_B = cv2.resize(cv_cutimg_B, (rsizex, rsizey)) #back image cv_resize_n = np.concatenate((cv_resize_F, cv_resize_B), axis=1) cv_resizex = cv_resize_n.transpose(2, 0, 1) in_imgcc1 = np.array([cv_resizex], dtype=np.float32) in_img1 = (in_imgcc1 - 128) / 128 img_nn_cL = mask_brrc * (in_img1 + 1.0) - 1.0 #mask img = img_nn_cL.astype(np.float32) return img
def callback(msg_1): global i global j #global ini global xcgg global lcg, lhg global vel_res, wvel_res #linear velocity, and angular velocity global img_nn_c global out_gonet global vres, wres global xc, yc, xyoffset j = j + 1 if j == 1: xkg = xcgg #previous image xd = np.zeros((3, 3, 128, 128), dtype=np.float32) # resize and crop image for msg_1 cv2_msg_img = bridge.imgmsg_to_cv2(msg_1) cv_imgc = bridge.cv2_to_imgmsg(cv2_msg_img, 'rgb8') pil_img = encode(cv_imgc) fg_img = PILImage.new('RGBA', pil_img.size, (0, 0, 0, 255)) draw = ImageDraw.Draw(fg_img) draw.ellipse(XYc, fill=(0, 0, 0, 0)) pil_img.paste(fg_img, (0, 0), fg_img.split()[3]) img_msg = decode(pil_img) cv2_imgd = bridge.imgmsg_to_cv2(img_msg, 'rgb8') #crop the image cv_cutimg = cv2_imgd[yc - xyoffset:yc + xyoffset, xc - xyoffset:xc + xyoffset] cv_cutimg = cv2.transpose(cv_cutimg) cv_cutimg = cv2.flip(cv_cutimg, 1) #resize the image 3x128x128 cv_resize1 = cv2.resize(cv_cutimg, (rsizex, rsizey)) cv_resizex = cv_resize1.transpose(2, 0, 1) in_imgcc1 = np.array([cv_resizex], dtype=np.float32) #normalization in_img1 = (in_imgcc1 - 128) / 128 for i in range(batchsize): #usually batchsize=1 img_nn_c[i] = in_img1 vresc[i][0][0][0] = vel_res wresc[i][0][0][0] = wvel_res xcg = Variable(cuda.to_gpu(img_nn_c)) vrescg = Variable(cuda.to_gpu(vresc)) wrescg = Variable(cuda.to_gpu(wresc)) #designing the virtual velocity from joypad input vel_ref(linear velocity) and wvel_ref(angular velocity) vres[0][0][0] = 0.5 if wvel_ref == 0.0: wres[0][0][0] = 0.0 elif wvel_ref != 0.0 and vel_ref == 0.0: if wvel_ref > 0.0: wres[0][0][0] = 1.0 else: wres[0][0][0] = -1.0 elif vel_ref < 0.0: wres[0][0][0] = 0.0 else: rd = 2.5 * vel_ref / wvel_ref wres[0][0][0] = 0.5 / rd if wres[0][0][0] > 1.0: wres[0][0][0] = 1.0 elif wres[0][0][0] < -1.0: wres[0][0][0] = -1.0 vresg = Variable(cuda.to_gpu(vres)) #virtual linear velocity wresg = Variable(cuda.to_gpu(wres)) #virtual angular velocity fl.l_LSTM.h = Variable(lhg) #internal value of LSTM fl.l_LSTM.c = Variable(lcg) #internal value of LSTM with chainer.using_config('train', False), chainer.no_backprop_mode(): img_gen = gen(invg(xcg)) dis_real = dis(xcg) dis_gen = dis(img_gen) output = fl(xcg - img_gen, dis_real - dis_gen, dis_real) out_gonet[0] = np.reshape(cuda.to_cpu(output.data), (batchsize)) lhg = fl.l_LSTM.h.data lcg = fl.l_LSTM.c.data xcgg = xcg #keep current image for the next step font = ImageFont.truetype( "/usr/share/fonts/truetype/freefont/FreeMonoBold.ttf", 25) stext = 35 for k in range(4): # 4steps prediction if k == 0: xkg = xkg xcg = xcg else: xkg = xcg xcg = xap #future prediction:start with chainer.using_config('train', False), chainer.no_backprop_mode(): if k == 0: x = dec(enc(xkg), vrescg, wrescg) else: x = dec(enc(xkg), vresg, wresg) genk = F.spatial_transformer_sampler(xkg, x) xkp = genk with chainer.using_config('train', False), chainer.no_backprop_mode(): xcf = dec(enc(xcg), vresg, wresg) xkf = dec(enc(xkp), vresg, wresg) gencf = F.spatial_transformer_sampler(xcg, xcf) genkf = F.spatial_transformer_sampler(xkp, xkf) indata = F.concat((genkf, gencf), axis=1) maskg = Variable(cuda.to_gpu(mask_batch)) with chainer.using_config('train', False), chainer.no_backprop_mode(): zL = encD(indata) xfLs = decD(zL) xfL = F.separate(xfLs, axis=1) apf0 = F.reshape(xfL[0], (batchsize, 1, 128, 128)) apf1 = F.reshape(xfL[1], (batchsize, 1, 128, 128)) apf2 = F.reshape(xfL[2], (batchsize, 1, 128, 128)) apf3 = F.reshape(xfL[3], (batchsize, 1, 128, 128)) mask_kL = F.reshape(xfL[4], (batchsize, 1, 128, 128)) mask_cL = F.reshape(xfL[5], (batchsize, 1, 128, 128)) apfLc = F.concat((apf0, apf1), axis=1) apfLk = F.concat((apf2, apf3), axis=1) genLcx = F.spatial_transformer_sampler(gencf, apfLc + maskg) genLkx = F.spatial_transformer_sampler(genkf, apfLk + maskg) maskL = F.concat((mask_kL, mask_cL), axis=1) mask_softL = F.softmax(maskL, axis=1) mask_sepL = F.separate(mask_softL, axis=1) mask_knL = F.reshape(mask_sepL[0], (batchsize, 1, 128, 128)) mask_cnL = F.reshape(mask_sepL[1], (batchsize, 1, 128, 128)) xap = F.scale(genLcx, mask_cnL, axis=0) + F.scale( genLkx, mask_knL, axis=0) #predicted image #GONet with chainer.using_config('train', False), chainer.no_backprop_mode(): img_gen = gen(invg(xap)) dis_real = dis(xap) dis_gen = dis(img_gen) output = fl(xap - img_gen, dis_real - dis_gen, dis_real) out_gonet[k + 1] = np.reshape(cuda.to_cpu(output.data), (batchsize)) #showing predicted image and traversable probability by GONet out_imgc = img_nn_c imgb = np.fmin(255.0, np.fmax(0.0, out_imgc * 128 + 128)) imgc = np.reshape(imgb, (3, 128, 128)) imgd = imgc.transpose(1, 2, 0) imge = imgd.astype(np.uint8) imgm = bridge.cv2_to_imgmsg(imge) imgg = PILImage.new('RGB', (128, 30)) d = ImageDraw.Draw(imgg) d.text((stext, 0), str(np.round(out_gonet[0][0], 2)), fill=(int(255 * (1.0 - out_gonet[0][0])), int(255 * out_gonet[0][0]), 0), font=font) imgtext = decode(imgg) imgtextcv = bridge.imgmsg_to_cv2(imgtext, 'bgr8') imgtextcvx = imgtextcv.transpose(2, 0, 1) imgc = np.concatenate((imgc, imgtextcvx), axis=1) # imgcc0 = imgc for im in range(1): out_imgc = cuda.to_cpu(xap.data[im]) imgb = np.fmin(255.0, np.fmax(0.0, out_imgc * 128 + 128)) imgc = np.reshape(imgb, (3, 128, 128)) if k == 0: if im == 0: imgg = PILImage.new('RGB', (128, 30)) d = ImageDraw.Draw(imgg) d.text((stext, 0), str(np.round(out_gonet[k + 1][im], 2)), fill=(int(255 * (1.0 - out_gonet[k + 1][im])), int(255 * out_gonet[k + 1][im]), 0), font=font) imgtext = decode(imgg) imgtextcv = bridge.imgmsg_to_cv2(imgtext, 'bgr8') imgtextcvx = imgtextcv.transpose(2, 0, 1) imgc = np.concatenate((imgc, imgtextcvx), axis=1) # imgcc1 = imgc elif k == 1: if im == 0: imgg = PILImage.new('RGB', (128, 30)) d = ImageDraw.Draw(imgg) d.text((stext, 0), str(np.round(out_gonet[k + 1][im], 2)), fill=(int(255 * (1.0 - out_gonet[k + 1][im])), int(255 * out_gonet[k + 1][im]), 0), font=font) imgtext = decode(imgg) imgtextcv = bridge.imgmsg_to_cv2(imgtext, 'bgr8') imgtextcvx = imgtextcv.transpose(2, 0, 1) imgc = np.concatenate((imgc, imgtextcvx), axis=1) # imgcc2 = imgc elif k == 2: if im == 0: imgg = PILImage.new('RGB', (128, 30)) d = ImageDraw.Draw(imgg) d.text((stext, 0), str(np.round(out_gonet[k + 1][im], 2)), fill=(int(255 * (1.0 - out_gonet[k + 1][im])), int(255 * out_gonet[k + 1][im]), 0), font=font) imgtext = decode(imgg) imgtextcv = bridge.imgmsg_to_cv2(imgtext, 'bgr8') imgtextcvx = imgtextcv.transpose(2, 0, 1) imgc = np.concatenate((imgc, imgtextcvx), axis=1) # imgcc3 = imgc elif k == 3: if im == 0: imgg = PILImage.new('RGB', (128, 30)) d = ImageDraw.Draw(imgg) d.text((stext, 0), str(np.round(out_gonet[k + 1][im], 2)), fill=(int(255 * (1.0 - out_gonet[k + 1][im])), int(255 * out_gonet[k + 1][im]), 0), font=font) imgtext = decode(imgg) imgtextcv = bridge.imgmsg_to_cv2(imgtext, 'bgr8') imgtextcvx = imgtextcv.transpose(2, 0, 1) imgc = np.concatenate((imgc, imgtextcvx), axis=1) # imgcc4 = imgc imgcc = np.concatenate((imgcc0, imgcc1, imgcc2, imgcc3, imgcc4), axis=2) imgdd = imgcc.transpose(1, 2, 0) imgee = imgdd.astype(np.uint8) imgmm = bridge.cv2_to_imgmsg(imgee) image_all.publish(imgmm) j = 0