Skip to content

Commit

Permalink
dev 20210601
Browse files Browse the repository at this point in the history
  • Loading branch information
@laorange
laorange committed Jun 1, 2021
1 parent d82f989 commit 7f37b70
Showing 1 changed file with 84 additions and 83 deletions.
167 changes: 84 additions & 83 deletions main.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,12 +5,13 @@
from pandas import DataFrame
import plotly
import plotly.express as px
from numba import jit
from tqdm import tqdm

# 全局变量
open_points = []
open_points_lists = []
open_points_f = []
open_points_recorder = []
closed_points_lists = []
obstacle_points = []
exceptions = [] # 该列表记录遇障情况，默认不输出
Expand All @@ -21,6 +22,8 @@ def this_point_is_an_obstacle(x: int, y: int, z: int):
OBSTACLE = False
# TODO: ↓ 在下方定义 2d 的障碍
if not opt.use_3d:
# if x + y == 30 and 5 < x < 20:
# OBSTACLE = True
if x == 2 and y in [6, 7, 8, 9, 10, 11, 12, 15, 16, 17, 18, 19, 20, 21, 25, 26, 27, 28]:
OBSTACLE = True
if x == 5 and y in [3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 18, 19, 20, 21, 25, 26, 27, 28]:
Expand Down Expand Up @@ -62,27 +65,7 @@ def this_point_is_an_obstacle(x: int, y: int, z: int):

# TODO: ↓ 在下方定义 3d 的障碍
else:
if x - y == 15 and z < 15:
OBSTACLE = True
if x - y == 15 and 15 <= z <= 20 and x <= 25:
OBSTACLE = True
if x == 25 and z > 5 and y >= 10:
OBSTACLE = True
if z == 20 and y < 25 and x <= 25:
OBSTACLE = True
if z == 10 and y > 10 and x >= 20:
OBSTACLE = True
if y == 25 and x < 20 and z <= 20:
OBSTACLE = True
if y == 25 and 5 <= x <= 15 and z >= 20:
OBSTACLE = True
if y == 10 and 15 <= x <= 25 and z >= 20:
OBSTACLE = True
if x == 15 and 10 <= y < 25 and z > 5:
OBSTACLE = True
if x == 15 and 5 <= y < 10 and z > 20:
OBSTACLE = True
if x + y == 15 and z < 15:
if 19 <= x + y <= 20 and x > 5 and y > 5 and 5 < z < 15:
OBSTACLE = True

