c='#8B4513')
hengzuobiao.append(circle_x)
zongzuobiao.append(circle_y)
plt.xlim(-5, 30)
plt.ylim(0, 30)
plt.plot(hengzuobiao, zongzuobiao, '+', c='#1C1C1C')
plt.show()
circle_x = circle_x - 0.25
print("转弯结束点")
print(circle_x, circle_y)
a = circle_x
b = circle_y
print("开始直行")
print("经判定，直行路径与左侧障碍物的距离小于安全距离，所以先直行，再斜着前进")
print(
func.jugg(func.houtui_x(a), func.houtui_y(b), 13.127906, 14.850528,
20.072206, 18.822528, 5.012206, 12.971528, 9.265606, 15.404528))
#上障碍物和橘色路线
while (func.jugg(func.houtui_x(a), func.houtui_y(b), 13.127906, 14.850528,
20.072206, 18.822528, -3.120794, 9.912528, 11.149906,
18.121528) == 1):
plt.plot((1.380606, 15.301006, 14.405506, 0.134806, 1.380606),
(2.756528, 10.696528, 11.953528, 3.744528, 2.756528),
c='r')
plt.plot((2.998806, 30.323206, 23.792806, -4.748594, 2.998806),
(0.220528, 15.844528, 29.414528, 12.996528, 0.220528),
c='b')
plt.plot((11.149906, -3.120794), (18.121528, 9.912528), c='#EE7621')
plt.plot((12.777706, -1.492994), (15.037528, 6.828528), c='g')
plt.plot((20.072206, 13.127906, 14.071306, 21.015606, 20.072206),
(18.822528, 14.850528, 13.201528, 17.173528, 18.822528),
(19.744528, 17.311528, 12.971528, 15.404528, 19.744528),
c='#8B4513')
hengzuobiao.append(circle_x)
zongzuobiao.append(circle_y)
plt.xlim(-5, 30)
plt.ylim(0, 30)
plt.plot(hengzuobiao, zongzuobiao, '+', c='#1C1C1C')
plt.show()
circle_x = circle_x - 0.25
print("转弯结束点")
print(circle_x, circle_y)
a = circle_x
b = circle_y
print("开始直行")
# print(jugg(houtui_x(a),houtui_y(b),14.405506,11.953528,0.134806,3.744528,12.777706,15.037528,-1.492994,6.828528))
while (func.jugg(func.houtui_x(a), func.houtui_y(b), 14.405506, 11.953528,
0.134806, 3.744528, 12.777706, 15.037528, -1.492994,
6.828528) == 1):
plt.plot((1.380606, 15.301006, 14.405506, 0.134806, 1.380606),
(2.756528, 10.696528, 11.953528, 3.744528, 2.756528),
c='r')
plt.plot((2.998806, 30.323206, 23.792806, -4.748594, 2.998806),
(0.220528, 15.844528, 29.414528, 12.996528, 0.220528),
c='b')
plt.plot((11.149906, -3.120794), (18.121528, 9.912528), c='#EE7621')
plt.plot((12.777706, -1.492994), (15.037528, 6.828528), c='g')
plt.plot((6.783106, 2.529706, 5.012206, 9.265606, 6.783106),
(19.744528, 17.311528, 12.971528, 15.404528, 19.744528),
c='#8B4513')
a = func.houtui_x(a)
b = func.houtui_y(b)
(19.744528, 17.311528, 12.971528, 15.404528, 19.744528), c='#8B4513')
plt.plot((18.39721694, 20.73791694, 13.59911694, 11.25831694, 18.39721694),
(9.025287575, 10.36928757, 23.54228757, 22.19828757, 9.025287575), c='#FFD700')
hengzuobiao.append(circle_x)
zongzuobiao.append(circle_y)
plt.xlim(-5, 30)
plt.ylim(0, 30)
plt.plot(hengzuobiao, zongzuobiao,'+',c ='#1C1C1C')
plt.show()
circle_x = circle_x - 0.25
print("转弯结束点")
print(circle_x,circle_y)
a = circle_x;b = circle_y
print("开始直行")
print("经判定，直行路径与左侧障碍物的距离小于安全距离，所以先直行，再斜着前进")
print(func.jugg(func.houtui_x(a),func.houtui_y(b),13.127906,14.850528,20.072206,18.822528,5.012206,12.971528,9.265606,15.404528))
#上障碍物和橘色路线
while(func.jugg(func.houtui_x(a),func.houtui_y(b),13.127906,14.850528,20.072206,18.822528,-3.120794,9.912528,11.149906,18.121528)==1):
plt.plot((1.380606, 15.301006, 14.405506, 0.134806,1.380606), (2.756528, 10.696528, 11.953528, 3.744528,2.756528),c ='r')
plt.plot((2.998806,30.323206,23.792806,-4.748594,2.998806), (0.220528,15.844528,29.414528,12.996528,0.220528),c='b')
plt.plot((11.149906,-3.120794), (18.121528,9.912528),c='#EE7621')
plt.plot((12.777706,-1.492994), (15.037528,6.828528),c='g')
plt.plot((20.072206, 13.127906, 14.071306, 21.015606, 20.072206),
(18.822528, 14.850528, 13.201528, 17.173528, 18.822528),
c='#9A32CD')
plt.plot((6.783106, 2.529706, 5.012206, 9.265606, 6.783106),
(19.744528, 17.311528, 12.971528, 15.404528, 19.744528), c='#8B4513')
plt.plot((18.39721694, 20.73791694, 13.59911694, 11.25831694, 18.39721694),
(9.025287575, 10.36928757, 23.54228757, 22.19828757, 9.025287575), c='#FFD700')
a=func.houtui_x(a)
b=func.houtui_y(b)
(18.822528, 14.850528, 13.201528, 17.173528, 18.822528),
c='#9A32CD')
hengzuobiao.append(circle_x)
zongzuobiao.append(circle_y)
plt.xlim(-5, 30)
plt.ylim(0, 30)
plt.plot(hengzuobiao, zongzuobiao, '+', c='#1C1C1C')
plt.show()
circle_x = circle_x - 0.25
print("转弯结束点")
print(circle_x, circle_y)
a = circle_x
b = circle_y
print("开始直行")
# print(jugg(houtui_x(a),houtui_y(b),14.405506,11.953528,0.134806,3.744528,12.777706,15.037528,-1.492994,6.828528))
while (func.jugg(func.houtui_x(a), func.houtui_y(b), 13.127906, 14.850528,
20.072206, 18.822528, 11.149906, 18.121528, -3.120794,
9.912528) == 1):
plt.plot((1.380606, 15.301006, 14.405506, 0.134806, 1.380606),
(2.756528, 10.696528, 11.953528, 3.744528, 2.756528),
c='r')
plt.plot((2.998806, 30.323206, 23.792806, -4.748594, 2.998806),
(0.220528, 15.844528, 29.414528, 12.996528, 0.220528),
c='b')
plt.plot((11.149906, -3.120794), (18.121528, 9.912528), c='#EE7621')
plt.plot((12.777706, -1.492994), (15.037528, 6.828528), c='g')
plt.plot((20.072206, 13.127906, 14.071306, 21.015606, 20.072206),
(18.822528, 14.850528, 13.201528, 17.173528, 18.822528),
c='#9A32CD')
a = func.houtui_x(a)
b = func.houtui_y(b)
(func.py_x(20.072206), func.py_x(13.127906), func.py_x(14.071306), func.py_x(21.015606), func.py_x(20.072206)),
(func.py_y(18.822528), func.py_y(14.850528), func.py_y(13.201528), func.py_y(17.173528), func.py_y(18.822528)),
c='#9A32CD')
hengzuobiao.append(circle_x)
zongzuobiao.append(circle_y)
plt.xlim(-5, 30)
plt.ylim(0, 30)
plt.plot(hengzuobiao, zongzuobiao,'+',c ='#1C1C1C')
plt.show()
circle_x = circle_x - 0.25
print("转弯结束点")
print(circle_x,circle_y)
a = circle_x;b = circle_y
print("开始直行")
# print(jugg(houtui_x(a),houtui_y(b),14.405506,11.953528,0.134806,3.744528,12.777706,15.037528,-1.492994,6.828528))
while(func.jugg(func.houtui_x(a),func.houtui_y(b),13.127906,14.850528,20.072206,18.822528,11.149906,18.121528,-3.120794,9.912528)==1):
plt.plot((1.380606, 15.301006, 14.405506, 0.134806,1.380606), (2.756528, 10.696528, 11.953528, 3.744528,2.756528),c ='r')
plt.plot((2.998806,30.323206,23.792806,-4.748594,2.998806), (0.220528,15.844528,29.414528,12.996528,0.220528),c='b')
plt.plot((11.149906,-3.120794), (18.121528,9.912528),c='#EE7621')
plt.plot((12.777706,-1.492994), (15.037528,6.828528),c='g')
plt.plot(
(func.py_x(20.072206), func.py_x(13.127906), func.py_x(14.071306), func.py_x(21.015606), func.py_x(20.072206)),
(func.py_y(18.822528), func.py_y(14.850528), func.py_y(13.201528), func.py_y(17.173528), func.py_y(18.822528)),
c='#9A32CD')
a=func.houtui_x(a)
b=func.houtui_y(b)
hengzuobiao.append(a)
zongzuobiao.append(b)
plt.xlim(-5, 30)
plt.ylim(0, 30)
plt.plot(hengzuobiao, zongzuobiao,'+',c ='#1C1C1C')