#395 Added variants of A*

Merged sarimmehdi
Coverage Reach
PathPlanning/AStar/a_star_variants.py PathPlanning/AStar/a_star.py PathPlanning/HybridAStar/hybrid_a_star.py PathPlanning/HybridAStar/dynamic_programming_heuristic.py PathPlanning/HybridAStar/car.py PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py PathPlanning/ClosedLoopRRTStar/pure_pursuit.py PathPlanning/ClosedLoopRRTStar/unicycle_model.py PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py PathPlanning/BugPlanning/bug.py PathPlanning/RRT/rrt.py PathPlanning/RRT/rrt_with_pathsmoothing.py PathPlanning/DubinsPath/dubins_path_planning.py PathPlanning/InformedRRTStar/informed_rrt_star.py PathPlanning/SpiralSpanningTreeCPP/spiral_spanning_tree_coverage_path_planner.py PathPlanning/StateLatticePlanner/state_lattice_planner.py PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py PathPlanning/VoronoiRoadMap/voronoi_road_map.py PathPlanning/VoronoiRoadMap/dijkstra_search.py PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py PathPlanning/Eta3SplinePath/eta3_spline_path.py PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py PathPlanning/VisibilityRoadMap/visibility_road_map.py PathPlanning/VisibilityRoadMap/geometry.py PathPlanning/DynamicWindowApproach/dynamic_window_approach.py PathPlanning/Dijkstra/dijkstra.py PathPlanning/RRTStarDubins/rrt_star_dubins.py PathPlanning/LQRRRTStar/lqr_rrt_star.py PathPlanning/DepthFirstSearch/depth_first_search.py PathPlanning/BreadthFirstSearch/breadth_first_search.py PathPlanning/PotentialFieldPlanning/potential_field_planning.py PathPlanning/RRTStar/rrt_star.py PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py PathPlanning/RRTDubins/rrt_dubins.py PathPlanning/WavefrontCPP/wavefront_coverage_path_planner.py PathPlanning/CubicSpline/cubic_spline_planner.py PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py PathPlanning/BezierPath/bezier_path.py PathPlanning/LQRPlanner/LQRplanner.py SLAM/FastSLAM2/fast_slam2.py SLAM/FastSLAM1/fast_slam1.py SLAM/GraphBasedSLAM/graph_based_slam.py SLAM/EKFSLAM/ekf_slam.py SLAM/iterative_closest_point/iterative_closest_point.py tests/test_grid_based_sweep_coverage_path_planner.py tests/test_wavefront_coverage_path_planner.py tests/test_dubins_path_planning.py tests/test_spiral_spanning_tree_coverage_path_planner.py tests/test_reeds_shepp_path_planning.py tests/test_grid_map_lib.py tests/test_rrt.py tests/test_rrt_star.py tests/test_histogram_filter.py tests/test_dynamic_window_approach.py tests/test_lqr_rrt_star.py tests/test_state_lattice_planner.py tests/test_closed_loop_rrt_star_car.py tests/test_frenet_optimal_trajectory.py tests/test_batch_informed_rrt_star.py tests/test_fast_slam1.py tests/test_rrt_dubins.py tests/test_graph_based_slam.py tests/test_rrt_star_reeds_shepp.py tests/test_fast_slam2.py tests/test_bipedal_planner.py tests/test_informed_rrt_star.py tests/test_depth_first_search.py tests/test_rrt_star_dubins.py tests/test_hybrid_a_star.py tests/test_breadth_first_search.py tests/test_dijkstra.py tests/test_a_star.py tests/test_n_joint_arm_to_point_control.py tests/test_a_star_variants.py tests/test_visibility_road_map_planner.py tests/test_bezier_path.py tests/test_voronoi_road_map_planner.py tests/test_drone_3d_trajectory_following.py tests/test_stanley_controller.py tests/test_LQR_planner.py tests/test_lqr_steer_control.py tests/test_rear_wheel_feedback.py tests/test_bug.py tests/test_lqr_speed_steer_control.py tests/test_raycasting_grid_map.py tests/test_potential_field_planning.py tests/test_quintic_polynomials_planner.py tests/test_move_to_pose.py tests/test_circle_fitting.py tests/test_pure_pursuit.py tests/test_two_joint_arm_to_point_control.py tests/test_particle_filter.py tests/test_kmeans_clustering.py tests/test_gaussian_grid_map.py tests/test_ekf_slam.py tests/test_unscented_kalman_filter.py tests/test_iterative_closest_point.py tests/test_extended_kalman_filter.py tests/test_probabilistic_road_map.py tests/test_eta3_spline_path.py tests/test_rocket_powered_landing.py tests/test_inverted_pendulum_mpc_control.py tests/test_cgmres_nmpc.py tests/test_model_predictive_speed_and_steer_control.py PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py PathTracking/lqr_steer_control/lqr_steer_control.py PathTracking/rear_wheel_feedback/rear_wheel_feedback.py PathTracking/pure_pursuit/pure_pursuit.py PathTracking/stanley_controller/stanley_controller.py PathTracking/move_to_pose/move_to_pose.py Localization/histogram_filter/histogram_filter.py Localization/unscented_kalman_filter/unscented_kalman_filter.py Localization/particle_filter/particle_filter.py Localization/extended_kalman_filter/extended_kalman_filter.py Mapping/grid_map_lib/grid_map_lib.py Mapping/kmeans_clustering/kmeans_clustering.py Mapping/raycasting_grid_map/raycasting_grid_map.py Mapping/circle_fitting/circle_fitting.py Mapping/gaussian_grid_map/gaussian_grid_map.py AerialNavigation/drone_3d_trajectory_following/drone_3d_trajectory_following.py AerialNavigation/drone_3d_trajectory_following/Quadrotor.py AerialNavigation/drone_3d_trajectory_following/TrajectoryGenerator.py ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py ArmNavigation/n_joint_arm_to_point_control/NLinkArm.py ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py Bipedal/bipedal_planner/bipedal_planner.py

