def execute_fulcoverage():
global coverage_list
idx = 0
#coverage_list.append(Fullpathmsgs())
first_point = Fullpathmsgs()
first_point.start_flag = True
first_point.x = coverage_list[0].x
first_point.y = coverage_list[0].y + (15 * 0.025)
first_point.rotation_flag = coverage_list[0].rotation_flag
first_point.wall = coverage_list[0].wall
first_point.direction = coverage_list[0].direction
first_point.arrival = coverage_list[0].arrival
coverage_list[0].x_pre = -0.222259953061
coverage_list[0].y_pre = -0.00213585657632
first_point.x_pre = coverage_list[-2].x
first_point.y_pre = coverage_list[-2].y
coverage_list.insert(0, first_point)
coverage_list[0].start_flag = True
coverage_list[-1].start_flag = False
coverage_list[-1].x = coverage_list[0].x
coverage_list[-1].y = coverage_list[0].y
coverage_list[-1].rotation_flag = coverage_list[0].rotation_flag
coverage_list[-1].wall[0] = True
coverage_list[-1].wall[1] = True
coverage_list[-1].direction = 2
coverage_list[-1].arrival = coverage_list[0].arrival
while idx < len(coverage_list):
try:
print("////////////////////////////////////////////////////////",
coverage_list[idx])
fullpath = rospy.ServiceProxy('fullpathmove', Fullpath)
resp1 = fullpath(coverage_list[idx])
while (not (resp1.result)):
None
if resp1.result == 1:
print("image_dodge_fail")
if idx < len(coverage_list) - 4:
result = movebase_client(coverage_list[idx + 4].x,
coverage_list[idx + 4].y, 1)
coverage_list[idx].arrival = False
coverage_list[idx].start_flag = False
coverage_list[idx + 1].arrival = False
coverage_list[idx + 1].start_flag = False
coverage_list[idx + 2].arrival = False
coverage_list[idx + 2].start_flag = False
coverage_list[idx + 3].arrival = False
coverage_list[idx + 3].start_flag = False
coverage_list[idx + 4].arrival = False
coverage_list[idx + 4].start_flag = False
idx = idx + 5
else:
result = movebase_client(
coverage_list[len(coverage_list) + 5 -
len(coverage_list)])
elif resp1.result == 100:
coverage_list[idx].arrival = True
coverage_list[idx].start_flag = True
idx = idx + 1
print(resp1.result)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
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, 2)
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, 2)
elif pt1 == 0 and pt2 == 3:
cv2.line(original, (x - hdistance / 2, y - hdistance / 2),
(x, y - hdistance / 2), blue_color, 2)
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, 2)
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, 2)
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, 2)
cv2.line(original, (x - hdistance / 2, y - hdistance / 2),
(x - hdistance / 2 - hdistance / 2, y - hdistance / 2),
blue_color, 2)
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, 2)
cv2.line(original, (x - hdistance / 2, y - hdistance / 2),
(x + hdistance / 2, y - hdistance / 2), blue_color, 2)
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, 2)
cv2.line(original, (x - hdistance / 2, y - hdistance / 2),
(x - hdistance / 2, y), blue_color, 2)
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, 2)
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, 2)
cv2.line(original, (x - hdistance / 2, y - hdistance / 2),
(x - hdistance / 2, y - hdistance / 2 - hdistance / 2),
blue_color, 2)
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, 2)
cv2.line(original, (x - hdistance / 2, y - hdistance / 2),
(x - hdistance / 2 + hdistance / 2, y - hdistance / 2),
blue_color, 2)
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, 2)
cv2.line(original, (x - hdistance / 2, y - hdistance / 2),
(x - hdistance / 2, y - hdistance / 2 + hdistance / 2),
blue_color, 2)
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, 2)
elif pt1 == 3 and pt2 == 1:
cv2.line(original, (x - hdistance / 2 + hdistance / 2, y - 8),
(x - hdistance / 2, y - hdistance / 2), blue_color, 2)
cv2.line(original, (x - hdistance / 2, y - hdistance / 2),
(x - hdistance / 2, y - hdistance / 2 - hdistance / 2),
blue_color, 2)
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, 2)
cv2.line(original, (x - hdistance / 2, y - hdistance / 2),
(x - hdistance / 2 + hdistance / 2, y - hdistance / 2),
blue_color, 2)
elif pt1 == 3 and pt2 == 4:
cv2.line(original, (x, y - hdistance / 2),
(x - hdistance / 2, y - hdistance / 2), blue_color, 2)
cv2.line(original, (x - hdistance / 2, y - hdistance / 2),
(x - hdistance / 2, y - hdistance / 2 + hdistance / 2),
blue_color, 2)
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, 2)
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, 2)
cv2.line(original, (x - hdistance / 2, y - hdistance / 2),
(x - hdistance / 2, y - hdistance / 2 - hdistance / 2),
blue_color, 2)
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, 2)
cv2.line(original,
(x - hdistance / 2 - hdistance / 2, y - hdistance / 2),
(x - hdistance / 2, y - hdistance / 2), blue_color, 2)
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, 2)
cv2.line(original, (x - hdistance / 2, y - hdistance / 2),
(x - hdistance / 2 + hdistance / 2, y - hdistance / 2),
blue_color, 2)
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
#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
'''
if first - 1 == yy and last - 1 == xx:
obstacle_flag = True
if pkt[yy][xx - 1][0] == 0 and pkt[yy][xx - 1][1] == 0:
xx_ = xx_ + hdistance / 2
if pkt[yy][xx + 1][0] == 0 and pkt[yy][xx + 1][1] == 0:
xx_ = xx_ - hdistance / 2
if pkt[yy + 1][xx][0] == 0 and pkt[yy + 1][xx][1] == 0:
yy_ = yy_ - hdistance / 2
if pkt[yy - 1][xx][0] == 0 and pkt[yy - 1][xx][1] == 0:
yy_ = yy_ + hdistance / 2
fullpath_info.wall = [False, False, False, False]
if pkt[yy][xx - 1][0] == 0 and pkt[yy][xx - 1][1] == 0:
fullpath_info.wall[1] = True
if pkt[yy][xx + 1][0] == 0 and pkt[yy][xx + 1][1] == 0:
fullpath_info.wall[2] = True
if pkt[yy + 1][xx][0] == 0 and pkt[yy + 1][xx][1] == 0:
fullpath_info.wall[3] = True
if pkt[yy - 1][xx][0] == 0 and pkt[yy - 1][xx][1] == 0:
fullpath_info.wall[0] = True
drowline1(pkt[yy][xx][0], pkt[yy][xx][1], xx_, yy_)
cv2.imshow('Original', original)