From d46fecc2a46ee3bad6c1425beefb036558a7f992 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Thu, 5 Apr 2018 16:36:36 +0900 Subject: [PATCH] keep coding --- PathPlanning/HybridAStar/hybrid_a_star.py | 31 ++++++++++++++++++++--- 1 file changed, 27 insertions(+), 4 deletions(-) diff --git a/PathPlanning/HybridAStar/hybrid_a_star.py b/PathPlanning/HybridAStar/hybrid_a_star.py index f4fe5460..13189cf3 100644 --- a/PathPlanning/HybridAStar/hybrid_a_star.py +++ b/PathPlanning/HybridAStar/hybrid_a_star.py @@ -14,16 +14,15 @@ import numpy as np import scipy.spatial import matplotlib.pyplot as plt import reeds_shepp_path_planning as rs +import heapq EXTEND_AREA = 5.0 # [m] +H_COST = 1.0 show_animation = True class Node: - """ - Node - """ def __init__(self, xind, yind, yawind, direction, x, y, yaw, directions, steer, cost, pind): # store kd-tree @@ -117,7 +116,7 @@ def hybrid_a_star_planning(start, goal, ox, oy, xyreso, yawreso): start[2], goal[2] = rs.pi_2_pi(start[2]), rs.pi_2_pi(goal[2]) tox, toy = ox[:], oy[:] - obkdtree = KDTree(np.vstack((tox, toy)).T) + # obkdtree = KDTree(np.vstack((tox, toy)).T) c = Config(tox, toy, xyreso, yawreso) @@ -126,11 +125,35 @@ def hybrid_a_star_planning(start, goal, ox, oy, xyreso, yawreso): ngoal = Node(int(goal[0] / xyreso), int(goal[1] / xyreso), int(goal[2] / yawreso), True, [goal[0]], [goal[1]], [goal[2]], [True], 0.0, 0.0, -1) + openList, closedList = {}, {} + h = [] + # goalqueue = queue.PriorityQueue() + pq = [] + openList[calc_index(nstart, c)] = nstart + heapq.heappush(pq, (calc_index(nstart, c), calc_cost(nstart, h, ngoal, c))) + rx, ry, ryaw = [], [], [] return rx, ry, ryaw +def calc_cost(n, h, ngoal, c): + + hcost = 1.0 + + return (n.cost + H_COST * hcost) + + +def calc_index(node, c): + ind = (node.yawind - c.minyaw) * c.xw * c.yw + \ + (node.yind - c.miny) * c.xw + (node.xind - c.minx) + + if ind <= 0: + print("Error(calc_index):", ind) + + return ind + + def main(): print("Start rrt start planning") # ====Search Path with RRT====