No flags found

Use flags to group coverage reports by test type, project and/or folders.
Then setup custom commit statuses and notifications for each flag.

e.g., #unittest #integration

#production #enterprise

#frontend #backend

Learn more about Codecov Flags here.

Showing 2 of 2 files from the diff.

@@ -0,0 +1,18 @@
Loading
1 +
import PathPlanning.AStar.a_star_variants as astar
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 +
        astar.show_animation = False
12 +
        astar.use_jump_point = True
13 +
        astar.main()
14 +
15 +
16 +
if __name__ == '__main__':  # pragma: no cover
17 +
    test = Test()
18 +
    test.test()

@@ -0,0 +1,460 @@
Loading
1 +
"""
2 +
astar variants
3 +
author: Sarim Mehdi(muhammadsarim.mehdi@studio.unibo.it)
4 +
Source: http://theory.stanford.edu/~amitp/GameProgramming/Variations.html
5 +
"""
6 +
7 +
import numpy as np
8 +
import matplotlib.pyplot as plt
9 +
10 +
show_animation = True
11 +
use_beam_search = False
12 +
use_iterative_deepening = False
13 +
use_dynamic_weighting = False
14 +
use_theta_star = False
15 +
use_jump_point = True
16 +
17 +
beam_capacity = 30
18 +
max_theta = 25
19 +
only_corners = False
20 +
w, epsilon, upper_bound_depth = 1, 4, 500
21 +
22 +
23 +
def draw_horizontal_line(start_x, start_y, length, o_x, o_y, o_dict):
24 +
    for i in range(start_x, start_x + length):
25 +
        for j in range(start_y, start_y + 2):
26 +
            o_x.append(i)
27 +
            o_y.append(j)
28 +
            o_dict[(i, j)] = True
29 +
30 +
31 +
def draw_vertical_line(start_x, start_y, length, o_x, o_y, o_dict):
32 +
    for i in range(start_x, start_x + 2):
33 +
        for j in range(start_y, start_y + length):
34 +
            o_x.append(i)
35 +
            o_y.append(j)
36 +
            o_dict[(i, j)] = True
37 +
38 +
39 +
def in_line_of_sight(obs_grid, x1, y1, x2, y2):
40 +
    t = 0
41 +
    while t <= 0.5:
42 +
        xt = (1 - t) * x1 + t * x2
43 +
        yt = (1 - t) * y1 + t * y2
