Skip to content

Commit

Permalink
Fix: Hybrid A* direction is incorrect (AtsushiSakai#1086)
Browse files Browse the repository at this point in the history
* fix to preseve direction history

* add path to Reeds Shepp and its users e.g. hybrid A*
  • Loading branch information
parmaski authored Dec 21, 2024
1 parent 7eeb9d2 commit b5988db
Show file tree
Hide file tree
Showing 2 changed files with 7 additions and 2 deletions.
5 changes: 3 additions & 2 deletions PathPlanning/HybridAStar/hybrid_a_star.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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)

Expand Down
4 changes: 4 additions & 0 deletions PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down

0 comments on commit b5988db

Please sign in to comment.