def draw(self):
if self.should_render:
if self.images is not None:
utils.draw_image(self.images[self.image_state], self.x, self.y)
else:
utils.draw_rect(self.color, self.x, self.y, self.width,
self.height)
def draw(self, surface: pygame.Surface):
if self.sprite_counter >= self.num_sprites * UPDATE_CONST:
self.sprite_counter = 0
sprite = self.get_sprite()
draw_image(surface, sprite, *self.position)
self.sprite_counter += 1
def test_wm():
wm = Watermark('/data/yuming/watermark-data/watermark.mat')()
writer = tf.summary.FileWriter('model-output', tf.get_default_graph())
with tf.Session() as (sess):
sess.run([tf.global_variables_initializer(), tf.local_variables_initializer()])
wm_val = sess.run(wm)
np.set_printoptions(threshold=(np.nan))
print(wm_val.shape)
images = [
{'data':np.squeeze(wm_val[0, :, :, :].astype(np.uint8)), 'title':'watermark'}]
image_tensor = draw_image(images)
image_str = draw_image(images)
writer.add_summary(image_str, global_step=0)
writer.close()
def test_filtimage():
original_image = Image('/data/yuming/watermark-data/image_paths.mat', 10)()
filtimage = FiltImage(freq='high')
filtered_image = filtimage(original_image)
writer = tf.summary.FileWriter('model-output', tf.get_default_graph())
with tf.Session() as (sess):
sess.run([
tf.global_variables_initializer(),
tf.local_variables_initializer()
])
original_image_val, filtered_image_val = sess.run(
[original_image, filtered_image])
images = [{
'data':
np.squeeze(original_image_val[0, :, :, :].astype(np.uint8)),
'title':
'original image'
}, {
'data':
np.squeeze(filtered_image_val[0, :, :, :].astype(np.uint8)),
'title':
'filtered image'
}]
image_str = draw_image(images)
writer.add_summary(image_str, global_step=0)
writer.close()
def test_noiseimage():
original_image = Image('/data/yuming/watermark-data/image_paths.mat', 10)()
noiseimage = NoiseImage(
[1, FLAGS.img_height, FLAGS.img_width, FLAGS.num_chans])
degraded_image = noiseimage(original_image)
writer = tf.summary.FileWriter('model-output', tf.get_default_graph())
with tf.Session() as (sess):
sess.run([
tf.global_variables_initializer(),
tf.local_variables_initializer()
])
original_image_val, degraded_image_val = sess.run(
[original_image, degraded_image])
images = [{
'data':
np.squeeze(original_image_val[0, :, :, :].astype(np.uint8)),
'title':
'original image'
}, {
'data':
np.squeeze(degraded_image_val[0, :, :, :].astype(np.uint8)),
'title':
'degraded image'
}]
image_str = draw_image(images)
writer.add_summary(image_str, global_step=0)
writer.close()
def test_freqimage():
from config import FLAGS
original_image = Image('/data/yuming/watermark-data/image_paths.mat', 10)()
mask = Mask(FLAGS.img_height, FLAGS.img_width, 64)()
mask = tf.cast(mask, tf.complex64)
freqimage = FreqImage(mask)
blurred_image = freqimage(original_image)
writer = tf.summary.FileWriter('model-output', tf.get_default_graph())
with tf.Session() as (sess):
sess.run([
tf.global_variables_initializer(),
tf.local_variables_initializer()
])
original_image_val, blurred_image_val = sess.run(
[original_image, blurred_image])
images = [{
'data':
np.squeeze(original_image_val[0, :, :, :].astype(np.uint8)),
'title':
'original image'
}, {
'data':
np.squeeze(blurred_image_val[0, :, :, :].astype(np.uint8)),
'title':
'blurred image'
}]
image_str = draw_image(images)
writer.add_summary(image_str, global_step=0)
writer.close()
def prediction_process(args, action_queue, experience_queue, work, ready, can_predict, should_reset, iteration, path_queue):
# Setup model
ts = time.time()
first = True
reward = 5.0
discount_factor = 0.5
path = path_queue.get()
image_path = path[0]; depth_path = path[1]; pc_path = path[2]; vis_path = path[3]; mixed_paths = path[4]; feat_paths = path[5]
trainer = Trainer(reward, discount_factor, False, args.primitive_lr, args.densenet_lr)
trainer.behavior_net.load_state_dict(torch.load(args.model))
trainer.target_net.load_state_dict(trainer.behavior_net.state_dict())
ready.value = True
cv2.namedWindow("prediction")
print("[Prediction Thread] Load model took %f seconds. Start prediction thread" %(time.time()-ts))
while work.value:
if should_reset.value:
print("[Prediction Thread] Receive reset command")
if first:
print("[Prediction Thread] Already in initial state, abort reset request...")
should_reset.value = False
ready.value = True
continue
ts = time.time()
ready.value = False
trainer.behavior_net.load_state_dict(torch.load(args.model))
first = True
path = path_queue.get()
image_path = path[0]; depth_path = path[1]; pc_path = path[2]; vis_path = path[3]; mixed_paths = path[4]; feat_paths = path[5]
print("[Prediction Thread] Reset complete! Took {} seconds".format(time.time()-ts))
should_reset.value = False
ready.value = True
continue
if not first:
while experience_queue.empty() and not should_reset.value and work.value:
pass
if not experience_queue.empty():
print("[Prediction Thread] Got experience, updating network...")
transition = experience_queue.get()
color = cv2.imread(transition.color)
depth = np.load(transition.depth)
next_color = cv2.imread(transition.next_color)
next_depth = np.load(transition.next_depth)
pixel_index = transition.pixel_idx
td_target = trainer.get_label_value(transition.reward, next_color, next_depth, transition.is_empty, pixel_index[0])
trainer.backprop(color, depth, pixel_index, td_target, 1.0, 1, True, True)
if can_predict.value:
if first: first = False
print("[Prediction Thread] Start prediction")
pc_response = _get_pc(iteration.value, True, pc_path)
color, depth, points = utils.get_heightmap(pc_response.pc, image_path, depth_path, iteration.value)
suck_1_prediction, suck_2_prediction, grasp_prediction = trainer.forward(color, depth, is_volatile=True)
heatmaps, mixed_imgs = utils.save_heatmap_and_mixed(suck_1_prediction, suck_2_prediction, grasp_prediction, feat_paths, mixed_paths, color, iteration.value)
action, action_str, pixel_index, angle = utils.greedy_policy(suck_1_prediction, suck_2_prediction, grasp_prediction)
visual_img = utils.draw_image(mixed_imgs[pixel_index[0]], False, pixel_index, vis_path + "vis_{:06}.jpg".format(iteration.value))
cv2.imshow("prediction", cv2.resize(visual_img, None, fx=2, fy=2)); cv2.waitKey(33)
utils.print_action(action_str, pixel_index, points[pixel_index[1], pixel_index[2]])
action_queue.put([action, action_str, points[pixel_index[1], pixel_index[2]], angle, pixel_index])
can_predict.value = False
print("[Prediction Thread] Prediction thread stop")
def compare_results(manual, mark, mask, label):
result, FN, FP, TN, TP, points = compare(manual, mask, mark)
metric = get_metric(FN, FP, TN, TP, label, points)
plt.figure(figsize=(18, 16), dpi=80, facecolor='w', edgecolor='k')
draw_image(manual, 1, 3, 1, False)
draw_image(mark, 1, 3, 2, False)
draw_image(result, 1, 3, 3)
display(Markdown(metric))
def test_input():
from config import FLAGS
input_ = Input(FLAGS.train_batch_size, [FLAGS.img_height, FLAGS.img_width, FLAGS.num_chans])
images = input_('/data/yuming/watermark-data/train_images.tfr')
writer = tf.summary.FileWriter('model-output', tf.get_default_graph())
with tf.Session() as (sess):
sess.run([tf.global_variables_initializer(), tf.local_variables_initializer()])
images_val = sess.run(images)
print(images_val.shape)
images = [{'data':np.squeeze(images_val[0, :, :, :].astype(np.uint8)), 'title':'image0'},
{'data':np.squeeze(images_val[1, :, :, :].astype(np.uint8)), 'title':'image1'},
{'data':np.squeeze(images_val[2, :, :, :].astype(np.uint8)), 'title':'image2'},
{'data':np.squeeze(images_val[3, :, :, :].astype(np.uint8)), 'title':'image3'}]
image_str = draw_image(images)
writer.add_summary(image_str, global_step=0)
writer.close()
def main(unused_argv):
if FLAGS.checkpoint_dir == '' or not os.path.exists(FLAGS.checkpoint_dir):
raise ValueError('invalid checkpoint directory {}'.format(FLAGS.checkpoint_dir))
checkpoint_dir = os.path.join(FLAGS.checkpoint_dir, '')
if FLAGS.output_dir == '':
raise ValueError('invalid output directory {}'.format(FLAGS.output_dir))
elif not os.path.exists(FLAGS.output_dir):
assert FLAGS.output_dir != FLAGS.checkpoint_dir
os.makedirs(FLAGS.output_dir)
print('reconstructing models and inputs.')
image = Image('/data/yuming/watermark-data/image_paths.mat', FLAGS.image_seq)()
wm = Watermark('/data/yuming/watermark-data/watermark.mat')()
dim = [1, FLAGS.img_height, FLAGS.img_width, FLAGS.num_chans]
image_upsampler = Upsampler(dim)
wm_upsampler = Upsampler([1] + dim[1:])
downsampler = Downsampler(dim)
blender = Blender(dim)
extrator = Extractor(dim)
image_upsampled = image_upsampler(image)
wm_upsampled = wm_upsampler(wm)
image_blended = blender(image_upsampled, wm_upsampled)
image_downsampled = downsampler(image_blended)
wm_extracted = extrator(image_downsampled)
# Calculate the psnr of the model.
psnr = PSNR()
image_psnr = psnr(image, image_downsampled)
wm_psnr = psnr(wm, wm_extracted)
summ_psnr_op = tf.summary.merge([tf.summary.text('image_psnr', tf.as_string(image_psnr)),
tf.summary.text('wm_psnr', tf.as_string(wm_psnr))])
saver = tf.train.Saver()
writer = tf.summary.FileWriter(FLAGS.output_dir, tf.get_default_graph())
config = tf.ConfigProto(allow_soft_placement = True, log_device_placement = False)
assert (FLAGS.gpus != ''), 'invalid GPU specification'
config.gpu_options.visible_device_list = FLAGS.gpus
with tf.Session(config = config) as sess:
sess.run(tf.local_variables_initializer())
ckpt = tf.train.get_checkpoint_state(checkpoint_dir)
if ckpt and ckpt.model_checkpoint_path:
# Restores from checkpoint
saver.restore(sess, ckpt.model_checkpoint_path)
# Assuming model_checkpoint_path looks something like:
# /my-favorite-path/cifar10_train/model.ckpt-0,
# extract global_step from it.
global_step = ckpt.model_checkpoint_path.split('/')[-1].split('-')[-1]
else:
print('No checkpoint file found')
return
summ_psnr_str, image_val, image_downsampled_val = \
sess.run([summ_psnr_op, image, image_downsampled])
writer.add_summary(summ_psnr_str, global_step = 0)
'''
images = [{'data': np.squeeze(image_val[0, :, :, :].astype(np.uint8)), 'title': "original image"},
{'data': np.squeeze(image_downsampled_val[0, :, :, :].astype(np.uint8)), 'title': "watermarked image"}]
'''
images = [{'data': np.squeeze(image_val[0, :, :, :].astype(np.uint8)), 'title': ""},
{'data': np.squeeze(image_downsampled_val[0, :, :, :].astype(np.uint8)), 'title': ""}]
image_str = draw_image(images)
writer.add_summary(image_str, global_step = 0)
def main(config):
test_transform = T.Compose([
T.Resize(224),
T.CenterCrop(224),
T.ToTensor(),
T.Normalize(mean=IMAGENET_MEAN, std=IMAGENET_STD),
])
test_dataset = AmazonDataset(config.paths.test_dir,
transform=test_transform,
target_transform=filename_clean,
dtype="test")
test_loader = DataLoader(test_dataset,
batch_size=config.training.batch_size,
num_workers=config.training.n_workers)
# define model
encoder = EncoderCNN()
decoder = DecoderRNN(config.model.embed_size, config.model.hidden_size,
encoder.output_size, 19, config.model.total_size)
encoder.load_state_dict(torch.load(config.paths.cnn_save_path))
decoder.load_state_dict(torch.load(config.paths.rnn_save_path))
if torch.cuda.is_available():
encoder.cuda(config.training.cuda_device)
decoder.cuda(config.training.cuda_device)
encoder.eval()
decoder.eval()
filenames = []
predictions = []
classes = find_classes(config.paths.label_list_file)
print("Running Predictions :")
for i, (images, filename) in enumerate(test_loader):
print("Batch [%d/%d]" % ((i + 1), len(test_loader)))
images = to_var(images, volatile=True)
cnn_features = encoder(images)
if attention:
attn, preds = decoder.sample(cnn_features)
else:
preds = decoder.sample(cnn_features)
prediction = []
for j in range(preds.size(0)):
pred = preds[j].data.cpu().numpy().tolist()
if 18 in pred:
pred = pred[:pred.index(18)]
prediction.append(' '.join([classes[k - 1] for k in pred]))
if attention and config.training.draw_image:
draw_image(attn, filename, prediction)
filenames += list(filename)
predictions += prediction
submission = pd.DataFrame()
submission['image_name'] = filenames
submission['tags'] = predictions
submission.to_csv(config.paths.submission_file, index=False)
grasp_prediction, depth, diff_path, iteration,
specific_tool)
else: # Testing
action, action_str, pixel_index, angle = utils.greedy_policy(
suck_1_prediction, suck_2_prediction, grasp_prediction,
specific_tool)
explore = False
explore_list.append(explore)
target_list.append(pixel_index)
position_list.append(points[pixel_index[1], pixel_index[2]])
del suck_1_prediction, suck_2_prediction, grasp_prediction
utils.print_action(action_str, pixel_index,
points[pixel_index[1], pixel_index[2]])
# Save (color heightmap + prediction heatmap + motion primitive and corresponding position), then show it
visual_img = utils.draw_image(
mixed_imgs[pixel_index[0]], explore, pixel_index,
vis_path + "vis_{:06}.jpg".format(iteration))
cv2.imshow("prediction",
cv2.resize(visual_img, None, fx=2, fy=2))
cv2.waitKey(33)
# Check if action valid (is NAN?)
is_valid = utils.check_if_valid(points[pixel_index[1],
pixel_index[2]])
# Visualize in RViz
_viz(points[pixel_index[1], pixel_index[2]], action, angle,
is_valid)
will_collide = None
if is_valid: # Only take action if valid
# suck_1(0) -> 3, suck_2(1) -> 2, other(2~5) (grasp) -> 1
tool_id = (3 - pixel_index[0]) if pixel_index[0] < 2 else 1
action_list.append(pixel_index[0])
def main(unused_argv):
if FLAGS.checkpoint_dir == '' or not os.path.exists(FLAGS.checkpoint_dir):
raise ValueError('invalid checkpoint directory {}'.format(
FLAGS.checkpoint_dir))
checkpoint_dir = os.path.join(FLAGS.checkpoint_dir, '')
if FLAGS.output_dir == '':
raise ValueError('invalid output directory {}'.format(
FLAGS.output_dir))
elif not os.path.exists(FLAGS.output_dir):
assert FLAGS.output_dir != FLAGS.checkpoint_dir
os.makedirs(FLAGS.output_dir)
print('reconstructing models and inputs.')
image = Image('/data/yuming/watermark-data/image_paths.mat',
FLAGS.image_seq)()
wm = Watermark('/data/yuming/watermark-data/watermark.mat')()
dim = [1, FLAGS.img_height, FLAGS.img_width, FLAGS.num_chans]
image_upsampler = Upsampler(dim)
wm_upsampler = Upsampler([1] + dim[1:])
downsampler = Downsampler(dim)
blender = Blender(dim)
extrator = Extractor(dim)
image_upsampled = image_upsampler(image)
wm_upsampled = wm_upsampler(wm)
image_blended = blender(image_upsampled, wm_upsampled)
image_downsampled = downsampler(image_blended)
mask = Mask(FLAGS.img_height, FLAGS.img_width, 80)()
mask = tf.cast(mask, tf.complex64)
freqimage = FreqImage(mask)
image_freqfiltered = freqimage(image_downsampled)
wm_extracted = extrator(image_freqfiltered)
enhance = Enhance(sharpen=True)
wm_extracted = enhance(wm_extracted)
saver = tf.train.Saver()
writer = tf.summary.FileWriter(FLAGS.output_dir, tf.get_default_graph())
config = tf.ConfigProto(allow_soft_placement=True,
log_device_placement=False)
assert (FLAGS.gpus != ''), 'invalid GPU specification'
config.gpu_options.visible_device_list = FLAGS.gpus
with tf.Session(config=config) as sess:
sess.run(tf.local_variables_initializer())
ckpt = tf.train.get_checkpoint_state(checkpoint_dir)
if ckpt and ckpt.model_checkpoint_path:
# Restores from checkpoint
saver.restore(sess, ckpt.model_checkpoint_path)
# Assuming model_checkpoint_path looks something like:
# /my-favorite-path/cifar10_train/model.ckpt-0,
# extract global_step from it.
global_step = ckpt.model_checkpoint_path.split('/')[-1].split(
'-')[-1]
else:
print('No checkpoint file found')
return
wm_val, image_downsampled_val, image_freqfiltered_val, wm_extracted_val = \
sess.run([wm, image_downsampled, image_freqfiltered, wm_extracted])
images = [{
'data':
np.squeeze(image_downsampled_val[0, :, :, :].astype(np.uint8)),
'title':
"watermarked image"
}, {
'data':
np.squeeze(image_freqfiltered_val[0, :, :, :].astype(np.uint8)),
'title':
"filtered image"
}, {
'data': np.squeeze(wm_val[0, :, :, :].astype(np.uint8)),
'title': "original watermark"
}, {
'data':
np.squeeze(wm_extracted_val[0, :, :, :].astype(np.uint8)),
'title':
"extracted watermark"
}]
image_str = draw_image(images)
writer.add_summary(image_str, global_step=0)
np.set_printoptions(threshold=sys.maxsize)
print(np.squeeze(wm_extracted_val))
writer.close()
def show_images(x):
plt.figure(figsize=(18, 16), dpi=80, facecolor='w', edgecolor='k')
draw_image(image_select.image.original, 1, 3, 1, False)
draw_image(manual_select.image.original, 1, 3, 2, False)
draw_image(mask_select.image.original, 1, 3, 3)
def draw(self):
super().draw()
for i in range(len(self.waffle_stack)):
utils.draw_image(self.waffle_stack[i].image, self.x + 25,
self.y + 20 - i * 12.5)
def draw(self, surface: pygame.Surface):
for z in range(1, self.d):
for x in range(self.w):
for y in range(self.h):
image = self.tmxdata.get_tile_image(x, y, z)
draw_image(surface, image, x, y)
def main():
g1 = tf.Graph()
g2 = tf.Graph()
config = tf.ConfigProto(allow_soft_placement=True)
config.gpu_options.per_process_gpu_memory_fraction = 0.4
sess1 = tf.Session(config=config, graph=g1)
sess2 = tf.Session(config=config, graph=g2)
step1_inputs, step1_boxes, step1_classes, step1_scores = load_step1(
g1, box_pb_path, sess1)
step2_inputs, step2_scores = load_step2(g2, cls_pb_path, sess2)
cap = cv2.VideoCapture(-1)
cap.set(3, 800)
cap.set(4, 600)
imagedir_for_test = '/media/commaai02/disk_1TB/huapu600/mei_yi_zhan/image'
imagelist = os.listdir(imagedir_for_test)
np.random.shuffle(imagelist)
for step in range(len(imagelist)):
#while(1):
detect_dict = {}
st = time.time()
detect_dict['image_bgr'] = cv2.imread(
os.path.join(imagedir_for_test, imagelist[step]))
#_, detect_dict['image_bgr'] = cap.read()
detect_dict['image_rgb'] = cv2.cvtColor(detect_dict['image_bgr'],
cv2.COLOR_BGR2RGB)
feed_dict1 = {
step1_inputs: np.expand_dims(detect_dict['image_rgb'], axis=0)
}
detect_dict['step1_boxes'], detect_dict['step1_classes'], detect_dict[
'step1_scores'] = sess1.run(
[step1_boxes, step1_classes, step1_scores],
feed_dict=feed_dict1)
detect_dict = step1_result_process(detect_dict, thr=0.8)
num_step1_detect_boxes = len(detect_dict['step1_classes'])
step2_scores_list = []
step2_classes_list = []
step2_skus_list = []
for i in range(num_step1_detect_boxes):
box_i = detect_dict['step1_boxes'][i]
_image = detect_dict['image_rgb'][int(box_i[0]):int(box_i[2]),
int(box_i[1]):int(box_i[3]), :]
pred = sess2.run([step2_scores],
feed_dict={step2_inputs: resize_image(_image)})
class_index = np.argmax(pred[0], 1)[0]
max_scores = pred[0][0][class_index]
step2_classes_list.append(class_index)
step2_scores_list.append(max_scores)
step2_skus_list.append(sku_classes[class_index])
detect_dict['step2_scores'] = np.array(step2_scores_list,
dtype=np.float32)
detect_dict['step2_classes'] = np.array(step2_classes_list,
dtype=np.int32)
detect_dict['step2_skus'] = np.array(step2_skus_list, dtype=np.int32)
detect_dict = step2_result_nms(detect_dict, max_ovr=0.5)
detect_dict = draw_image(detect_dict)
#cv2.imshow('detector',detect_dict['image_bgr'])
cv2.imwrite(
'/media/commaai02/disk_1TB/huapu600/mei_yi_zhan/test/p1/' +
imagelist[step], detect_dict['image_bgr'])
print('time: %f' % (time.time() - st))
k = cv2.waitKey(10) & 0xFF
if k == 27:
break
cap.release()
cv2.destroyAllWindows()
pc_res = get_pc_client()
color, depth, points, depth_img_msg = utils.get_heightmap(pc_res.pc, "", 0)
else:
_, depth_img_path, _, _, _, _, _ = utils.get_file_path(args.color_img_path)
color = cv2.imread(args.color_img_path)
depth = cv2.imread(depth_img_path, -1)
suck_predictions, grasp_predictions, state_feat = \
trainer.forward(color, depth, is_volatile=True)
suck_predictions, grasp_predictions = utils.standarization(
suck_predictions, grasp_predictions)
suck_heatmap = utils.vis_affordance(suck_predictions[0])
suck_mixed = cv2.addWeighted(color, 1.0, suck_heatmap, 0.4, 0)
tmp = np.where(suck_predictions == np.max(suck_predictions))
best_pixel = [tmp[0][0], tmp[1][0], tmp[2][0]]
suck_img = utils.draw_image(suck_mixed, 1, best_pixel)
cv2.imwrite("suck.jpg", suck_img)
grasp_mixed = []
for i in range(len(grasp_predictions)):
grasp_heatmap = utils.vis_affordance(grasp_predictions[i])
name = "grasp_{}.jpg".format(i)
grasp_mixed_idx = cv2.addWeighted(color, 1.0, grasp_heatmap, 0.4, 0)
tmp = np.where(grasp_predictions == np.max(grasp_predictions[i]))
best_pixel = [tmp[0][0], tmp[1][0], tmp[2][0]]
grasp_img = utils.draw_image(grasp_mixed_idx, 0, best_pixel)
cv2.imwrite("grasp_{}.jpg".format(i), grasp_img)
grasp_mixed.append(grasp_mixed_idx)
#action, action_str, pixel_index, angle = utils.greedy_policy(suck_predictions, grasp_predictions)
#visual_img = None
#if action:
coder = Coder()
image = np.random.rand(224, 224, 3)
data = np.reshape(image, (1, 3, 224, 224))
gt = coder._generate_boxes(1)
inputs = prepare_inputs(data, gt)
caffe.set_mode_gpu()
net = solver.net
utils.set_inputs(net, **inputs)
for step in range(100):
solver.step(1)
delta = unpack_outputs(net.blobs['preds_reshape'].data)
probs = unpack_outputs(net.blobs['final_probs'].data)
bboxes = np.zeros((100, 5))
bboxes[:, 0:4] = coder.decode(delta)
bboxes[:, 4] = probs[:, 1]
dets = utils.nms(bboxes)
if step % 10 == 0:
ax = utils.draw_image(image)
utils.vis_bboxes(ax, dets * 224, 'red')
utils.vis_bboxes(ax, gt * 224, 'green')
plt.axis('off')
plt.tight_layout()
plt.savefig('%04d.png'%step)
plt.close()
if not grasp_only:
action, action_str, pixel_index, angle = utils.greedy_policy(suck_predictions, grasp_predictions)
else: # Grasp-only
action = 0
action_str = 'grasp'
pixel_index, angle = utils.grasp_only_policy(grasp_predictions)
explore_list.append(explore)
if explore == 1: print "Use exploring..."
del suck_predictions, grasp_predictions, state_feat
print "[%f]: Take action: \033[0;31m %s\033[0m at \
\033[0;32m(%d, %d)\033[0m with theta \033[0;33m%f \033[0m" %(time.time(), action_str, pixel_index[1], \
pixel_index[2], angle)
# Draw color + heatmap + motion
visual_img = None
if action: # SUCK
visual_img = utils.draw_image(suck_mixed, action, pixel_index)
else: # GRASP
visual_img = utils.draw_image(grasp_mixed[pixel_index[0]], action, pixel_index)
vis_name = vis_path + "vis_{:06}.jpg".format(iteration)
cv2.imwrite(vis_name, visual_img)
# Invalid conditions:
# 1. NaN or origin point
# 2. Point belongs to the plane
# TODO 3. Gripper collision with object
print "###### [%f, %f, %f] ######" %(points[pixel_index[1], pixel_index[2], 0], points[pixel_index[1], pixel_index[2], 1], points[pixel_index[1], pixel_index[2], 2])
#check_valid_req = check_validRequest()
#check_valid_req.pc = pc_response.pc
#check_valid_req.p.x = points[pixel_index[1], pixel_index[2], 0]
#check_valid_req.p.y = points[pixel_index[1], pixel_index[2], 1]
#check_valid_req.p.z = points[pixel_index[1], pixel_index[2], 2]