return OBSTACLE
Expand All @@ -104,7 +87,7 @@ def __init__(self, x: int, y: int, z: int, g: float or int = 0.0, route_input=No
if not opt.straight:
self.h = ((opt.end_x - self.x) ** 2 + (opt.end_y - self.y) ** 2) ** 0.5

self.f = 0.999 * self.g + 1 * self.h # 略微降低g的占比，使得在原本f相同时，取h更小的点
self.f = 1 * self.g + 1 * self.h
self.t = g if t == 0 else t # t: 默认值为g, 数值越大颜色越鲜艳
self.route = route_input if route_input is not None else [] # 从终点到该点的路径
self.close = False # 是否close
Expand Down Expand Up @@ -165,6 +148,7 @@ def move(self, axis: str):
new_point = Point(new_coordinate['x'], new_coordinate['y'], new_coordinate['z'], g_new, new_route)
if new_point.to_list() not in open_points_lists:
open_points_lists.append(new_point.to_list())
open_points_recorder[-1].append(new_point.to_list())
open_points.append(new_point)
open_points_f.append(new_point.f)
else:
Expand All @@ -179,6 +163,7 @@ def move(self, axis: str):

def iterate_one_time(self):
threads = []
open_points_recorder.append([])
if opt.use_3d: # 3d 情况
axis_list = ["x", "y", "z", "-x", "-y", "-z"]
if not opt.straight:
Expand Down Expand Up @@ -227,57 +212,68 @@ def determine_best_point() -> Point:


# 使用plotly进行可视化
def visualize(route_mode: bool, last_point: Point):
t_ls = []
def visualize(last_point: Point): # last_point: Point, route_mode: bool = True
t_ls = [] # step
c_ls = [] # color
x_ls = []
y_ls = []
z_ls = []
if not route_mode:
for closed_point in closed_points_lists:
t_ls.append(closed_points_lists.index(closed_point))
x_ls.append(closed_point[0])
y_ls.append(closed_point[1])
z_ls.append(closed_point[2])
else:
last_best_point = last_point
routes = last_best_point.route
if routes:
for i in range(len(routes)):
t_ls.append(i)
x_ls.append(routes[i][0])
y_ls.append(routes[i][1])
z_ls.append(routes[i][2])
t_ls.append(len(routes))
x_ls.append(last_best_point.x)
y_ls.append(last_best_point.y)
z_ls.append(last_best_point.z)

# if opt.use_3d:
for obstacle_point in obstacle_points:
t_ls.append(-100)
x_ls.append(obstacle_point[0])
y_ls.append(obstacle_point[1])
z_ls.append(obstacle_point[2])
# else:
# for t in t_ls:
# for obstacle_point in obstacle_points:
# t_ls.append(t)
# x_ls.append(obstacle_point[0])
# y_ls.append(obstacle_point[1])
# z_ls.append(opt.start_z)

data = DataFrame({'x': x_ls, 'y': y_ls, 'z': z_ls, 'step': t_ls})
# if opt.use_3d:
fig = px.scatter_3d(data, x='x', y='y', z='z', color='step')
# else:
# fig = plotly.express.scatter(data, x='x', y='y', animation_frame='step')
plotly.offline.plot(fig, filename=f"{'result_route' if route_mode else 'result_scan'}.html")

final_recorder = [step for step in open_points_recorder if step]

step_len = len(final_recorder)
for step in tqdm(final_recorder):
step_num = final_recorder.index(step) + 1
for point in step:
for t in range(step_num, step_len + 1):
t_ls.append(t)
c_ls.append(step_num)
x_ls.append(point[0])
y_ls.append(point[1])
z_ls.append(point[2])
t_ls.append(step_num)
c_ls.append(step_num * 1.5)
x_ls.append(opt.end_x)
y_ls.append(opt.end_y)
z_ls.append(opt.end_z)

last_best_point = last_point
routes = last_best_point.route
if routes:
for i in range(len(routes)):
t_ls.append(step_len + 1)
c_ls.append(0)
x_ls.append(routes[i][0])
y_ls.append(routes[i][1])
z_ls.append(routes[i][2])
t_ls.append(step_len + 1)
c_ls.append(100)
x_ls.append(last_best_point.x)
y_ls.append(last_best_point.y)
z_ls.append(last_best_point.z)

t_ls_route = list(set(t_ls[:]))
for t in tqdm(t_ls_route):
for obstacle_point in obstacle_points:
t_ls.append(t)
c_ls.append(-100)
x_ls.append(obstacle_point[0])
y_ls.append(obstacle_point[1])
z_ls.append(obstacle_point[2])

data = DataFrame({'x': x_ls, 'y': y_ls, 'z': z_ls, 'step': t_ls, 'color': c_ls})
print("共计{}个点，现在开始画图，预计需要{:.0f}秒".format(len(x_ls), 2.0 + len(x_ls) * 2e-05))
start_time_draw = perf_counter()
fig = px.scatter_3d(data, x='x', y='y', z='z', animation_frame='step', color='color')
plotly.offline.plot(fig, filename=f"output.html")
print("画图用时{:.2f}秒".format(perf_counter() - start_time_draw))


def prepare_before_iterate(start_point: OperationalPoint):
open_points_lists.append(start_point.to_list())
open_points.append(start_point)
open_points_f.append(start_point.f)
open_points_recorder.append([start_point.to_list()])


if __name__ == '__main__':
Expand Down Expand Up @@ -316,26 +312,31 @@ def prepare_before_iterate(start_point: OperationalPoint):
starting_h = best_point.h
if not opt.debug:
prepare_before_iterate(start_point=best_point)
else:
open_points_recorder.append([best_point.to_list()])
open_points_recorder.append([[opt.end_x, opt.end_y, opt.end_z]])

while not REACH_THE_DESTINATION:
count += 1
best_point = determine_best_point()
new_operational_point = OperationalPoint(best_point.x, best_point.y, best_point.z,
best_point.g, best_point.route)
progress = 1 - new_operational_point.h / starting_h
if count // 100:
print("\r当前位置({}, {}, {})，迭代速度：{:.2f}轮/秒，有效路程: {:.2f}%".format(new_operational_point.x,
new_operational_point.y,
new_operational_point.z,
100 / (perf_counter() - break_point_time),
100 * progress), end='')
break_point_time = perf_counter()
count = 0
new_operational_point.iterate_one_time()
try:
count += 1
best_point = determine_best_point()
new_operational_point = OperationalPoint(best_point.x, best_point.y, best_point.z,
best_point.g, best_point.route)
progress = 1 - new_operational_point.h / starting_h
if count // 100:
print("\r当前位置({}, {}, {})，迭代速度:{:.2f}轮/秒，有效路程:{:.2f}%".format(new_operational_point.x,
new_operational_point.y,
new_operational_point.z,
100 / (perf_counter() - break_point_time),
100 * progress), end='')
break_point_time = perf_counter()
count = 0
new_operational_point.iterate_one_time()
except KeyboardInterrupt:
print('中断')
break

print("\n用时: {:.4f}秒".format(perf_counter() - start_time))

if not opt.debug:
visualize(last_point=best_point, route_mode=True)
visualize(last_point=best_point, route_mode=False)
input('\n请敲击回车来结束程序:')
visualize(last_point=best_point)
# input('\n请敲击回车来结束程序:')

0 comments on commit 7f37b70

Please sign in to comment.