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)