44 +
        if obs_grid[(int(xt), int(yt))]:
45 +
            return False, None
46 +
        xt = (1 - t) * x2 + t * x1
47 +
        yt = (1 - t) * y2 + t * y1
48 +
        if obs_grid[(int(xt), int(yt))]:
49 +
            return False, None
50 +
        t += 0.001
51 +
    dist = np.linalg.norm(np.array([x1, y1] - np.array([x2, y2])))
52 +
    return True, dist
53 +
54 +
55 +
def find_keypoints(o_dict):
56 +
    offsets1 = [(1, 0), (0, 1), (-1, 0), (1, 0)]
57 +
    offsets2 = [(1, 1), (-1, 1), (-1, -1), (1, -1)]
58 +
    offsets3 = [(0, 1), (-1, 0), (0, -1), (0, -1)]
59 +
    c_list = []
60 +
    for grid_point, obs_status in o_dict.items():
61 +
        if obs_status:
62 +
            continue
63 +
        empty_space = True
64 +
        x, y = grid_point
65 +
        for i in [-1, 0, 1]:
66 +
            for j in [-1, 0, 1]:
67 +
                if (x + i, y + j) not in o_dict.keys():
68 +
                    continue
69 +
                if o_dict[(x + i, y + j)]:
70 +
                    empty_space = False
71 +
                    break
72 +
        if empty_space:
73 +
            continue
74 +
        for offset1, offset2, offset3 in zip(offsets1, offsets2, offsets3):
75 +
            i1, j1 = offset1
76 +
            i2, j2 = offset2
77 +
            i3, j3 = offset3
78 +
            if ((x + i1, y + j1) not in o_dict.keys()) or \
79 +
                    ((x + i2, y + j2) not in o_dict.keys()) or \
80 +
                    ((x + i3, y + j3) not in o_dict.keys()):
81 +
                continue
82 +
            obs_count = 0
83 +
            if o_dict[(x + i1, y + j1)]:
84 +
                obs_count += 1
85 +
            if o_dict[(x + i2, y + j2)]:
86 +
                obs_count += 1
87 +
            if o_dict[(x + i3, y + j3)]:
88 +
                obs_count += 1
89 +
            if obs_count == 3 or obs_count == 1:
90 +
                c_list.append((x, y))
91 +
                plt.plot(x, y, ".y")
92 +
                break
93 +
    if only_corners:
94 +
        return c_list
95 +
96 +
    e_list = []
97 +
    for corner in c_list:
98 +
        x1, y1 = corner
99 +
        for other_corner in c_list:
100 +
            x2, y2 = other_corner
101 +
            if x1 == x2 and y1 == y2:
102 +
                continue
103 +
            reachable, _ = in_line_of_sight(o_dict, x1, y1, x2, y2)
104 +
            if not reachable:
105 +
                continue
106 +
            x_m, y_m = int((x1 + x2) / 2), int((y1 + y2) / 2)
107 +
            e_list.append((x_m, y_m))
108 +
            plt.plot(x_m, y_m, ".y")
109 +
    return c_list + e_list
110 +
111 +
112 +
class Search_Algo:
113 +
    def __init__(self, obs_grid, goal_x, goal_y, start_x, start_y,
114 +
                 limit_x, limit_y, corner_list=None):
115 +
        self.start_pt = [start_x, start_y]
116 +
        self.goal_pt = [goal_x, goal_y]
117 +
        self.obs_grid = obs_grid
118 +
        g_cost, h_cost = 0, self.get_hval(start_x, start_y, goal_x, goal_y)
119 +
        f_cost = g_cost + h_cost
120 +
        self.all_nodes, self.open_set = {}, []
121 +
122 +
        if use_jump_point:
123 +
            for corner in corner_list:
124 +
                i, j = corner
125 +
                h_c = self.get_hval(i, j, goal_x, goal_y)
126 +
                self.all_nodes[(i, j)] = {'pos': [i, j], 'pred': None,
127 +
                                          'gcost': np.inf, 'hcost': h_c,
128 +
                                          'fcost': np.inf,
129 +
                                          'open': True, 'in_open_list': False}
130 +
            self.all_nodes[tuple(self.goal_pt)] = \
