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
'''
import actionlib from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal import tf from nav_msgs.msg import MapMetaData from geometry_msgs.msg import Quaternion from geometry_msgs.msg import TransformStamped from full_coverage.srv import Fullpath from full_coverage.msg import Fullpathmsgs from std_srvs.srv import Empty g_map = None qq = None c = 0 obstacle_flag = False fullpath_info = Fullpathmsgs() coverage_list = [] testcnt = 0 blue_color = (255, 0, 0) green_color = (0, 255, 0) red_color = (0, 0, 255) white_color = (255, 255, 255) p_file = None p_file2 = None p_num = 0 p_num2 = 0 pre_dest = 0
from full_coverage.msg import Fullpathmsgs
from zetabot_main.srv import Dbsrv
import json
# g_cell_json = {
# 'map_name' : 'None',
# 'cell_name' : 'None',
# 'x' : 0.0,
# 'y' : 0.0
# }
g_map = None
qq = None
c = 0
obstacle_flag = False
fullpath_info = Fullpathmsgs()
coverage_list = []
testcnt = 0
blue_color = (255, 0, 0)
green_color = (0, 255, 0)
red_color = (0, 0, 255)
white_color = (255, 255, 255)
p_file = None
p_file2 = None
p_num = 0
p_num2 = 0
pre_dest = 0
from nav_msgs.msg import MapMetaData
from geometry_msgs.msg import Quaternion
from geometry_msgs.msg import TransformStamped
from full_coverage.srv import Fullpath
from full_coverage.msg import Fullpathmsgs
from std_msgs.msg import Bool
from std_srvs.srv import Empty
import my_first_ros_pkg.msg
from zetabot_main.msg import ScheduleFullcoverageAction, ScheduleFullcoverageGoal
g_map = None
qq = None
c = 0
obstacle_flag = False
fullpath_info = Fullpathmsgs()
coverage_list = []
testcnt = 0
blue_color = (255, 0, 0)
green_color = (0, 255, 0)
red_color = (0, 0, 255)
white_color = (255, 255, 255)
original = None
p_file = open('test.txt', mode='wt')
p_file2 = open('test2.txt', mode='wt')
p_num = 0
p_num2 = 0
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
x = x + 8
y = y + 8
p_num = p_num + 1
if pt1 == 0 and pt2 == 1:
cv2.line(original, (x - 8, y - 8), (x - 8, y), blue_color, 2)
elif pt1 == 0 and pt2 == 2:
cv2.line(original, (x - 8, y - 8), (x - 8 - 7, y - 8), blue_color, 2)
elif pt1 == 0 and pt2 == 3:
cv2.line(original, (x - 8, y - 8), (x, y - 8), blue_color, 2)
elif pt1 == 0 and pt2 == 4:
cv2.line(original, (x - 8, x - 8), (x - 8, y - 8 + 7), blue_color, 2)
elif pt1 == 1 and pt2 == 0:
cv2.line(original, (x - 8, y - 8 - 7), (x - 8, y - 8), blue_color, 2)
elif pt1 == 1 and pt2 == 2:
cv2.line(original, (x - 8, y - 8 - 7), (x - 8, y - 8), blue_color, 2)
cv2.line(original, (x - 8, y - 8), (x - 8 - 7, y - 8), blue_color, 2)
elif pt1 == 1 and pt2 == 3:
cv2.line(original, (x - 8, y - 8 - 7), (x - 8, y - 8), blue_color, 2)
cv2.line(original, (x - 8, y - 8), (x + 8, y - 8), blue_color, 2)
elif pt1 == 1 and pt2 == 4:
cv2.line(original, (x - 8, y - 8 - 7), (x - 8, y - 8), blue_color, 2)
cv2.line(original, (x - 8, y - 8), (x - 8, y), blue_color, 2)
elif pt1 == 2 and pt2 == 0:
cv2.line(original, (x - 8 - 7, y - 8), (x - 8, y - 8), blue_color, 2)
elif pt1 == 2 and pt2 == 1:
cv2.line(original, (x - 8 - 7, y - 8), (x - 8, y - 8), blue_color, 2)
cv2.line(original, (x - 8, y - 8), (x - 8, y - 8 - 7), blue_color, 2)
elif pt1 == 2 and pt2 == 3:
cv2.line(original, (x - 8 - 7, y - 8), (x - 8, y - 8), blue_color, 2)
cv2.line(original, (x - 8, y - 8), (x - 8 + 7, y - 8), blue_color, 2)
elif pt1 == 2 and pt2 == 4:
cv2.line(original, (x - 8 - 7, y - 8), (x - 8, y - 8), blue_color, 2)
cv2.line(original, (x - 8, y - 8), (x - 8, y - 8 + 7), blue_color, 2)
elif pt1 == 3 and pt2 == 0:
cv2.line(original, (x - 8 + 7, y - 8), (x - 8, y - 8), blue_color, 2)
elif pt1 == 3 and pt2 == 1:
cv2.line(original, (x - 8 + 7, y - 8), (x - 8, y - 8), blue_color, 2)
cv2.line(original, (x - 8, y - 8), (x - 8, y - 8 - 7), blue_color, 2)
elif pt1 == 3 and pt2 == 2:
cv2.line(original, (x - 8 - 7, y - 8), (x - 8, y - 8), blue_color, 2)
cv2.line(original, (x - 8, y - 8), (x - 8 + 7, y - 8), blue_color, 2)
elif pt1 == 3 and pt2 == 4:
cv2.line(original, (x, y - 8), (x - 8, y - 8), blue_color, 2)
cv2.line(original, (x - 8, y - 8), (x - 8, y - 8 + 7), blue_color, 2)
elif pt1 == 4 and pt2 == 0:
cv2.line(original, (x - 8, y - 8 + 7), (x - 8, y - 8), blue_color, 2)
elif pt1 == 4 and pt2 == 1:
cv2.line(original, (x - 8, y - 8 + 7), (x - 8, y - 8), blue_color, 2)
cv2.line(original, (x - 8, y - 8), (x - 8, y - 8 - 7), blue_color, 2)
elif pt1 == 4 and pt2 == 2:
cv2.line(original, (x - 8, y - 8 + 7), (x - 8, y - 8), blue_color, 2)
cv2.line(original, (x - 8 - 7, y - 8), (x - 8, y - 8), blue_color, 2)
elif pt1 == 4 and pt2 == 3:
cv2.line(original, (x - 8, y - 8 + 7), (x - 8, y - 8), blue_color, 2)
cv2.line(original, (x - 8, y - 8), (x - 8 + 7, y - 8), 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)
ll.append(
Fullpathmsgs(tfs.transform.translation.x, tfs.transform.translation.y,
c))
if pre_dest != pt2:
p_num2 = p_num2 + 1
tfs = calcPixel2Pos(x, y, 1)
print(tfs)
#result = movebase_client(tfs.transform.translation.x,tfs.transform.translation.y,tfs.transform.translation.z)
#if result:
# rospy.loginfo("Goal execution done!")
#cv2.waitKey(0)
p_file2.write('no' + str(p_num2) + ' x: ' + str(x) + ' y: ' +
str(y) + '\n')
pre_dest = pt2
print("tests0")
rospy.wait_for_service('fullpathmove')
print("tests1")
try:
fullpath = rospy.ServiceProxy('fullpathmove', Fullpath)
print(ll)
resp1 = fullpath(ll)
while (not (resp1.result)):
None
if resp1.result == 1:
print("image_dodge_fail")
result = movebase_client(tfs.transform.translation.x,
tfs.transform.translation.y,
tfs.transform.translation.z)
if result:
rospy.loginfo("Goal execution done!")
print("tests3")
print(resp1.result)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
obstacle_flag = False
ll = []
import os
import commands
import re
import struct
import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
import tf
from nav_msgs.msg import MapMetaData
from geometry_msgs.msg import Quaternion
from geometry_msgs.msg import TransformStamped
from full_coverage.srv import Fullpath
from full_coverage.msg import Fullpathmsgs
testsrv =Fullpathmsgs()
def main():
'''
a = Fullpathmsgs(1,2,-1)
b = Fullpathmsgs(2,2,-1)
c = Fullpathmsgs(3,2,-1)
d = Fullpathmsgs(9,9,9)
ll = [a,b,c]
ll.append(d)
print(len(ll))'''
print (testsrv)