Skip to content

Commit 409be20

Browse files
sarimmehdisarimmehdi
authored
Implemented bug algorithms (AtsushiSakai#378)
* Create bug.py * Update bug.py * Update bug.py * Update bug.py * Update bug.py * Update bug.py * Add files via upload * Update test_bug.py * Update test_bug.py * Update bug.py * Update test_bug.py * Delete bug.py * Create BugPlanning * Delete BugPlanning * Create bug.py * Update bug.py * Update test_bug.py * Update bug.py * Update bug.py
1 parent fa22708 commit 409be20

File tree

2 files changed

+344
-0
lines changed

2 files changed

+344
-0
lines changed

‎PathPlanning/BugPlanning/bug.py‎

Lines changed: 327 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,327 @@
1+
"""
2+
Bug Planning
3+
author: Sarim Mehdi([email protected])
4+
Source: https://sites.google.com/site/ece452bugalgorithms/
5+
"""
6+
7+
import numpy as np
8+
import matplotlib.pyplot as plt
9+
10+
show_animation = True
11+
12+
13+
class BugPlanner:
14+
def __init__(self, start_x, start_y, goal_x, goal_y, obs_x, obs_y):
15+
self.goal_x = goal_x
16+
self.goal_y = goal_y
17+
self.obs_x = obs_x
18+
self.obs_y = obs_y
19+
self.r_x = [start_x]
20+
self.r_y = [start_y]
21+
self.out_x = []
22+
self.out_y = []
23+
for o_x, o_y in zip(obs_x, obs_y):
24+
for add_x, add_y in zip([1, 0, -1, -1, -1, 0, 1, 1],
25+
[1, 1, 1, 0, -1, -1, -1, 0]):
26+
cand_x, cand_y = o_x+add_x, o_y+add_y
27+
valid_point = True
28+
for _x, _y in zip(obs_x, obs_y):
29+
if cand_x == _x and cand_y == _y:
30+
valid_point = False
31+
break
32+
if valid_point:
33+
self.out_x.append(cand_x), self.out_y.append(cand_y)
34+
35+
def mov_normal(self):
36+
return self.r_x[-1] + np.sign(self.goal_x - self.r_x[-1]), \
37+
self.r_y[-1] + np.sign(self.goal_y - self.r_y[-1])
38+
39+
def mov_to_next_obs(self, visited_x, visited_y):
40+
for add_x, add_y in zip([1, 0, -1, 0], [0, 1, 0, -1]):
41+
c_x, c_y = self.r_x[-1] + add_x, self.r_y[-1] + add_y
42+
for _x, _y in zip(self.out_x, self.out_y):
43+
use_pt = True
44+
if c_x == _x and c_y == _y:
45+
for v_x, v_y in zip(visited_x, visited_y):
46+
if c_x == v_x and c_y == v_y:
47+
use_pt = False
48+
break
49+
if use_pt:
50+
return c_x, c_y, False
51+
if not use_pt:
52+
break
53+
return self.r_x[-1], self.r_y[-1], True
54+
55+
def bug0(self):
56+
'''Greedy algorithm where you move towards goal
57+
until you hit an obstacle. Then you go around it
58+
(pick an arbitrary direction), until it is possible
59+
for you to start moving towards goal in a greedy manner again'''
60+
mov_dir = 'normal'
61+
cand_x, cand_y = -np.inf, -np.inf
62+
if show_animation:
63+
plt.plot(self.obs_x, self.obs_y, ".k")
64+
plt.plot(self.r_x[-1], self.r_y[-1], "og")
65+
plt.plot(self.goal_x, self.goal_y, "xb")
66+
plt.plot(self.out_x, self.out_y, ".")
67+
plt.grid(True)
68+
plt.title('BUG 0')
69+
70+
for x_ob, y_ob in zip(self.out_x, self.out_y):
71+
if self.r_x[-1] == x_ob and self.r_y[-1] == y_ob:
72+
mov_dir = 'obs'
73+
break
74+
75+
visited_x, visited_y = [], []
76+
while True:
77+
if self.r_x[-1] == self.goal_x and \
78+
self.r_y[-1] == self.goal_y:
79+
break
80+
if mov_dir == 'normal':
81+
cand_x, cand_y = self.mov_normal()
82+
if mov_dir == 'obs':
83+
cand_x, cand_y, _ = self.mov_to_next_obs(visited_x, visited_y)
84+
if mov_dir == 'normal':
85+
found_boundary = False
86+
for x_ob, y_ob in zip(self.out_x, self.out_y):
87+
if cand_x == x_ob and cand_y == y_ob:
88+
self.r_x.append(cand_x), self.r_y.append(cand_y)
89+
visited_x[:], visited_y[:] = [], []
90+
visited_x.append(cand_x), visited_y.append(cand_y)
91+
mov_dir = 'obs'
92+
found_boundary = True
93+
break
94+
if not found_boundary:
95+
self.r_x.append(cand_x), self.r_y.append(cand_y)
96+
elif mov_dir == 'obs':
97+
can_go_normal = True
98+
for x_ob, y_ob in zip(self.obs_x, self.obs_y):
99+
if self.mov_normal()[0] == x_ob and \
100+
self.mov_normal()[1] == y_ob:
101+
can_go_normal = False
102+
break
103+
if can_go_normal:
104+
mov_dir = 'normal'
105+
else:
106+
self.r_x.append(cand_x), self.r_y.append(cand_y)
107+
visited_x.append(cand_x), visited_y.append(cand_y)
108+
if show_animation:
109+
plt.plot(self.r_x, self.r_y, "-r")
110+
plt.pause(0.001)
111+
if show_animation:
112+
plt.show()
113+
114+
def bug1(self):
115+
'''Move towards goal in a greedy manner.
116+
When you hit an obstacle, you go around it and
117+
back to where you hit the obstacle initially.
118+
Then, you go to the point on the obstacle that is
119+
closest to your goal and you start moving towards
120+
goal in a greedy manner from that new point.'''
121+
mov_dir = 'normal'
122+
cand_x, cand_y = -np.inf, -np.inf
123+
exit_x, exit_y = -np.inf, -np.inf
124+
dist = np.inf
125+
back_to_start = False
126+
second_round = False
127+
if show_animation:
128+
plt.plot(self.obs_x, self.obs_y, ".k")
129+
plt.plot(self.r_x[-1], self.r_y[-1], "og")
130+
plt.plot(self.goal_x, self.goal_y, "xb")
131+
plt.plot(self.out_x, self.out_y, ".")
132+
plt.grid(True)
133+
plt.title('BUG 1')
134+
135+
for xob, yob in zip(self.out_x, self.out_y):
136+
if self.r_x[-1] == xob and self.r_y[-1] == yob:
137+
mov_dir = 'obs'
138+
break
139+
140+
visited_x, visited_y = [], []
141+
while True:
142+
if self.r_x[-1] == self.goal_x and \
143+
self.r_y[-1] == self.goal_y:
144+
break
145+
if mov_dir == 'normal':
146+
cand_x, cand_y = self.mov_normal()
147+
if mov_dir == 'obs':
148+
cand_x, cand_y, back_to_start = \
149+
self.mov_to_next_obs(visited_x, visited_y)
150+
if mov_dir == 'normal':
151+
found_boundary = False
152+
for x_ob, y_ob in zip(self.out_x, self.out_y):
153+
if cand_x == x_ob and cand_y == y_ob:
154+
self.r_x.append(cand_x), self.r_y.append(cand_y)
155+
visited_x[:], visited_y[:] = [], []
156+
visited_x.append(cand_x), visited_y.append(cand_y)
157+
mov_dir = 'obs'
158+
dist = np.inf
159+
back_to_start = False
160+
second_round = False
161+
found_boundary = True
162+
break
163+
if not found_boundary:
164+
self.r_x.append(cand_x), self.r_y.append(cand_y)
165+
elif mov_dir == 'obs':
166+
d = np.linalg.norm(np.array([cand_x, cand_y] -
167+
np.array([self.goal_x,
168+
self.goal_y])))
169+
if d < dist and not second_round:
170+
exit_x, exit_y = cand_x, cand_y
171+
dist = d
172+
if back_to_start and not second_round:
173+
second_round = True
174+
del self.r_x[-len(visited_x):]
175+
del self.r_y[-len(visited_y):]
176+
visited_x[:], visited_y[:] = [], []
177+
self.r_x.append(cand_x), self.r_y.append(cand_y)
178+
visited_x.append(cand_x), visited_y.append(cand_y)
179+
if cand_x == exit_x and \
180+
cand_y == exit_y and \
181+
second_round:
182+
mov_dir = 'normal'
183+
if show_animation:
184+
plt.plot(self.r_x, self.r_y, "-r")
185+
plt.pause(0.001)
186+
if show_animation:
187+
plt.show()
188+
189+
def bug2(self):
190+
'''Move towards goal in a greedy manner.
191+
When you hit an obstacle, you go around it and
192+
keep track of your distance from the goal.
193+
If the distance from your goal was decreasing before
194+
and now it starts increasing, that means the current
195+
point is probably the closest point to the
196+
goal (this may or may not be true because the algorithm
197+
doesn't explore the entire boundary around the obstacle).
198+
So, you depart from this point and continue towards the
199+
goal in a greedy manner'''
200+
mov_dir = 'normal'
201+
cand_x, cand_y = -np.inf, -np.inf
202+
if show_animation:
203+
plt.plot(self.obs_x, self.obs_y, ".k")
204+
plt.plot(self.r_x[-1], self.r_y[-1], "og")
205+
plt.plot(self.goal_x, self.goal_y, "xb")
206+
plt.plot(self.out_x, self.out_y, ".")
207+
208+
straight_x, straight_y = [self.r_x[-1]], [self.r_y[-1]]
209+
hit_x, hit_y = [], []
210+
while True:
211+
if straight_x[-1] == self.goal_x and \
212+
straight_y[-1] == self.goal_y:
213+
break
214+
c_x = straight_x[-1] + np.sign(self.goal_x - straight_x[-1])
215+
c_y = straight_y[-1] + np.sign(self.goal_y - straight_y[-1])
216+
for x_ob, y_ob in zip(self.out_x, self.out_y):
217+
if c_x == x_ob and c_y == y_ob:
218+
hit_x.append(c_x), hit_y.append(c_y)
219+
break
220+
straight_x.append(c_x), straight_y.append(c_y)
221+
if show_animation:
222+
plt.plot(straight_x, straight_y, ",")
223+
plt.plot(hit_x, hit_y, "d")
224+
plt.grid(True)
225+
plt.title('BUG 2')
226+
227+
for x_ob, y_ob in zip(self.out_x, self.out_y):
228+
if self.r_x[-1] == x_ob and self.r_y[-1] == y_ob:
229+
mov_dir = 'obs'
230+
break
231+
232+
visited_x, visited_y = [], []
233+
while True:
234+
if self.r_x[-1] == self.goal_x \
235+
and self.r_y[-1] == self.goal_y:
236+
break
237+
if mov_dir == 'normal':
238+
cand_x, cand_y = self.mov_normal()
239+
if mov_dir == 'obs':
240+
cand_x, cand_y, _ = self.mov_to_next_obs(visited_x, visited_y)
241+
if mov_dir == 'normal':
242+
found_boundary = False
243+
for x_ob, y_ob in zip(self.out_x, self.out_y):
244+
if cand_x == x_ob and cand_y == y_ob:
245+
self.r_x.append(cand_x), self.r_y.append(cand_y)
246+
visited_x[:], visited_y[:] = [], []
247+
visited_x.append(cand_x), visited_y.append(cand_y)
248+
del hit_x[0]
249+
del hit_y[0]
250+
mov_dir = 'obs'
251+
found_boundary = True
252+
break
253+
if not found_boundary:
254+
self.r_x.append(cand_x), self.r_y.append(cand_y)
255+
elif mov_dir == 'obs':
256+
self.r_x.append(cand_x), self.r_y.append(cand_y)
257+
visited_x.append(cand_x), visited_y.append(cand_y)
258+
for i_x, i_y in zip(range(len(hit_x)), range(len(hit_y))):
259+
if cand_x == hit_x[i_x] and cand_y == hit_y[i_y]:
260+
del hit_x[i_x]
261+
del hit_y[i_y]
262+
mov_dir = 'normal'
263+
break
264+
if show_animation:
265+
plt.plot(self.r_x, self.r_y, "-r")
266+
plt.pause(0.001)
267+
if show_animation:
268+
plt.show()
269+
270+
271+
def main(bug_0, bug_1, bug_2):
272+
# set obstacle positions
273+
o_x, o_y = [], []
274+
275+
s_x = 0.0
276+
s_y = 0.0
277+
g_x = 167.0
278+
g_y = 50.0
279+
280+
for i in range(20, 40):
281+
for j in range(20, 40):
282+
o_x.append(i)
283+
o_y.append(j)
284+
285+
for i in range(60, 100):
286+
for j in range(40, 80):
287+
o_x.append(i)
288+
o_y.append(j)
289+
290+
for i in range(120, 140):
291+
for j in range(80, 100):
292+
o_x.append(i)
293+
o_y.append(j)
294+
295+
for i in range(80, 140):
296+
for j in range(0, 20):
297+
o_x.append(i)
298+
o_y.append(j)
299+
300+
for i in range(0, 20):
301+
for j in range(60, 100):
302+
o_x.append(i)
303+
o_y.append(j)
304+
305+
for i in range(20, 40):
306+
for j in range(80, 100):
307+
o_x.append(i)
308+
o_y.append(j)
309+
310+
for i in range(120, 160):
311+
for j in range(40, 60):
312+
o_x.append(i)
313+
o_y.append(j)
314+
315+
if bug_0:
316+
my_Bug = BugPlanner(s_x, s_y, g_x, g_y, o_x, o_y)
317+
my_Bug.bug0()
318+
if bug_1:
319+
my_Bug = BugPlanner(s_x, s_y, g_x, g_y, o_x, o_y)
320+
my_Bug.bug1()
321+
if bug_2:
322+
my_Bug = BugPlanner(s_x, s_y, g_x, g_y, o_x, o_y)
323+
my_Bug.bug2()
324+
325+
326+
if __name__ == '__main__':
327+
main(bug_0=True, bug_1=False, bug_2=False)

‎tests/test_bug.py‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
from PathPlanning.BugPlanning import bug as b
2+
from unittest import TestCase
3+
import sys
4+
import os
5+
sys.path.append(os.path.dirname(__file__) + "/../")
6+
7+
8+
class Test(TestCase):
9+
10+
def test(self):
11+
b.show_animation = False
12+
b.main(bug_0=True, bug_1=True, bug_2=True)
13+
14+
15+
if __name__ == '__main__': # pragma: no cover
16+
test = Test()
17+
test.test()

0 commit comments

Comments
 (0)