131 +
                {'pos': self.goal_pt, 'pred': None,
132 +
                 'gcost': np.inf, 'hcost': 0, 'fcost': np.inf,
133 +
                 'open': True, 'in_open_list': True}
134 +
        else:
135 +
            for i in range(limit_x):
136 +
                for j in range(limit_y):
137 +
                    h_c = self.get_hval(i, j, goal_x, goal_y)
138 +
                    self.all_nodes[(i, j)] = {'pos': [i, j], 'pred': None,
139 +
                                              'gcost': np.inf, 'hcost': h_c,
140 +
                                              'fcost': np.inf,
141 +
                                              'open': True,
142 +
                                              'in_open_list': False}
143 +
        self.all_nodes[tuple(self.start_pt)] = \
144 +
            {'pos': self.start_pt, 'pred': None,
145 +
             'gcost': g_cost, 'hcost': h_cost, 'fcost': f_cost,
146 +
             'open': True, 'in_open_list': True}
147 +
        self.open_set.append(self.all_nodes[tuple(self.start_pt)])
148 +
149 +
    def get_hval(self, x1, y1, x2, y2):
150 +
        x, y = x1, y1
151 +
        val = 0
152 +
        while x != x2 or y != y2:
153 +
            if x != x2 and y != y2:
154 +
                val += 14
155 +
            else:
156 +
                val += 10
157 +
            x, y = x + np.sign(x2 - x), y + np.sign(y2 - y)
158 +
        return val
159 +
160 +
    def get_farthest_point(self, x, y, i, j):
161 +
        i_temp, j_temp = i, j
162 +
        counter = 1
163 +
        got_goal = False
164 +
        while not self.obs_grid[(x + i_temp, y + j_temp)] and \
165 +
                counter < max_theta:
166 +
            i_temp += i
167 +
            j_temp += j
168 +
            counter += 1
169 +
            if [x + i_temp, y + j_temp] == self.goal_pt:
170 +
                got_goal = True
171 +
                break
172 +
            if (x + i_temp, y + j_temp) not in self.obs_grid.keys():
173 +
                break
174 +
        return i_temp - 2*i, j_temp - 2*j, counter, got_goal
175 +
176 +
    def jump_point(self):
177 +
        '''Jump point: Instead of exploring all empty spaces of the
178 +
        map, just explore the corners.'''
179 +
        plt.title('Jump Point')
180 +
181 +
        goal_found = False
182 +
        while len(self.open_set) > 0:
183 +
            self.open_set = sorted(self.open_set, key=lambda x: x['fcost'])
184 +
            lowest_f = self.open_set[0]['fcost']
185 +
            lowest_h = self.open_set[0]['hcost']
186 +
            lowest_g = self.open_set[0]['gcost']
187 +
            p = 0
188 +
            for p_n in self.open_set[1:]:
189 +
                if p_n['fcost'] == lowest_f and \
190 +
                        p_n['gcost'] < lowest_g:
191 +
                    lowest_g = p_n['gcost']
192 +
                    p += 1
193 +
                elif p_n['fcost'] == lowest_f and \
194 +
                        p_n['gcost'] == lowest_g and \
195 +
                        p_n['hcost'] < lowest_h:
196 +
                    lowest_h = p_n['hcost']
197 +
                    p += 1
198 +
                else:
199 +
                    break
200 +
            current_node = self.all_nodes[tuple(self.open_set[p]['pos'])]
201 +
            x1, y1 = current_node['pos']
202 +
203 +
            for cand_pt, cand_node in self.all_nodes.items():
204 +
                x2, y2 = cand_pt
205 +
                if x1 == x2 and y1 == y2:
206 +
                    continue
207 +
                if np.linalg.norm(np.array([x1, y1] -
208 +
                                           np.array([x2, y2]))) > 100:
209 +
                    continue
210 +
                reachable, offset = in_line_of_sight(self.obs_grid, x1,
211 +
                                                     y1, x2, y2)
212 +
                if not reachable:
213 +
                    continue
214 +
215 +
                if list(cand_pt) == self.goal_pt:
216 +
                    current_node['open'] = False
217 +
                    self.all_nodes[tuple(cand_pt)]['pred'] = \
218 +
                        current_node['pos']
