diff --git a/PathPlanning/BugPlanning/bug.py b/PathPlanning/BugPlanning/bug.py new file mode 100644 index 00000000..21bbbbb1 --- /dev/null +++ b/PathPlanning/BugPlanning/bug.py @@ -0,0 +1,327 @@ +""" +Bug Planning +author: Sarim Mehdi(muhammadsarim.mehdi@studio.unibo.it) +Source: https://sites.google.com/site/ece452bugalgorithms/ +""" + +import numpy as np +import matplotlib.pyplot as plt + +show_animation = True + + +class BugPlanner: + def __init__(self, start_x, start_y, goal_x, goal_y, obs_x, obs_y): + self.goal_x = goal_x + self.goal_y = goal_y + self.obs_x = obs_x + self.obs_y = obs_y + self.r_x = [start_x] + self.r_y = [start_y] + self.out_x = [] + self.out_y = [] + for o_x, o_y in zip(obs_x, obs_y): + for add_x, add_y in zip([1, 0, -1, -1, -1, 0, 1, 1], + [1, 1, 1, 0, -1, -1, -1, 0]): + cand_x, cand_y = o_x+add_x, o_y+add_y + valid_point = True + for _x, _y in zip(obs_x, obs_y): + if cand_x == _x and cand_y == _y: + valid_point = False + break + if valid_point: + self.out_x.append(cand_x), self.out_y.append(cand_y) + + def mov_normal(self): + return self.r_x[-1] + np.sign(self.goal_x - self.r_x[-1]), \ + self.r_y[-1] + np.sign(self.goal_y - self.r_y[-1]) + + def mov_to_next_obs(self, visited_x, visited_y): + for add_x, add_y in zip([1, 0, -1, 0], [0, 1, 0, -1]): + c_x, c_y = self.r_x[-1] + add_x, self.r_y[-1] + add_y + for _x, _y in zip(self.out_x, self.out_y): + use_pt = True + if c_x == _x and c_y == _y: + for v_x, v_y in zip(visited_x, visited_y): + if c_x == v_x and c_y == v_y: + use_pt = False + break + if use_pt: + return c_x, c_y, False + if not use_pt: + break + return self.r_x[-1], self.r_y[-1], True + + def bug0(self): + '''Greedy algorithm where you move towards goal + until you hit an obstacle. Then you go around it + (pick an arbitrary direction), until it is possible + for you to start moving towards goal in a greedy manner again''' + mov_dir = 'normal' + cand_x, cand_y = -np.inf, -np.inf + if show_animation: + plt.plot(self.obs_x, self.obs_y, ".k") + plt.plot(self.r_x[-1], self.r_y[-1], "og") + plt.plot(self.goal_x, self.goal_y, "xb") + plt.plot(self.out_x, self.out_y, ".") + plt.grid(True) + plt.title('BUG 0') + + for x_ob, y_ob in zip(self.out_x, self.out_y): + if self.r_x[-1] == x_ob and self.r_y[-1] == y_ob: + mov_dir = 'obs' + break + + visited_x, visited_y = [], [] + while True: + if self.r_x[-1] == self.goal_x and \ + self.r_y[-1] == self.goal_y: + break + if mov_dir == 'normal': + cand_x, cand_y = self.mov_normal() + if mov_dir == 'obs': + cand_x, cand_y, _ = self.mov_to_next_obs(visited_x, visited_y) + if mov_dir == 'normal': + found_boundary = False + for x_ob, y_ob in zip(self.out_x, self.out_y): + if cand_x == x_ob and cand_y == y_ob: + self.r_x.append(cand_x), self.r_y.append(cand_y) + visited_x[:], visited_y[:] = [], [] + visited_x.append(cand_x), visited_y.append(cand_y) + mov_dir = 'obs' + found_boundary = True + break + if not found_boundary: + self.r_x.append(cand_x), self.r_y.append(cand_y) + elif mov_dir == 'obs': + can_go_normal = True + for x_ob, y_ob in zip(self.obs_x, self.obs_y): + if self.mov_normal()[0] == x_ob and \ + self.mov_normal()[1] == y_ob: + can_go_normal = False + break + if can_go_normal: + mov_dir = 'normal' + else: + self.r_x.append(cand_x), self.r_y.append(cand_y) + visited_x.append(cand_x), visited_y.append(cand_y) + if show_animation: + plt.plot(self.r_x, self.r_y, "-r") + plt.pause(0.001) + if show_animation: + plt.show() + + def bug1(self): + '''Move towards goal in a greedy manner. + When you hit an obstacle, you go around it and + back to where you hit the obstacle initially. + Then, you go to the point on the obstacle that is + closest to your goal and you start moving towards + goal in a greedy manner from that new point.''' + mov_dir = 'normal' + cand_x, cand_y = -np.inf, -np.inf + exit_x, exit_y = -np.inf, -np.inf + dist = np.inf + back_to_start = False + second_round = False + if show_animation: + plt.plot(self.obs_x, self.obs_y, ".k") + plt.plot(self.r_x[-1], self.r_y[-1], "og") + plt.plot(self.goal_x, self.goal_y, "xb") + plt.plot(self.out_x, self.out_y, ".") + plt.grid(True) + plt.title('BUG 1') + + for xob, yob in zip(self.out_x, self.out_y): + if self.r_x[-1] == xob and self.r_y[-1] == yob: + mov_dir = 'obs' + break + + visited_x, visited_y = [], [] + while True: + if self.r_x[-1] == self.goal_x and \ + self.r_y[-1] == self.goal_y: + break + if mov_dir == 'normal': + cand_x, cand_y = self.mov_normal() + if mov_dir == 'obs': + cand_x, cand_y, back_to_start = \ + self.mov_to_next_obs(visited_x, visited_y) + if mov_dir == 'normal': + found_boundary = False + for x_ob, y_ob in zip(self.out_x, self.out_y): + if cand_x == x_ob and cand_y == y_ob: + self.r_x.append(cand_x), self.r_y.append(cand_y) + visited_x[:], visited_y[:] = [], [] + visited_x.append(cand_x), visited_y.append(cand_y) + mov_dir = 'obs' + dist = np.inf + back_to_start = False + second_round = False + found_boundary = True + break + if not found_boundary: + self.r_x.append(cand_x), self.r_y.append(cand_y) + elif mov_dir == 'obs': + d = np.linalg.norm(np.array([cand_x, cand_y] - + np.array([self.goal_x, + self.goal_y]))) + if d < dist and not second_round: + exit_x, exit_y = cand_x, cand_y + dist = d + if back_to_start and not second_round: + second_round = True + del self.r_x[-len(visited_x):] + del self.r_y[-len(visited_y):] + visited_x[:], visited_y[:] = [], [] + self.r_x.append(cand_x), self.r_y.append(cand_y) + visited_x.append(cand_x), visited_y.append(cand_y) + if cand_x == exit_x and \ + cand_y == exit_y and \ + second_round: + mov_dir = 'normal' + if show_animation: + plt.plot(self.r_x, self.r_y, "-r") + plt.pause(0.001) + if show_animation: + plt.show() + + def bug2(self): + '''Move towards goal in a greedy manner. + When you hit an obstacle, you go around it and + keep track of your distance from the goal. + If the distance from your goal was decreasing before + and now it starts increasing, that means the current + point is probably the closest point to the + goal (this may or may not be true because the algorithm + doesn't explore the entire boundary around the obstacle). + So, you depart from this point and continue towards the + goal in a greedy manner''' + mov_dir = 'normal' + cand_x, cand_y = -np.inf, -np.inf + if show_animation: + plt.plot(self.obs_x, self.obs_y, ".k") + plt.plot(self.r_x[-1], self.r_y[-1], "og") + plt.plot(self.goal_x, self.goal_y, "xb") + plt.plot(self.out_x, self.out_y, ".") + + straight_x, straight_y = [self.r_x[-1]], [self.r_y[-1]] + hit_x, hit_y = [], [] + while True: + if straight_x[-1] == self.goal_x and \ + straight_y[-1] == self.goal_y: + break + c_x = straight_x[-1] + np.sign(self.goal_x - straight_x[-1]) + c_y = straight_y[-1] + np.sign(self.goal_y - straight_y[-1]) + for x_ob, y_ob in zip(self.out_x, self.out_y): + if c_x == x_ob and c_y == y_ob: + hit_x.append(c_x), hit_y.append(c_y) + break + straight_x.append(c_x), straight_y.append(c_y) + if show_animation: + plt.plot(straight_x, straight_y, ",") + plt.plot(hit_x, hit_y, "d") + plt.grid(True) + plt.title('BUG 2') + + for x_ob, y_ob in zip(self.out_x, self.out_y): + if self.r_x[-1] == x_ob and self.r_y[-1] == y_ob: + mov_dir = 'obs' + break + + visited_x, visited_y = [], [] + while True: + if self.r_x[-1] == self.goal_x \ + and self.r_y[-1] == self.goal_y: + break + if mov_dir == 'normal': + cand_x, cand_y = self.mov_normal() + if mov_dir == 'obs': + cand_x, cand_y, _ = self.mov_to_next_obs(visited_x, visited_y) + if mov_dir == 'normal': + found_boundary = False + for x_ob, y_ob in zip(self.out_x, self.out_y): + if cand_x == x_ob and cand_y == y_ob: + self.r_x.append(cand_x), self.r_y.append(cand_y) + visited_x[:], visited_y[:] = [], [] + visited_x.append(cand_x), visited_y.append(cand_y) + del hit_x[0] + del hit_y[0] + mov_dir = 'obs' + found_boundary = True + break + if not found_boundary: + self.r_x.append(cand_x), self.r_y.append(cand_y) + elif mov_dir == 'obs': + self.r_x.append(cand_x), self.r_y.append(cand_y) + visited_x.append(cand_x), visited_y.append(cand_y) + for i_x, i_y in zip(range(len(hit_x)), range(len(hit_y))): + if cand_x == hit_x[i_x] and cand_y == hit_y[i_y]: + del hit_x[i_x] + del hit_y[i_y] + mov_dir = 'normal' + break + if show_animation: + plt.plot(self.r_x, self.r_y, "-r") + plt.pause(0.001) + if show_animation: + plt.show() + + +def main(bug_0, bug_1, bug_2): + # set obstacle positions + o_x, o_y = [], [] + + s_x = 0.0 + s_y = 0.0 + g_x = 167.0 + g_y = 50.0 + + for i in range(20, 40): + for j in range(20, 40): + o_x.append(i) + o_y.append(j) + + for i in range(60, 100): + for j in range(40, 80): + o_x.append(i) + o_y.append(j) + + for i in range(120, 140): + for j in range(80, 100): + o_x.append(i) + o_y.append(j) + + for i in range(80, 140): + for j in range(0, 20): + o_x.append(i) + o_y.append(j) + + for i in range(0, 20): + for j in range(60, 100): + o_x.append(i) + o_y.append(j) + + for i in range(20, 40): + for j in range(80, 100): + o_x.append(i) + o_y.append(j) + + for i in range(120, 160): + for j in range(40, 60): + o_x.append(i) + o_y.append(j) + + if bug_0: + my_Bug = BugPlanner(s_x, s_y, g_x, g_y, o_x, o_y) + my_Bug.bug0() + if bug_1: + my_Bug = BugPlanner(s_x, s_y, g_x, g_y, o_x, o_y) + my_Bug.bug1() + if bug_2: + my_Bug = BugPlanner(s_x, s_y, g_x, g_y, o_x, o_y) + my_Bug.bug2() + + +if __name__ == '__main__': + main(bug_0=True, bug_1=False, bug_2=False) diff --git a/tests/test_bug.py b/tests/test_bug.py new file mode 100644 index 00000000..31d8b2c2 --- /dev/null +++ b/tests/test_bug.py @@ -0,0 +1,17 @@ +from PathPlanning.BugPlanning import bug as b +from unittest import TestCase +import sys +import os +sys.path.append(os.path.dirname(__file__) + "/../") + + +class Test(TestCase): + + def test(self): + b.show_animation = False + b.main(bug_0=True, bug_1=True, bug_2=True) + + +if __name__ == '__main__': # pragma: no cover + test = Test() + test.test()