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