219 +
                    goal_found = True
220 +
                    break
221 +
222 +
                g_cost = offset + current_node['gcost']
223 +
                h_cost = self.all_nodes[cand_pt]['hcost']
224 +
                f_cost = g_cost + h_cost
225 +
                cand_pt = tuple(cand_pt)
226 +
                if f_cost < self.all_nodes[cand_pt]['fcost']:
227 +
                    self.all_nodes[cand_pt]['pred'] = current_node['pos']
228 +
                    self.all_nodes[cand_pt]['gcost'] = g_cost
229 +
                    self.all_nodes[cand_pt]['fcost'] = f_cost
230 +
                    if not self.all_nodes[cand_pt]['in_open_list']:
231 +
                        self.open_set.append(self.all_nodes[cand_pt])
232 +
                        self.all_nodes[cand_pt]['in_open_list'] = True
233 +
                    plt.plot(cand_pt[0], cand_pt[1], "r*")
234 +
235 +
                if goal_found:
236 +
                    break
237 +
            plt.pause(0.001)
238 +
            if goal_found:
239 +
                current_node = self.all_nodes[tuple(self.goal_pt)]
240 +
            while goal_found:
241 +
                if current_node['pred'] is None:
242 +
                    break
243 +
                x = [current_node['pos'][0], current_node['pred'][0]]
244 +
                y = [current_node['pos'][1], current_node['pred'][1]]
245 +
                plt.plot(x, y, "b")
246 +
                current_node = self.all_nodes[tuple(current_node['pred'])]
247 +
                plt.pause(0.001)
248 +
            if goal_found:
249 +
                break
250 +
251 +
            current_node['open'] = False
252 +
            current_node['in_open_list'] = False
253 +
            plt.plot(current_node['pos'][0], current_node['pos'][1], "g*")
254 +
            del self.open_set[p]
255 +
            current_node['fcost'], current_node['hcost'] = np.inf, np.inf
256 +
        if show_animation:
257 +
            plt.show()
258 +
259 +
    def astar(self):
260 +
        '''Beam search: Maintain an open list of just 30 nodes.
261 +
        If more than 30 nodes, then get rid of ndoes with high
262 +
        f values.
263 +
        Iterative deepening: At every iteration, get a cut-off
264 +
        value for the f cost. This cut-off is minimum of the f
265 +
        value of all nodes whose f value is higher than the
266 +
        current cut-off value. Nodes with f value higher than
267 +
        the current cut off value are not put in the open set.
268 +
        Dynamic weighting: Multiply heuristic with the following:
269 +
        (1 + epsilon - (epsilon*d)/N) where d is the current
270 +
        iteration of loop and N is upper bound on number of
271 +
        iterations.
272 +
        Theta star: Same as A star but you don't need to move
273 +
        one neighbor at a time. In fact, you can look for the
274 +
        next node as far out as you can as long as there is a
275 +
        clear line of sight from your current node to that node.'''
276 +
        if use_beam_search:
277 +
            plt.title('A* with beam search')
278 +
        elif use_iterative_deepening:
279 +
            plt.title('A* with iterative deepening')
280 +
        elif use_dynamic_weighting:
281 +
            plt.title('A* with dynamic weighting')
282 +
        elif use_theta_star:
283 +
            plt.title('Theta*')
284 +
        else:
285 +
            plt.title('A*')
286 +
287 +
        goal_found = False
288 +
        curr_f_thresh = np.inf
289 +
        depth = 0
290 +
        no_valid_f = False
291 +
        while len(self.open_set) > 0:
292 +
            self.open_set = sorted(self.open_set, key=lambda x: x['fcost'])
293 +
            lowest_f = self.open_set[0]['fcost']
294 +
            lowest_h = self.open_set[0]['hcost']
295 +
            lowest_g = self.open_set[0]['gcost']
296 +
            p = 0
297 +
            for p_n in self.open_set[1:]:
298 +
                if p_n['fcost'] == lowest_f and \
299 +
                        p_n['gcost'] < lowest_g:
300 +
                    lowest_g = p_n['gcost']
301 +
                    p += 1
302 +
                elif p_n['fcost'] == lowest_f and \
303 +
                        p_n['gcost'] == lowest_g and \
