diff --git a/PathPlanning/HybridAStar/hybrid_a_star.py b/PathPlanning/HybridAStar/hybrid_a_star.py index 03db0dc167..1962efe295 100644 --- a/PathPlanning/HybridAStar/hybrid_a_star.py +++ b/PathPlanning/HybridAStar/hybrid_a_star.py @@ -105,12 +105,13 @@ def calc_next_node(current, steer, direction, config, ox, oy, kd_tree): x, y, yaw = current.x_list[-1], current.y_list[-1], current.yaw_list[-1] arc_l = XY_GRID_RESOLUTION * 1.5 - x_list, y_list, yaw_list = [], [], [] + x_list, y_list, yaw_list, direction_list = [], [], [], [] for _ in np.arange(0, arc_l, MOTION_RESOLUTION): x, y, yaw = move(x, y, yaw, MOTION_RESOLUTION * direction, steer) x_list.append(x) y_list.append(y) yaw_list.append(yaw) + direction_list.append(direction == 1) if not check_car_collision(x_list, y_list, yaw_list, ox, oy, kd_tree): return None @@ -134,7 +135,7 @@ def calc_next_node(current, steer, direction, config, ox, oy, kd_tree): cost = current.cost + added_cost + arc_l node = Node(x_ind, y_ind, yaw_ind, d, x_list, - y_list, yaw_list, [d], + y_list, yaw_list, direction_list, parent_index=calc_index(current, config), cost=cost, steer=steer) diff --git a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py index 8e134ff38b..618d1d99ba 100644 --- a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py +++ b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py @@ -6,6 +6,10 @@ co-author Videh Patel(@videh25) : Added the missing RS paths """ +import sys +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) + import math import matplotlib.pyplot as plt