def BornQnext(self, Qrand, Qnear):
Qnext = [0, 0, 0, 0, 0]
theta = np.arctan2(Qrand[1] - Qnear[1], Qrand[0] - Qnear[0])
Qnext[0] = Qnear[0] + self.step * np.cos(theta)
Qnext[1] = Qnear[1] + self.step * np.sin(theta)
Qnext[2] = len(self.tree)
Qnext[3] = Qnear[2]
Qnext[4] = Qnear[4]+self.Calculate_Distance(Qnext[0], Qnext[1], Qnear[0], Qnear[1])
if self.CheckStatus(Qnext) is True:
# self.shave_rrt(Qnext) # 若追求效率，则可删掉这句话
self.tree.append(Qnext)
# draw a line
line = [Qnear[0], Qnear[1], Qnext[0], Qnext[1]]
self.lines.append(line)
# import ipdb;ipdb.set_trace()
if self.draw:
if len(self.lines) == 1:
send_tree = SendDebug('LINE', self.lines)
send_tree.send()
else:
# import ipdb;ipdb.set_trace()
send_tree = SendDebug('LINE', [self.lines, []])
send_tree.send()
if self.CheckGoal(Qnext) is True:
self.goalNode[2] = len(self.tree)
self.goalNode[3] = Qnext[2]
self.tree.append(self.goalNode)
return True
return False
def debug_module():
global lines, path_lines, circles, barrier
while True:
# try:
# debug_info = SendDebug('LINE', [[], path_lines], infos=barrier, circles=circles, id=id, color_rob=color)
debug_info = SendDebug('LINE', [[], path_lines])
debug_info.send()
def run_line(color, robot_id, barriers, target_x, target_y, global_p, local_p,
receive):
global_planner = global_p
local_planner = local_p
while True:
receive.get_info(color, robot_id)
global_path = global_planner(receive.robot_info['x'],
receive.robot_info['y'],
target_x,
target_y,
barriers,
receive,
color=color,
id=robot_id)
status, tree, lines = global_path.Generate_Path()
path, path_lines = global_path.Get_Path()
path, path_lines = global_path.merge()
debug_info = SendDebug('LINE', [lines, path_lines])
debug_info.send()
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
if distance((now_x, now_y), (target_x, target_y)) > 7:
motion = local_planner()
if not motion.line_control(path, robot_id, color, receive,
target_x, target_y, barriers):
control = Send()
control.send_msg(robot_id, 0, 0, 0)
else:
continue
else:
control = Send()
control.send_msg(robot_id, 0, 0, 0)
return
def run(color, robot_id, barriers, target_x, target_y, global_p, local_p,
receive):
global_planner = global_p
local_planner = local_p
status = False
while not status:
time_start = time.time()
receive.get_info(color, robot_id)
global_path = global_planner(receive.robot_info['x'],
receive.robot_info['y'], target_x,
target_y, barriers, receive)
status, tree, lines = global_path.Generate_Path()
path, path_lines = global_path.Get_Path()
print('ori:', len(path))
path, path_lines = global_path.merge()
print('nodes:', len(path))
time_end = time.time()
print('path cost:', time_end - time_start)
debug_info = SendDebug('LINE', [lines, path_lines])
debug_info.send()
end = time.time()
print('total cost:', end - time_start)
motion = local_planner()
motion.path_control(path, robot_id, color, receive)
control = Send()
control.send_msg(robot_id, 0, 0, 0)
print(status)
def debug_module():
global lines, path_lines, circles
while True:
try:
debug_info = SendDebug('LINE', [[], path_lines], infos=infos, circles=circles)
debug_info.send()
except:
continue
def debug_module():
global lines, path_lines
while True:
debug_info = SendDebug('LINE', [lines, path_lines],
circles=radius,
infos=infos)
# debug_info = SendDebug('LINE', [lines, path_lines])
debug_info.send()
def debug_module():
global lines, path_lines
while True:
try:
debug_info = SendDebug('LINE', [lines, path_lines])
debug_info.send()
except:
continue
def run_shrink(color, robot_id, barriers, target_x, target_y, global_p,
local_p, receive):
global_planner = global_p
local_planner = local_p
R = 25
index = 1
while True:
if index > 4:
index = 4
r = R / index
receive.get_info(color, robot_id)
starttime = time.time()
global_path = global_planner(
receive.robot_info['x'],
receive.robot_info['y'],
target_x,
target_y,
barriers,
# receive, color=color, id=robot_id)
receive,
color=color,
id=robot_id,
inflateRadius=r,
dis_threshold=r)
status, tree, lines = global_path.Generate_Path()
path, path_lines = global_path.Get_Path()
path, path_lines = global_path.merge()
endtime = time.time()
print('road planning time: ', endtime - starttime)
debug_info = SendDebug('LINE', [lines, path_lines])
debug_info.send()
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
# import ipdb;ipdb.set_trace()
if distance((now_x, now_y), (target_x, target_y)) > 7:
motion = local_planner()
status, index = motion.line_control(path,
robot_id,
color,
receive,
target_x,
target_y,
barriers,
r,
index=index)
if not status:
control = Send()
control.send_msg(robot_id, 0, 0, 0)
else:
index = 1
continue
else:
control = Send()
control.send_msg(robot_id, 0, 0, 0)
return
def run_while(color, robot_id, barriers, target_x, target_y, global_p, local_p,
receive):
global_planner = global_p
local_planner = local_p
while True:
time_start = time.time()
receive.get_info(color, robot_id)
global_path = global_planner(receive.robot_info['x'],
receive.robot_info['y'], target_x,
target_y, barriers, receive)
status, tree, lines = global_path.Generate_Path()
path, path_lines = global_path.Get_Path()
print('ori:', len(path))
path, path_lines = global_path.merge()
print('nodes:', len(path))
time_end = time.time()
print('path cost:', time_end - time_start)
debug_info = SendDebug('LINE', [lines, path_lines])
debug_info.send()
end = time.time()
print('total cost:', end - time_start)
if len(path) < 2:
point = path[0]
else:
point = path[1]
receive.get_info(color, robot_id)
now_x = receive.robot_info['x']
now_y = receive.robot_info['y']
# import ipdb;ipdb.set_trace()
if distance((now_x, now_y), (target_x, target_y)) > 20:
motion = local_planner()
motion.point_control(point, robot_id, color, receive)
control = Send()
control.send_msg(robot_id, 0, 0, 0)
else:
control = Send()
control.send_msg(robot_id, 0, 0, 0)
return
path.append([parent_x, parent_y])
for i in range(len(self.tree)):
if parent_id == -1:
break
point = self.tree[parent_id]
x, y, _, parent_id, _ = point
path.append([x, y])
path_lines.append([x, y, parent_x, parent_y])
parent_x = x
parent_y = y
return path[::-1], path_lines[::-1]
if __name__ == '__main__':
time_start = time.time()
receive = Receive()
my_rrt = RRT(-290, -220, 290, 220, receive,
[['yellow', 0], ['yellow', 1], ['yellow', 2], ['yellow', 3],
['yellow', 4], ['yellow', 5], ['yellow', 6], ['yellow', 7]])
status, tree, lines = my_rrt.Generate_Path()
path, path_lines = my_rrt.Get_Path()
time_end = time.time()
print('path cost:', time_end - time_start)
send_tree = SendDebug('LINE', [lines, path_lines])
send_tree.send()
end = time.time()
print('total cost:', end - time_start)
print(path)
def RUN_mark(color, robot_id, barriers, target_x, target_y, global_p, local_p, receive):
global_planner = global_p
local_planner = local_p
R = 25
index = 1
r = R / index
receive.get_info(color, robot_id)
score = 0
path_last = []
lines_last = []
path_lines_last = []
score_list = []
path_list = []
flag = 0
s = time.time()
for index in range(20):
global_path = global_planner(target_x, target_y, -target_x, -target_y, barriers,
# receive, color=color, id=robot_id)
receive, color=color, id=robot_id, inflateRadius=R, dis_threshold=R)
status, tree, lines = global_path.Generate_Path()
if status:
flag += 1
path, path_lines = global_path.Get_Path()
path, path_lines = global_path.merge()
score_now = mark(path, barriers, receive, color, robot_id)
score_list.append(score_now)
path_list.append(path)
lines_last.append(lines)
path_lines_last.append(path_lines)
score_list = np.array(score_list)
path_list = np.array(path_list)
ind = np.argpartition(score_list, -5)[-5:]
# import ipdb; ipdb.set_trace()
path_select = path_list[ind]
path_select = [interpolate_path(path_s) for path_s in path_select]
lines_last = np.array(lines_last)[ind]
path_lines_last = np.array(path_lines_last)[ind]
e = time.time()
print('cost:', e-s)
print(flag)
if flag == 0:
func(color, robot_id, receive)
return
global_path = global_planner(receive.robot_info['x'], receive.robot_info['y'], target_x, target_y, barriers,
# receive, color=color, id=robot_id)
receive, color=color, id=robot_id, inflateRadius=R, dis_threshold=R)
status, tree, lines = global_path.Generate_Path()
path, path_lines = global_path.Get_Path()
path, path_lines = global_path.merge()
path_start = path
debug_info = SendDebug('LINE', [lines, path_lines])
debug_info.send()
motion = local_planner()
motion.line_control(path_start, robot_id, color, receive, target_x, target_y, barriers, r,
index=index)
index = 1
num = 0
r = R / index
while True:
path_last = path_select[num]
debug_info = SendDebug('LINE', [lines_last[num], path_lines_last[num]])
debug_info.send()
receive.get_info(color, robot_id)
motion = local_planner()
motion.line_control(path_last, robot_id, color, receive, target_x, target_y, barriers, r,
index=index)
target_x *= -1
target_y *= -1
path_last = path_last[::-1]
receive.get_info(color, robot_id)
motion = local_planner()
motion.line_control(path_last, robot_id, color, receive, target_x, target_y, barriers, r,
index=index)
num += 1
if num == 5:
num = 0
def debug_module():
global lines, path_lines, circles, barrier
debug_info = SendDebug('LINE', [[], path_lines])
debug_info.send()
def RUN_mark(color, robot_id, barriers, target_x, target_y, global_p, local_p,
receive):
global_planner = global_p
local_planner = local_p
R = 30
index = 1
r = R / index
receive.get_info(color, robot_id)
score = 0
path_last = []
lines_last = []
path_lines_last = []
flag = 0
s = time.time()
for index in range(20):
global_path = global_planner(
target_x,
target_y,
-target_x,
-target_y,
barriers,
# receive, color=color, id=robot_id)
receive,
color=color,
id=robot_id,
inflateRadius=R,
dis_threshold=R)
status, tree, lines = global_path.Generate_Path()
if status:
flag += 1
path, path_lines = global_path.Get_Path()
path, path_lines = global_path.merge()
score_now = mark(path, barriers, receive, color, robot_id)
if score_now >= score:
score = score_now
path_last = path
lines_last = lines
path_lines_last = path_lines
path_last = interpolate_path(path_last)
e = time.time()
print('cost:', e - s)
print(flag)
if flag == 0:
func(color, robot_id, receive)
return
global_path = global_planner(
receive.robot_info['x'],
receive.robot_info['y'],
target_x,
target_y,
barriers,
# receive, color=color, id=robot_id)
receive,
color=color,
id=robot_id,
inflateRadius=R,
dis_threshold=R)
status, tree, lines = global_path.Generate_Path()
path, path_lines = global_path.Get_Path()
path, path_lines = global_path.merge()
path_start = path
debug_info = SendDebug('LINE', [lines, path_lines])
debug_info.send()
motion = local_planner()
motion.line_control(path_start,
robot_id,
color,
receive,
target_x,
target_y,
barriers,
r,
index=index)
debug_info = SendDebug('LINE', [lines_last, path_lines_last])
debug_info.send()
# receive.get_info(color, robot_id)
# now_x = receive.robot_info['x']
# now_y = receive.robot_info['y']
index = 1
r = R / index
while True:
receive.get_info(color, robot_id)
motion = local_planner()
motion.line_control(path_last,
robot_id,
color,
receive,
target_x,
target_y,
barriers,
r,
index=index)
target_x *= -1
target_y *= -1
path_last = path_last[::-1]