304 +
                        p_n['hcost'] < lowest_h:
305 +
                    lowest_h = p_n['hcost']
306 +
                    p += 1
307 +
                else:
308 +
                    break
309 +
            current_node = self.all_nodes[tuple(self.open_set[p]['pos'])]
310 +
311 +
            while len(self.open_set) > beam_capacity and use_beam_search:
312 +
                del self.open_set[-1]
313 +
314 +
            f_cost_list = []
315 +
            if use_dynamic_weighting:
316 +
                w = (1 + epsilon - epsilon*depth/upper_bound_depth)
317 +
            for i in range(-1, 2):
318 +
                for j in range(-1, 2):
319 +
                    x, y = current_node['pos']
320 +
                    if (i == 0 and j == 0) or \
321 +
                            ((x + i, y + j) not in self.obs_grid.keys()):
322 +
                        continue
323 +
                    if (i, j) in [(1, 0), (0, 1), (-1, 0), (0, -1)]:
324 +
                        offset = 10
325 +
                    else:
326 +
                        offset = 14
327 +
                    if use_theta_star:
328 +
                        new_i, new_j, counter, goal_found = \
329 +
                            self.get_farthest_point(x, y, i, j)
330 +
                        offset = offset * counter
331 +
                        cand_pt = [current_node['pos'][0] + new_i,
332 +
                                   current_node['pos'][1] + new_j]
333 +
                    else:
334 +
                        cand_pt = [current_node['pos'][0] + i,
335 +
                                   current_node['pos'][1] + j]
336 +
337 +
                    if use_theta_star and goal_found:
338 +
                        current_node['open'] = False
339 +
                        cand_pt = self.goal_pt
340 +
                        self.all_nodes[tuple(cand_pt)]['pred'] = \
341 +
                            current_node['pos']
342 +
                        break
343 +
344 +
                    if cand_pt == self.goal_pt:
345 +
                        current_node['open'] = False
346 +
                        self.all_nodes[tuple(cand_pt)]['pred'] = \
347 +
                            current_node['pos']
348 +
                        goal_found = True
349 +
                        break
350 +
351 +
                    cand_pt = tuple(cand_pt)
352 +
                    if not self.obs_grid[tuple(cand_pt)] and \
353 +
                            self.all_nodes[cand_pt]['open']:
354 +
                        g_cost = offset + current_node['gcost']
355 +
                        h_cost = self.all_nodes[cand_pt]['hcost']
356 +
                        if use_dynamic_weighting:
357 +
                            h_cost = h_cost * w
358 +
                        f_cost = g_cost + h_cost
359 +
                        if f_cost < self.all_nodes[cand_pt]['fcost'] and \
360 +
                                f_cost <= curr_f_thresh:
361 +
                            f_cost_list.append(f_cost)
362 +
                            self.all_nodes[cand_pt]['pred'] = \
363 +
                                current_node['pos']
364 +
                            self.all_nodes[cand_pt]['gcost'] = g_cost
365 +
                            self.all_nodes[cand_pt]['fcost'] = f_cost
366 +
                            if not self.all_nodes[cand_pt]['in_open_list']:
367 +
                                self.open_set.append(self.all_nodes[cand_pt])
368 +
                                self.all_nodes[cand_pt]['in_open_list'] = True
369 +
                            plt.plot(cand_pt[0], cand_pt[1], "r*")
370 +
                        if curr_f_thresh < f_cost < \
371 +
                                self.all_nodes[cand_pt]['fcost']:
372 +
                            no_valid_f = True
373 +
                if goal_found:
374 +
                    break
375 +
            plt.pause(0.001)
376 +
            if goal_found:
377 +
                current_node = self.all_nodes[tuple(self.goal_pt)]
378 +
            while goal_found:
379 +
                if current_node['pred'] is None:
380 +
                    break
381 +
                if use_theta_star or use_jump_point:
382 +
                    x, y = [current_node['pos'][0], current_node['pred'][0]], \
383 +
                             [current_node['pos'][1], current_node['pred'][1]]
384 +
                    plt.plot(x, y, "b")
385 +
                else:
386 +
                    plt.plot(current_node['pred'][0],
387 +
                             current_node['pred'][1], "b*")
