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 __init__(self): self.send = Send() self.debug = SendDebug() self.v = 200 self.threshold = 0.4 self.time_turn = 0.3 self.angle_threshold = 5 * PI / 6
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: try: debug_info = SendDebug('LINE', [lines, path_lines]) 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 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 __init__(self): self.send = Send() self.debug = SendDebug() self.v = 300 self.threshold = 0.3 self.time_turn = 0.3 self.angle_threshold = 5 * PI / 6 self.up = 60 self.x = 9 self.y = 12
def __init__(self): self.send = Send() self.debug = SendDebug() self.v = 350 self.threshold = 0.25 self.time_turn = 0.3 self.angle_threshold = 5 * PI / 6 self.up = 60 self.rtt_distance = 60 self.wallthreshold = 30 self.wall_k = 2400 self.w = 300 self.h = 200 self.d_k = 360000 self.v_k = 0.7 self.time_threshold = 1
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
def __init__(self): self.send = Send() self.debug = SendDebug() self.v = 280 self.threshold = 0.5
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 = 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]
def debug_module(): global lines, path_lines, circles, barrier debug_info = SendDebug('LINE', [[], path_lines]) debug_info.send()
def __init__(self): self.send = Send() self.debug = SendDebug()
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