class Greed_Plan(object):
def __init__(self, pntLst , typeLst, lst_x, lst_y, lst_z, pnt_A, pnt_B , a1 ,a2 ,b1 ,b2 ,th ):
self._pntLst = pntLst
self._pntNum = len(pntLst)
self._pntBind = len(pntLst) - 1
self._typeLst = typeLst
self._pnt_A = pnt_A
self._pnt_B = pnt_B
self._a1 = a1
self._a2 = a2
self._b1 = b1
self._b2 = b2
self._th = th
# print(self._b1)
# raise Exception('xx')
self._lst_x = lst_x
self._lst_y = lst_y
self._lst_z = lst_z
self._min_x = min(self._lst_x)
self._min_y = min(self._lst_y)
self._min_z = min(self._lst_z)
self._max_x = max(self._lst_x)
self._max_y = max(self._lst_y)
self._max_z = max(self._lst_z)
self.calDisMat()
self._init_sta = STA(pos = self._pnt_A, er = ER(0,0))
self._tree = nx.DiGraph()
self._openLst = []
# nx.Graph
self._tree.add_node(0,sta = self._init_sta)
self.updateOpenList(0)
self._path = []
self._pathLst = []
def plan(self):
# print(self._disMat)
print('begin plan')
# c_Pos = self._pnt_A
# self.draw()
searchTimes = 1
while True:
randPnt = self.getRandPos()
# print(randPnt)
disLst = []
for ed,er in self._openLst:
tPnt = self._pntLst[ed.tInd]
dis = self.calDis(tPnt,randPnt)
disLst.append(dis)
if len(disLst)== 0:
print('bug disLst')
break
minInd = disLst.index(min(disLst))
min_ed, min_er = self._openLst[minInd]
# print('searchTimes = ', searchTimes)
min_sta = STA(pos = self._pntLst[min_ed.tInd], er = min_er)
if min_ed.tInd in self._tree:
print('min_ed = ', min_ed.tInd)
raise Exception('xx')
self._tree.add_node(min_ed.tInd, sta = min_sta)
self._tree.add_edge(min_ed.tInd,min_ed.sInd)
# print(self._tree.number_of_nodes())
# raise Exception('xx')
goal_valid, goal_er = self.calGoalER(min_sta)
if goal_valid:
# self._tree.add_node(self._pntBind, sta = STA(self._pnt_B,goal_er))
print('find path')
_path = [self._pntBind]
ind = min_ed.tInd
while True:
_path.append(ind)
# print(self._path)
lst = list(self._tree.neighbors(ind))
if len(lst) == 0:
break
ind = lst[0]
_path = list(reversed(_path))
print(_path)
print(len(_path))
self._pathLst.append(_path)
# break
# print(list(self._tree.neighbors(min_ed.tInd)))
# raise Exception('xx')
if min_ed.tInd == self._pntBind:
raise Exception('xx')
# print(self._openLst)
self._openLst.remove(self._openLst[minInd])
# print(self._openLst)
self.updateOpenList(min_ed.tInd)
searchTimes += 1
if searchTimes > 600:
break
if searchTimes != self._tree.number_of_nodes():
print('searchTime = ',searchTimes)
raise Exception('ssssss')
print('pathLst', len(self._pathLst))
# nx.draw(self._tree)
# plt.show()
# print(len(self._))
# for ind in self._tree:
# print(self._typeLst[ind])
# self.drawTree()
# self.draw()
# raise Exception('xx')
pass
def earseOpenList(self,addInd):
pass
# while True:
# for
def updateOpenList(self,addInd):
disLst = list(self._disMat[addInd, :])
'''
earse
'''
earseLst = []
for ind in range(len(self._openLst)):
ed,er = self._openLst[ind]
if ed.tInd == addInd:
earseLst.append(self._openLst[ind])
for unit in earseLst:
self._openLst.remove(unit)
# for ed,er in self._openLst:
# ed.tInd
# print(disLst)
resLst = [(-1,sys.float_info.max,ER(-1,-1)) for i in range(5)]
for i in range(self._pntNum):
if i in self._tree:
continue
elif i == addInd:
continue
else:
# print(resLst[0][1])
# print(disLst[i])
if resLst[0][1] > disLst[i]:
sta = self._tree.nodes[addInd]['sta']
if i == 521:
print('addInd', addInd)
valid,er = self.calER(sta,self._pntLst[i],self._typeLst[i])
if valid:
resLst[1] = (resLst[0][0],resLst[0][1],ER(resLst[0][2][0],resLst[0][2][1]))
resLst[0] = (i,disLst[i],er)
resLst = sorted(resLst, key = lambda x: x[1])
# print(resLst)
for openInd,dis,er in resLst:
if openInd == -1:
continue
self._openLst.append([ED(sInd= addInd, tInd= openInd),er])
# print(self._openLst)
# print(resLst)
# seqLst = sorted(disLst)
# print(seqLst)
# print(disLst)
# raise Exception('xx')
def calGoalER(self,sta: STA):
e_h = self.calDis(sta.pos, self._pnt_B) / 1000 + sta.er.e_h
e_v = self.calDis(sta.pos, self._pnt_B) / 1000 + sta.er.e_v
# if e_v == 20.2911741348684:
# raise Exception('xxx')
if e_h < self._th and e_v < self._th:
return True, ER(e_v = e_v, e_h = e_h)
else:
return False,ER(e_v = e_v, e_h = e_h)
def calER(self, sta:STA, pos: Point3D, e_type):
# e_h = self.calD(sta.pos, pos) / 1000 + sta.er.e_h
# e_v = self.calV(sta.pos, pos) / 1000 + sta.er.e_v
e_h = self.calDis(sta.pos, pos) / 1000 + sta.er.e_h
e_v = self.calDis(sta.pos, pos) / 1000 + sta.er.e_v
valid = False
if e_type == CorType.Vertical:
if e_v <= self._a1 and e_h <= self._a2:
valid = True
e_v = 0
if e_type == CorType.Horizontal:
if e_v <= self._b1 and e_h <= self._b2:
valid = True
e_h = 0
return valid,ER(e_v = e_v, e_h = e_h)
# STA.er.e_v
# def find(self, self._init_sta , ):
def getRandPos(self):
# rand_x = np.random.randint(self._min_x,self._max_x)
# rand_y = np.random.randint(self._min_y,self._max_y)
# rand_z = np.random.randint(self._min_z,self._max_z)
rand_x = np.random.random() *(self._max_x - self._min_x) + self._min_x
rand_y = np.random.random() *(self._max_y - self._min_y) + self._min_y
rand_z = np.random.random() *(self._max_z - self._min_z) + self._min_z
return Point3D(rand_x, rand_y, rand_z)
# print(self._pnt_A)
# print(self._pnt_B)
# print('xxx', self.calH(self._pnt_A,self._pnt_B))
def calDisMat(self):
self._disMat = np.zeros((len(self._pntLst), len(self._pntLst)))
for i in range(len(self._pntLst)):
for j in range (len(self._pntLst)):
if i == j:
self._disMat[i][j] = 0
else:
pa = self._pntLst[i]
pb = self._pntLst[j]
dis = math.sqrt((pa.x-pb.x)**2 + (pa.y-pb.y)**2 + (pa.z-pb.z)**2)
self._disMat[i][j] = dis
return True
'''
计算水平距离
'''
# def calH(self,pos_a :Point3D, pos_b:Point3D):
# # print(pnt2d_a)
# dis = math.sqrt((pos_a.x- pos_b.x)**2 + (pos_a.y- pos_b.y)**2)
# return dis
# '''
# 计算垂直距离
# '''
# def calV(self,pos_a :Point3D, pos_b:Point3D):
# dis = abs(pos_b.z - pos_a.z)
# return dis
'''
计算垂直距离
'''
def calDis(self,pos_a :Point3D, pos_b:Point3D):
dis = math.sqrt((pos_a.x- pos_b.x)**2 + (pos_a.y- pos_b.y)**2 + (pos_a.z- pos_b.z)**2)
return dis
def draw(self):
self._scatterLst = []
v_x = []
v_y = []
v_z = []
h_x = []
h_y = []
h_z = []
for i in range(len(self._pntLst)):
if self._typeLst[i] == CorType.Vertical:
v_x.append(float(self._pntLst[i].x))
v_y.append(float(self._pntLst[i].y))
v_z.append(float(self._pntLst[i].z))
else:
h_x.append(float(self._pntLst[i].x))
h_y.append(float(self._pntLst[i].y))
h_z.append(float(self._pntLst[i].z))
s_x = []
s_y = []
s_z = []
for ind in self._tree:
s_x.append(self._pntLst[ind].x)
s_y.append(self._pntLst[ind].y)
s_z.append(self._pntLst[ind].z)
p_x = []
p_y = []
p_z = []
for ind in self._path:
p_x.append(self._pntLst[ind].x)
p_y.append(self._pntLst[ind].y)
p_z.append(self._pntLst[ind].z)
# print(len(v_x))
# raise Exception('XX')
# print(h_x)
# self._scatterLst.append(go.Scatter3d(x= [0,2], y=[0,3], z=[0,1]))
# self._scatterLst.append(go.Scatter3d(x = v_x ,y = v_y , z = v_z, mode = 'markers', marker=dict(color = 'blue', size = 5), name = '垂直'))
# self._scatterLst.append(go.Scatter3d(x = h_x ,y = h_y , z = h_z, mode = 'markers', marker=dict(color = 'orangered', size = 5),name = '水平'))
self._scatterLst.append(go.Scatter3d(x = v_x ,y = v_y , z = v_z, mode = 'markers', marker=dict(size = 8), name = '垂直'))
self._scatterLst.append(go.Scatter3d(x = h_x ,y = h_y , z = h_z, mode = 'markers', marker=dict(size = 8),name = '水平'))
self._scatterLst.append(go.Scatter3d(x = [float(self._pnt_A.x)] ,y = [float(self._pnt_A.y)] , z = [float(self._pnt_A.z)],
mode = 'markers', marker = dict(size = 15),name = 'A'))
self._scatterLst.append(go.Scatter3d(x = [float(self._pnt_B.x)] ,y = [float(self._pnt_B.y)] , z = [float(self._pnt_B.z)],
mode = 'markers',marker = dict(size = 15),name = 'B'))
self._scatterLst.append(go.Scatter3d(x=s_x, y=s_y, z=s_z, mode='markers', marker=dict(size= 5), name='search'))
self._scatterLst.append(go.Scatter3d(x=p_x, y=p_y, z=p_z, mode='lines', line= dict(width = 3), name='path'))
fig = go.Figure(data=self._scatterLst)
plotly.offline.plot(fig, filename='test_ins_2.html')
def drawTree(self):
lst = []
for edge in self._tree.edges:
# print(edge)
s_x = self._pntLst[edge[0]].x
s_y = self._pntLst[edge[0]].y
t_x = self._pntLst[edge[1]].x
t_y = self._pntLst[edge[1]].y
lst.append((s_x,s_y,t_x,t_y))
# raise Exception('xx')
# Pos(self._pntLst)
# for ind in self._tree:
# s_x.append(self._pntLst[ind].x)
# s_y.append(self._pntLst[ind].y)
# s_z.append(self._pntLst[ind].z)
#
_shapeLst = []
mark_x = []
mark_y = []
for p in range(len(lst)):
pnt0 = Pnt(lst[p][0], lst[p][1])
pnt1 = Pnt(lst[p][2], lst[p][3])
mark_x.append(pnt0.x)
mark_x.append(pnt1.x)
mark_y.append(pnt0.y)
mark_y.append(pnt1.y)
line = Line(pnt0, pnt1)
lineDic = line.line2dict()
# print(randColor())
lineDic['line']['color'] = 'darkred'
# lineDic['line']['color'] = 'rgba(15,15,15,0.5)'
lineDic['line']['width'] = 3
_shapeLst.append(lineDic)
#
markTrace = go.Scatter(mode='markers',
x=mark_x,
y=mark_y,
marker=dict(size=3),
name='Spanning-Tree')
_scatterLst = []
_scatterLst.append(markTrace)
layout = dict()
layout['shapes'] = _shapeLst
fig = go.Figure(data = _scatterLst, layout = layout)
fig.show()
# fig.show()
# sp.Point2D.distance()
def checkPath(self):
for _path in self._pathLst:
sta = self._init_sta
for i in range(1,len(_path)-1):
ind = _path[i]
valid, er = self.calER(sta, self._pntLst[ind], self._typeLst[ind])
print(ind,' ',self._tree.nodes[ind]['sta'].er)
print(ind,' ',er)
if valid == False:
raise Exception('error bug')
sta = STA(pos = self._pntLst[ind],er = er)
valid, er = self.calGoalER(sta)
print('end er = ',er)
# print(self._pntBind, ' ', self._tree.nodes[self._pntBind]['sta'].er)
if valid == False:
raise Exception('error bug')
def testP(self):
vaild,er = self.calER(self._init_sta,self._pntLst[1],self._typeLst[1])
print(self._pntLst[1])
print(self._typeLst[1])
print(vaild)
print(er)
def savePathLst(self,case,randSeed):
f_data = open('.//data//ins_'+str(case) + 'rs' + str(randSeed)+'_920_s.dat', 'w')
i = 0
for path in self._pathLst:
rd.writeConf(f_data,'path_' + str(i), path)
i += 1
f_data.close()
def drawDubinsPath(self):
p_data = open('.//data//ins_' + str(case) + 'Best' + '角度' + 'pro2_922_s.dat', 'w')
print(self._chom)
print(self._path)
# gp._path = [0, 503, 200, 80, 237, 170, 278, 369, 214, 397, 612]
# # gp._chrom = [51.12083145920054, 178.6546480374125, 96.53216136306273, 65.68120520420554, 339.2745239620836, 29.692324175129926, 357.1342339082977, 13.96126135651108, 24.67063637610153, 43.081945140688966, 343.72373332373553, 55.53416525291733, 294.97751482614825, 160.55714352603155, 335.62029387958466, 135.1113892974001, 272.2317633544588, 115.50965977974849, 131.70937812167222, 255.65689160142625, 174.68614612015506, 1.1100793596630467]
# gp._chrom = [35.64144355095377, 136.07851338455714, 351.42728912239573, 73.16130147768374, 33.52748385753523,
# 337.8683291201086, 14.20709305212051, 53.08119797734826, 70.25048824593296, 93.78034812463086,
# 359.7118978558144, 29.566128322279766, 324.4248087013906, 267.3070360965535, 44.7340138926815,
# 142.5291949602862, 30.810401439704037, 216.8063045595583, 311.32800296731324, 110.98127345792193,
# 266.86503744086997, 201.95337987458723]
self._chom = self._chrom
# exit()
# exit()
psi_lst = []
gama_lst = []
init_psi = numpy.deg2rad(self._chom[0])
init_gama = numpy.deg2rad((self._chom[1] - 180)/2)
psi_lst.append(init_psi)
gama_lst.append(init_gama)
dirPos_init = DirPoint3D(self._pntLst[0].x,
self._pntLst[0].y,
self._pntLst[0].z,
init_psi,
init_gama)
sta = STADir(dir_pos= dirPos_init, er = ER(0,0))
# yawLst = []
# yawLst.append(0)
# for i in range(2,len(self._chom)):
# yawLst.append(self._chom[i]*math.pi*2)
yawLst = []
yawLst.append(0)
for i in range(2, len(self._chom)):
yawLst.append(numpy.deg2rad(self._chom[i]))
# print(yawLst)
d_x = []
d_y = []
d_z = []
t_dis = 0
pos_dir_x = []
pos_dir_y = []
pos_dir_z = []
end_dir_x = []
end_dir_y = []
end_dir_z = []
dis_d_lst = []
for ind in range(1,len(self._path)):
goal_x = self._pntLst[self._path[ind]].x
goal_y = self._pntLst[self._path[ind]].y
goal_z = self._pntLst[self._path[ind]].z
drawB = False
if ind > 5:
drawB = False
# break
px, py, pz, dis_d, a_psi, a_gama, p3_x, p3_y, p3_z = gdb.Gdubins3D(sta.dir_pos.x,sta.dir_pos.y,sta.dir_pos.z,
goal_x,goal_y,goal_z,
sta.dir_pos.psi,sta.dir_pos.gama,yawLst[ind],drawB)
dis_d_lst.append(dis_d)
psi_lst.append(a_psi)
gama_lst.append(a_gama)
# exit()
pos_dir_x.append(sta.dir_pos.x + 1000*math.cos(sta.dir_pos.psi))
pos_dir_y.append(sta.dir_pos.y + 1000*math.sin(sta.dir_pos.psi))
pos_dir_z.append(sta.dir_pos.z + 1000*math.sin(sta.dir_pos.gama))
end_dir_x.append(goal_x + 1000*math.cos(a_psi)*math.cos(a_gama))
end_dir_y.append(goal_y + 1000*math.sin(a_psi)*math.cos(a_gama))
end_dir_z.append(goal_z + 1000*math.sin(a_gama))
print(1000*math.cos(a_psi)*math.cos(a_gama))
print(1000*math.sin(a_psi)*math.cos(a_gama))
print(1000*math.sin(a_gama))
print('a_psi',a_psi)
print('a_gama', a_gama)
# d_x.extend(px)
# d_y.extend(py)
# d_z.extend(pz)
# print(px)
# print(py)
# print(py)
# print(pz)
# print(px)
# print(len(px))
# print(px[1])
for j in range(len(px)):
d_x.append(px[j])
d_y.append(py[j])
d_z.append(pz[j])
# if ind >= 8:
# # exit()
# break
# print(d_x)
# exit()
# print(d_y)
# print(d_z)
# break
# exit()
# print(goal_dir)
# exit()
dirPos = DirPoint3D(self._pntLst[self._path[ind]].x,
self._pntLst[self._path[ind]].y,
self._pntLst[self._path[ind]].z,
a_psi,
a_gama)
print('draw ', ind)
sta = STADir(dirPos, ER(0,0))
print(sum(dis_d_lst))
print(sum(dis_d_lst))
# exit()
rd.writeConf(p_data,'psi',psi_lst)
rd.writeConf(p_data,'gama',gama_lst)
exit()
self._scatterLst = []
v_x = []
v_y = []
v_z = []
h_x = []
h_y = []
h_z = []
for i in range(len(self._pntLst)):
if self._typeLst[i] == CorType.Vertical:
v_x.append(float(self._pntLst[i].x))
v_y.append(float(self._pntLst[i].y))
v_z.append(float(self._pntLst[i].z))
else:
h_x.append(float(self._pntLst[i].x))
h_y.append(float(self._pntLst[i].y))
h_z.append(float(self._pntLst[i].z))
# s_x = []
# s_y = []
# s_z = []
#
# for ind in self._tree:
# s_x.append(self._pntLst[ind].x)
# s_y.append(self._pntLst[ind].y)
# s_z.append(self._pntLst[ind].z)
p_x = []
p_y = []
p_z = []
for ind in self._path:
p_x.append(self._pntLst[ind].x)
p_y.append(self._pntLst[ind].y)
p_z.append(self._pntLst[ind].z)
f_data = open('.//data//ins_' + str(case) + 'Best' + 'rs' + 'pro2_922_s.dat', 'w')
m_data = open('.//data//ins_' + str(case) + 'Best' + '局部' + 'pro2_922_s.dat', 'w')
min_x = []
min_y = []
min_z = []
print(self._path[-2])
for i in range(0,len(d_x)):
if gdb.distance([d_x[i],d_y[i],d_z[i]],[self._pntLst[self._path[-2]].x,
self._pntLst[self._path[-2]].y,
self._pntLst[self._path[-2]].z]) < 200:
min_x.append(d_x[i])
min_y.append(d_y[i])
min_z.append(d_z[i])
rd.writeConf(m_data,'x',min_x)
rd.writeConf(m_data,'y',min_y)
rd.writeConf(m_data,'z',min_z)
w_x = []
w_y = []
w_z = []
for i in range(0,len(d_x),10):
w_x.append(d_x[i])
w_y.append(d_y[i])
w_z.append(d_z[i])
rd.writeConf(f_data,'x',w_x)
rd.writeConf(f_data,'y',w_y)
rd.writeConf(f_data,'z',w_z)
f_data.close()
f_data = open('.//data//ins_' + str(case) + 'Best' + 'rs' + 'dis_pro2_922_s.dat', 'w')
rd.writeConf(f_data,'dis',dis_d_lst)
f_data.close()
# self._scatterLst.append(go.Scatter3d(x = v_x ,y = v_y , z = v_z, mode = 'markers', marker=dict(size = 8), name = '垂直'))
# self._scatterLst.append(go.Scatter3d(x = h_x ,y = h_y , z = h_z, mode = 'markers', marker=dict(size = 8),name = '水平'))
self._scatterLst.append(go.Scatter3d(x = [float(self._pnt_A.x)] ,y = [float(self._pnt_A.y)] , z = [float(self._pnt_A.z)],
mode = 'markers', marker = dict(size = 15),name = 'A'))
self._scatterLst.append(go.Scatter3d(x = [float(self._pnt_B.x)] ,y = [float(self._pnt_B.y)] , z = [float(self._pnt_B.z)],
mode = 'markers',marker = dict(size = 15),name = 'B'))
self._scatterLst.append(go.Scatter3d(x = d_x, y = d_y, z = d_z, mode='lines', line= dict(width = 3) , name='dUBINS'))
self._scatterLst.append(go.Scatter3d(x=p_x, y=p_y, z=p_z, mode='lines', line= dict(width = 3), name='path'))
for i in range(len(pos_dir_x)):
self._scatterLst.append(go.Scatter3d(x=[self._pntLst[self._path[i]].x, pos_dir_x[i] ],
y=[self._pntLst[self._path[i]].y, pos_dir_y[i] ],
z=[self._pntLst[self._path[i]].z, pos_dir_z[i] ], mode='lines', name = 'start',line=dict(width=5)))
for i in range(len(pos_dir_x)):
self._scatterLst.append(go.Scatter3d(x=[self._pntLst[self._path[i + 1]].x, end_dir_x[i] ],
y=[self._pntLst[self._path[i + 1]].y, end_dir_y[i] ],
z=[self._pntLst[self._path[i + 1]].z, end_dir_z[i] ], mode='lines', name = 'end', line=dict(width=5)))
fig = go.Figure(data=self._scatterLst)
plotly.offline.plot(fig, filename='test_ins_1_dubins.html')
for rowInd, colInd in componentUnit:
_robUnReachRowLst.append(rowInd)
_robUnReachColLst.append(colInd)
print(len(_robReachRowLst))
print(len(_robUnReachColLst))
# for
fileCfg = './/benchmarkData//r' + str(robNum) + '_r' + str(row) + '_c' + str(col) + '_s' + str(r_seed)\
+ insName
f_con = open(fileCfg, 'w')
f_con.write('time ' + datetime.datetime.now().strftime("%Y-%m-%d %H:%M:%S") +
'\n')
rd.writeConf(f_con, 'row', [row])
rd.writeConf(f_con, 'col', [col])
rd.writeConf(f_con, 'robRow', rob_row_lst)
rd.writeConf(f_con, 'robCol', rob_col_lst)
# rd.writeConf(f_con, 'obstacle', obstacleLst)
rd.writeConf(f_con, 'robReachRowLst', _robReachRowLst)
rd.writeConf(f_con, 'robReachColLst', _robReachColLst)
rd.writeConf(f_con, 'robUnReachRowLst', _robUnReachRowLst)
rd.writeConf(f_con, 'robUnReachColLst', _robUnReachColLst)
grid = []
for rowID, colID in np.ndindex(envMat.shape):
grid.append(int(envMat[rowID][colID]))
rd.writeConf(f_con, 'grid', grid)
f_con.close()
def writeConf(robNum, taskNum, combConf):
robPosLst = generatePos(robNum, combConf[0])
robAbiLst = generateRate(robNum, combConf[2], robPosLst)
robPosLst = [(robPos.x, robPos.y) for robPos in robPosLst]
robVelLst = [1 for x in range(robNum)]
sum_robAbi = sum(robAbiLst)
taskPosLst = generatePos(taskNum, combConf[1])
taskRateLst = generateRate(taskNum, combConf[3], taskPosLst)
conLst = generateConRate(taskNum,combConf[4],taskPosLst)
# deal with the unable instance
for i in range(len(taskRateLst)):
taskRate = taskRateLst[i]
while taskRate * robNum / 2 > sum_robAbi:
taskRate *= random.uniform(0.5, 1)
taskRateLst[i] = taskRate
taskPosLst = [(taskPos.x, taskPos.y) for taskPos in taskPosLst]
for i in range(len(taskRateLst)):
conLst[i] = conLst[i] * taskRateLst[i]
taskStateLst = [1 for i in range(taskNum)]
comp_threhold = 0.1
rob2tskDisMat = np.zeros((robNum, taskNum))
rob2tskDisLst = []
for i in range(robNum):
for j in range(taskNum):
d2 = float(EuclideanDis(robPosLst[i], taskPosLst[j]))
rob2tskDisMat[i][j] = d2
rob2tskDisLst.append(d2)
tskDisLst = []
tskDisMat = np.zeros((taskNum, taskNum))
for i in range(taskNum):
for j in range(taskNum):
d2 = EuclideanDis(taskPosLst[i], taskPosLst[j])
tskDisMat[i][j] = d2
tskDisLst.append(d2)
nameLst = []
nameLst.append(robNum)
nameLst.append(taskNum)
for disType in combConf:
nameLst.append(disType.name)
nameLst.append('thre' + str(comp_threhold))
nameLst = [str(x) for x in nameLst]
sep = '_'
sep = sep.join(nameLst)
insFileDir = './conBenchmark/'
insFileName = insFileDir + sep + 'MPDAins.dat'
f_ins = open(insFileName, 'w')
f_ins.write('time ' + datetime.datetime.now().strftime("%Y-%m-%d %H:%M:%S") + '\n')
f_ins.write(sep + '\n')
rd.writeConf(f_ins, 'robNum', [robNum])
rd.writeConf(f_ins, 'taskNum', [taskNum])
# writeConf(f_ins,'max_cordx',[max_cordx])
# writeConf(f_ins,'max_cordy',[max_cordy])
# writeConf(f_ins,'max_vel',[max_vel])
# writeConf(f_ins,'max_abi',[max_abi])
# writeConf(f_ins,'max_rat',[max_rat])
# writeConf(f_ins,'max_state',[max_state])
rd.writeConf(f_ins, 'comp_threhold', [comp_threhold])
rd.writeConf(f_ins, 'rob2tskDis', rob2tskDisLst)
rd.writeConf(f_ins, 'tskDis', tskDisLst)
f_ins.write('\n')
rob_xLst = [x[0] for x in robPosLst]
rob_yLst = [y[1] for y in robPosLst]
rd.writeConf(f_ins, 'rob_x', rob_xLst)
rd.writeConf(f_ins, 'rob_y', rob_yLst)
rd.writeConf(f_ins, 'rob_vel', robVelLst)
rd.writeConf(f_ins, 'rob_abi', robAbiLst)
f_ins.write('\n')
tsk_xLst = [x[0] for x in taskPosLst]
tsk_yLst = [y[1] for y in taskPosLst]
rd.writeConf(f_ins, 'tsk_x', tsk_xLst)
rd.writeConf(f_ins, 'tsk_y', tsk_yLst)
rd.writeConf(f_ins, 'tsk_rat', taskRateLst)
rd.writeConf(f_ins, 'tsk_state', taskStateLst)
rd.writeConf(f_ins,'tsk_con',conLst)
f_ins.close()
def calDubins(self,pos_a: Point3D,):
# '''
# 计算水平距离
# '''
# def calH(self,pos_a :Point3D, pos_b:Point3D):
# # print(pnt2d_a)
# dis = math.sqrt((pos_a.x- pos_b.x)**2 + (pos_a.y- pos_b.y)**2)
# return dis
# '''
# 计算垂直距离
# '''
# def calV(self,pos_a :Point3D, pos_b:Point3D):
# dis = abs(pos_b.z - pos_a.z)
# return dis
'''
计算垂直距离
'''
def calDis(self,pos_a :Point3D, pos_b:Point3D):
dis = math.sqrt((pos_a.x- pos_b.x)**2 + (pos_a.y- pos_b.y)**2 + (pos_a.z- pos_b.z)**2)
return dis
def draw(self):
self._scatterLst = []
v_x = []
v_y = []
v_z = []
h_x = []
h_y = []
h_z = []
for i in range(len(self._pntLst)):
if self._typeLst[i] == CorType.Vertical:
v_x.append(float(self._pntLst[i].x))
v_y.append(float(self._pntLst[i].y))
v_z.append(float(self._pntLst[i].z))
else:
h_x.append(float(self._pntLst[i].x))
h_y.append(float(self._pntLst[i].y))
h_z.append(float(self._pntLst[i].z))
s_x = []
s_y = []
s_z = []
for ind in self._tree:
s_x.append(self._pntLst[ind].x)
s_y.append(self._pntLst[ind].y)
s_z.append(self._pntLst[ind].z)
p_x = []
p_y = []
p_z = []
for ind in self._path:
p_x.append(self._pntLst[ind].x)
p_y.append(self._pntLst[ind].y)
p_z.append(self._pntLst[ind].z)
# print(len(v_x))
# raise Exception('XX')
# print(h_x)
# self._scatterLst.append(go.Scatter3d(x= [0,2], y=[0,3], z=[0,1]))
# self._scatterLst.append(go.Scatter3d(x = v_x ,y = v_y , z = v_z, mode = 'markers', marker=dict(color = 'blue', size = 5), name = '垂直'))
# self._scatterLst.append(go.Scatter3d(x = h_x ,y = h_y , z = h_z, mode = 'markers', marker=dict(color = 'orangered', size = 5),name = '水平'))
self._scatterLst.append(go.Scatter3d(x = v_x ,y = v_y , z = v_z, mode = 'markers', marker=dict(size = 8), name = '垂直'))
self._scatterLst.append(go.Scatter3d(x = h_x ,y = h_y , z = h_z, mode = 'markers', marker=dict(size = 8),name = '水平'))
self._scatterLst.append(go.Scatter3d(x = [float(self._pnt_A.x)] ,y = [float(self._pnt_A.y)] , z = [float(self._pnt_A.z)],
mode = 'markers', marker = dict(size = 15),name = 'A'))
self._scatterLst.append(go.Scatter3d(x = [float(self._pnt_B.x)] ,y = [float(self._pnt_B.y)] , z = [float(self._pnt_B.z)],
mode = 'markers',marker = dict(size = 15),name = 'B'))
self._scatterLst.append(go.Scatter3d(x=s_x, y=s_y, z=s_z, mode='markers', marker=dict(size= 5), name='search'))
self._scatterLst.append(go.Scatter3d(x=p_x, y=p_y, z=p_z, mode='lines', line= dict(width = 3), name='path'))
fig = go.Figure(data=self._scatterLst)
plotly.offline.plot(fig, filename='test_ins_2.html')
def drawTree(self):
lst = []
for edge in self._tree.edges:
# print(edge)
s_x = self._pntLst[edge[0]].x
s_y = self._pntLst[edge[0]].y
t_x = self._pntLst[edge[1]].x
t_y = self._pntLst[edge[1]].y
lst.append((s_x,s_y,t_x,t_y))
# raise Exception('xx')
# Pos(self._pntLst)
# for ind in self._tree:
# s_x.append(self._pntLst[ind].x)
# s_y.append(self._pntLst[ind].y)
# s_z.append(self._pntLst[ind].z)
#
_shapeLst = []
mark_x = []
mark_y = []
for p in range(len(lst)):
pnt0 = Pnt(lst[p][0], lst[p][1])
pnt1 = Pnt(lst[p][2], lst[p][3])
mark_x.append(pnt0.x)
mark_x.append(pnt1.x)
mark_y.append(pnt0.y)
mark_y.append(pnt1.y)
line = Line(pnt0, pnt1)
lineDic = line.line2dict()
# print(randColor())
lineDic['line']['color'] = 'darkred'
# lineDic['line']['color'] = 'rgba(15,15,15,0.5)'
lineDic['line']['width'] = 3
_shapeLst.append(lineDic)
#
markTrace = go.Scatter(mode='markers',
x=mark_x,
y=mark_y,
marker=dict(size=3),
name='Spanning-Tree')
_scatterLst = []
_scatterLst.append(markTrace)
layout = dict()
layout['shapes'] = _shapeLst
fig = go.Figure(data = _scatterLst, layout = layout)
fig.show()
# fig.show()
# sp.Point2D.distance()
def checkPath(self):
for _path in self._pathLst:
sta = self._init_sta
for i in range(1,len(_path)-1):
ind = _path[i]
valid, er = self.calER(sta, self._pntLst[ind], self._typeLst[ind])
print(ind,' ',self._tree.nodes[ind]['sta'].er)
print(ind,' ',er)
if valid == False:
raise Exception('error bug')
sta = STA(pos = self._pntLst[ind],er = er)
valid, er = self.calGoalER(sta)
print('end er = ',er)
# print(self._pntBind, ' ', self._tree.nodes[self._pntBind]['sta'].er)
if valid == False:
raise Exception('error bug')
def testP(self):
vaild,er = self.calER(self._init_sta,self._pntLst[1],self._typeLst[1])
print(self._pntLst[1])
print(self._typeLst[1])
print(vaild)
print(er)
def savePathLst(self,case,randSeed):
f_data = open('.//data//ins_'+str(case) + 'rs' + str(randSeed)+'_920_s.dat', 'w')
i = 0
for path in self._pathLst:
rd.writeConf(f_data,'path_' + str(i), path)
i += 1
f_data.close()