388 +
                current_node = self.all_nodes[tuple(current_node['pred'])]
389 +
                plt.pause(0.001)
390 +
            if goal_found:
391 +
                break
392 +
393 +
            if use_iterative_deepening and f_cost_list:
394 +
                curr_f_thresh = min(f_cost_list)
395 +
            if use_iterative_deepening and not f_cost_list:
396 +
                curr_f_thresh = np.inf
397 +
            if use_iterative_deepening and not f_cost_list and no_valid_f:
398 +
                current_node['fcost'], current_node['hcost'] = np.inf, np.inf
399 +
                continue
400 +
401 +
            current_node['open'] = False
402 +
            current_node['in_open_list'] = False
403 +
            plt.plot(current_node['pos'][0], current_node['pos'][1], "g*")
404 +
            del self.open_set[p]
405 +
            current_node['fcost'], current_node['hcost'] = np.inf, np.inf
406 +
            depth += 1
407 +
        if show_animation:
408 +
            plt.show()
409 +
410 +
411 +
def main():
412 +
    # set obstacle positions
413 +
    obs_dict = {}
414 +
    for i in range(101):
415 +
        for j in range(101):
416 +
            obs_dict[(i, j)] = False
417 +
    o_x, o_y = [], []
418 +
419 +
    s_x = 10.0
420 +
    s_y = 10.0
421 +
    g_x = 90.0
422 +
    g_y = 90.0
423 +
424 +
    # draw outer border of maze
425 +
    draw_vertical_line(0, 0, 100, o_x, o_y, obs_dict)
426 +
    draw_vertical_line(98, 0, 100, o_x, o_y, obs_dict)
427 +
    draw_horizontal_line(0, 0, 100, o_x, o_y, obs_dict)
428 +
    draw_horizontal_line(0, 98, 100, o_x, o_y, obs_dict)
429 +
430 +
    # draw inner walls
431 +
    all_x = [25, 25, 50, 50, 70, 85, 12, 35, 35]
432 +
    all_y = [25, 30, 0, 75, 50, 0, 35, 70, 0]
433 +
    all_len = [50, 40, 25, 25, 25, 40, 30, 20, 30]
434 +
    for x, y, l in zip(all_x, all_y, all_len):
435 +
        draw_vertical_line(x, y, l, o_x, o_y, obs_dict)
436 +
437 +
    all_x[:], all_y[:], all_len[:] = [], [], []
438 +
    all_x = [50, 25, 0, 25, 40, 80, 65, 90]
439 +
    all_y = [25, 50, 80, 50, 60, 60, 10, 70]
440 +
    all_len = [25, 50, 10, 10, 20, 20, 10, 10]
441 +
    for x, y, l in zip(all_x, all_y, all_len):
442 +
        draw_horizontal_line(x, y, l, o_x, o_y, obs_dict)
443 +
444 +
    plt.plot(o_x, o_y, ".k")
445 +
    plt.plot(s_x, s_y, "og")
446 +
    plt.plot(g_x, g_y, "xb")
447 +
    plt.grid(True)
448 +
449 +
    if use_jump_point:
450 +
        keypoint_list = find_keypoints(obs_dict)
451 +
        search_obj = Search_Algo(obs_dict, g_x, g_y, s_x, s_y, 101, 101,
452 +
                                 keypoint_list)
453 +
        search_obj.jump_point()
454 +
    else:
455 +
        search_obj = Search_Algo(obs_dict, g_x, g_y, s_x, s_y, 101, 101)
456 +
        search_obj.astar()
457 +
458 +
459 +
if __name__ == '__main__':
460 +
    main()

Learn more Showing 2 files with coverage changes found.

New file PathPlanning/AStar/a_star_variants.py
New
Loading file...
New file tests/test_a_star_variants.py
New
Loading file...
Files Coverage
AerialNavigation/drone_3d_trajectory_following 92.35%
ArmNavigation 94.93%
Localization 89.79%
Mapping 90.46%
PathPlanning -1.54% 86.32%
PathTracking 91.10%
SLAM 97.50%
tests 0.09% 93.00%
Bipedal/bipedal_planner/bipedal_planner.py 85.53%
Project Totals (129 files) 88.78%
Loading