def drowline1(pt1, pt2, x, y):
global p_num
global p_num2
global p_file
global p_file2
global pre_dest
global obstacle_flag
global ll
global fullpath_info
global testcnt
x = x + hdistance
y = y + hdistance
p_num = p_num + 1
if pt1 == 0 and pt2 == 1:
cv2.line(original, (x - hdistance / 2, y - hdistance / 2),
(x - hdistance / 2, y), blue_color, 1)
elif pt1 == 0 and pt2 == 2:
cv2.line(original, (x - hdistance / 2, y - hdistance / 2),
(x - hdistance / 2 - hdistance / 2, y - hdistance / 2),
blue_color, 1)
elif pt1 == 0 and pt2 == 3:
cv2.line(original, (x - hdistance / 2, y - hdistance / 2),
(x, y - hdistance / 2), blue_color, 1)
elif pt1 == 0 and pt2 == 4:
cv2.line(original, (x - hdistance / 2, x - hdistance / 2),
(x - hdistance / 2, y - hdistance / 2 + hdistance / 2),
blue_color, 1)
elif pt1 == 1 and pt2 == 0:
cv2.line(original,
(x - hdistance / 2, y - hdistance / 2 - hdistance / 2),
(x - hdistance / 2, y - hdistance / 2), blue_color, 1)
elif pt1 == 1 and pt2 == 2:
cv2.line(original,
(x - hdistance / 2, y - hdistance / 2 - hdistance / 2),
(x - hdistance / 2, y - hdistance / 2), blue_color, 1)
cv2.line(original, (x - hdistance / 2, y - hdistance / 2),
(x - hdistance / 2 - hdistance / 2, y - hdistance / 2),
blue_color, 1)
elif pt1 == 1 and pt2 == 3:
cv2.line(original,
(x - hdistance / 2, y - hdistance / 2 - hdistance / 2),
(x - hdistance / 2, y - hdistance / 2), blue_color, 1)
cv2.line(original, (x - hdistance / 2, y - hdistance / 2),
(x + hdistance / 2, y - hdistance / 2), blue_color, 1)
elif pt1 == 1 and pt2 == 4:
cv2.line(original,
(x - hdistance / 2, y - hdistance / 2 - hdistance / 2),
(x - hdistance / 2, y - hdistance / 2), blue_color, 1)
cv2.line(original, (x - hdistance / 2, y - hdistance / 2),
(x - hdistance / 2, y), blue_color, 1)
elif pt1 == 2 and pt2 == 0:
cv2.line(original,
(x - hdistance / 2 - hdistance / 2, y - hdistance / 2),
(x - hdistance / 2, y - hdistance / 2), blue_color, 1)
elif pt1 == 2 and pt2 == 1:
cv2.line(original,
(x - hdistance / 2 - hdistance / 2, y - hdistance / 2),
(x - hdistance / 2, y - hdistance / 2), blue_color, 1)
cv2.line(original, (x - hdistance / 2, y - hdistance / 2),
(x - hdistance / 2, y - hdistance / 2 - hdistance / 2),
blue_color, 1)
elif pt1 == 2 and pt2 == 3:
cv2.line(original,
(x - hdistance / 2 - hdistance / 2, y - hdistance / 2),
(x - hdistance / 2, y - hdistance / 2), blue_color, 1)
cv2.line(original, (x - hdistance / 2, y - hdistance / 2),
(x - hdistance / 2 + hdistance / 2, y - hdistance / 2),
blue_color, 1)
elif pt1 == 2 and pt2 == 4:
cv2.line(original,
(x - hdistance / 2 - hdistance / 2, y - hdistance / 2),
(x - hdistance / 2, y - hdistance / 2), blue_color, 1)
cv2.line(original, (x - hdistance / 2, y - hdistance / 2),
(x - hdistance / 2, y - hdistance / 2 + hdistance / 2),
blue_color, 1)
elif pt1 == 3 and pt2 == 0:
cv2.line(original,
(x - hdistance / 2 + hdistance / 2, y - hdistance / 2),
(x - hdistance / 2, y - hdistance / 2), blue_color, 1)
elif pt1 == 3 and pt2 == 1:
cv2.line(original, (x - hdistance / 2 + hdistance / 2, y - 8),
(x - hdistance / 2, y - hdistance / 2), blue_color, 1)
cv2.line(original, (x - hdistance / 2, y - hdistance / 2),
(x - hdistance / 2, y - hdistance / 2 - hdistance / 2),
blue_color, 1)
elif pt1 == 3 and pt2 == 2:
cv2.line(original,
(x - hdistance / 2 - hdistance / 2, y - hdistance / 2),
(x - hdistance / 2, y - hdistance / 2), blue_color, 1)
cv2.line(original, (x - hdistance / 2, y - hdistance / 2),
(x - hdistance / 2 + hdistance / 2, y - hdistance / 2),
blue_color, 1)
elif pt1 == 3 and pt2 == 4:
cv2.line(original, (x, y - hdistance / 2),
(x - hdistance / 2, y - hdistance / 2), blue_color, 1)
cv2.line(original, (x - hdistance / 2, y - hdistance / 2),
(x - hdistance / 2, y - hdistance / 2 + hdistance / 2),
blue_color, 1)
elif pt1 == 4 and pt2 == 0:
cv2.line(original,
(x - hdistance / 2, y - hdistance / 2 + hdistance / 2),
(x - hdistance / 2, y - hdistance / 2), blue_color, 1)
elif pt1 == 4 and pt2 == 1:
cv2.line(original,
(x - hdistance / 2, y - hdistance / 2 + hdistance / 2),
(x - hdistance / 2, y - hdistance / 2), blue_color, 1)
cv2.line(original, (x - hdistance / 2, y - hdistance / 2),
(x - hdistance / 2, y - hdistance / 2 - hdistance / 2),
blue_color, 1)
elif pt1 == 4 and pt2 == 2:
cv2.line(original,
(x - hdistance / 2, y - hdistance / 2 + hdistance / 2),
(x - hdistance / 2, y - hdistance / 2), blue_color, 1)
cv2.line(original,
(x - hdistance / 2 - hdistance / 2, y - hdistance / 2),
(x - hdistance / 2, y - hdistance / 2), blue_color, 1)
elif pt1 == 4 and pt2 == 3:
cv2.line(original,
(x - hdistance / 2, y - hdistance / 2 + hdistance / 2),
(x - hdistance / 2, y - hdistance / 2), blue_color, 1)
cv2.line(original, (x - hdistance / 2, y - hdistance / 2),
(x - hdistance / 2 + hdistance / 2, y - hdistance / 2),
blue_color, 1)
p_file.write('no' + str(p_num) + ' x: ' + str(x) + ' y: ' + str(y) +
'\n')
# cv2.waitKey(0)
tfs = calcPixel2Pos(x, y, 1)
fullpath_info.x = tfs.transform.translation.x
fullpath_info.y = tfs.transform.translation.y
fullpath_info_temp = Fullpathmsgs()
fullpath_info_temp.x = fullpath_info.x
fullpath_info_temp.y = fullpath_info.y
fullpath_info_temp.rotation_flag = fullpath_info.rotation_flag
fullpath_info_temp.wall = fullpath_info.wall
fullpath_info_temp.direction = fullpath_info.direction
fullpath_info_temp.arrival = fullpath_info.arrival
fullpath_info_temp.start_flag = fullpath_info.start_flag
fullpath_info_temp.cell_name = fullpath_info.cell_name
# ll.append(Fullpathmsgs(tfs.transform.translation.x,tfs.transform.translation.y,c))
coverage_list.append(fullpath_info_temp)
coverage_list[0].x_pre = coverage_list[0].x
coverage_list[0].y_pre = coverage_list[0].y - (15 * 0.025)
idx = 0
while idx < len(coverage_list):
if idx != 0:
coverage_list[idx].x_pre = coverage_list[idx - 1].x
coverage_list[idx].y_pre = coverage_list[idx - 1].y
idx = idx + 1
'''
# print pkt[i-1][j-1][1],;
if pkt[i - 1][j - 1][1] != 0 or pkt[i - 1][j - 1][0] != 0:
test_row = True
break
test_col = False
for i in range(1, arrw * 2, 1):
if test_col:
break
for j in range(1, arrh * 2, 1):
# print pkt[i-1][j-1][1],;
if pkt[j - 1][i - 1][1] != 0 or pkt[j - 1][i - 1][0] != 0:
test_col = True
cell_margin_col = i
break
fullpath_info.cell_name = colnum_string(
yy + 1 - cell_margin_row) + str(xx + 1 - cell_margin_col)
print(fullpath_info.cell_name)
drowline1(pkt[yy][xx][0], pkt[yy][xx][1], xx_, yy_)
cv2.imshow('Original', original)
# cv2.waitKey(0)
# idx=0
# for i in range(len(coverage_list)-10):
# coverage_list.pop()
# idx = idx + 1
create_celljson()
p_file.close()
select_child_cell()
cv2.imshow('Original', original)