def receive_module(): global infos global x, y, ori global i global color, id global threshold global vx, vy x, y = 0, 0 vx = 0 vy = 0 threshold = 30 i = 0 infos = [] receive = Receive() color = 'blue' id = 5 while True: infos = receive.thread_infos() x, y, _, _, ori = select_info(infos, color, id)
def receive_module(): global infos global x, y, ori global i global color, id global threshold global vx, vy x, y = 0, 0 vx = 0 vy = 0 threshold = 30 i = 0 infos = [] receive = Receive() while True: try: infos = receive.thread_infos() x, y, _, _, ori = select_info(infos, color, id) except: continue
def runLine(color, robot_id, start_x, start_y, goal_x, goal_y, vmax=150, threshold=0.3): receive = Receive() send = Send() receive.get_info(color, robot_id) now_x = receive.robot_info['x'] now_y = receive.robot_info['y'] point = [now_x, now_y] now_ori = receive.robot_info['ori'] error = distance(point, [goal_x, goal_y]) error_max = distance([start_x, start_y], [goal_x, goal_y]) while error > 10: p = 1 orientation_need_now = math.atan2((goal_y - now_y), (goal_x - now_x)) theta = now_ori - orientation_need_now if error < error_max * threshold: p = error / (threshold * error_max) * math.log(2) p = math.exp(p) - 1 vx_now = vmax * math.cos(theta) * p vy_now = vmax * math.sin(theta) * p send.send_msg(robot_id, vx_now, vy_now, 0) receive.get_info(color, robot_id) now_x = receive.robot_info['x'] now_y = receive.robot_info['y'] now_ori = receive.robot_info['ori'] point = [now_x, now_y] error = distance(point, [goal_x, goal_y])
parent_x, parent_y, _, parent_id, _ = point 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)
for i in range(self.maxtry): u, best_trajectory = self.DWA_control(x, u) x = self.motion(x, u, self.dt) trajectory = np.vstack((trajectory,x)) if np.sqrt((x[0]-self.goal[0])**2 + (x[1]-self.goal[1])**2) <= 20: break if i == self.maxtry-1: print('cant find !') return trajectory if __name__ == '__main__': color = 'blue' robot_id = 0 receive = Receive() receive.get_info(color, robot_id) start_x = receive.robot_info['x'] start_y = receive.robot_info['y'] start_ori = receive.robot_info['ori'] time_start = time.time() my_dwa = DynamicWindow(start_x, start_y, start_ori, 200, 200, [['yellow', 0], ['yellow', 1], ['yellow', 2], ['yellow', 3], ['yellow', 4], ['yellow', 5], ['yellow', 6], ['yellow', 7]], receive) trajectory = my_dwa.Generate_control() time_end = time.time() print('time_cost:', time_end-time_start) print(trajectory)