github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 06:28:00 -05:00
Code Issues Packages Projects Releases Wiki Activity

fix doc artifact link in CI (#660)

Browse Source
This commit is contained in:
Atsushi Sakai
2022-04-11 23:06:13 +09:00
committed by GitHub
parent 38261ec67e
commit af2061a1e7
2 changed files with 10 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
PathPlanning/HybridAStar/car.py
View File

@@ -12,14 +12,14 @@ import matplotlib.pyplot as plt
import numpy as np
from scipy.spatial.transform import Rotation as Rot
WB = 3. # rear to front wheel
W = 2. # width of car
WB = 3.0 # rear to front wheel
W = 2.0 # width of car
LF = 3.3 # distance from rear to vehicle front end
LB = 1.0 # distance from rear to vehicle back end
MAX_STEER = 0.6 # [rad] maximum steering angle
W_BUBBLE_DIST = (LF - LB) / 2.0
W_BUBBLE_R = sqrt(((LF + LB) / 2.0) ** 2 + 1)
BUBBLE_DIST = (LF - LB) / 2.0 # distance from rear to center of vehicle.
BUBBLE_R = np.hypot((LF + LB) / 2.0, W / 2.0) # bubble radius
# vehicle rectangle vertices
VRX = [LF, LF, -LB, -LB, LF]
@@ -28,10 +28,10 @@ VRY = [W / 2, -W / 2, -W / 2, W / 2, W / 2]
def check_car_collision(x_list, y_list, yaw_list, ox, oy, kd_tree):
for i_x, i_y, i_yaw in zip(x_list, y_list, yaw_list):
cx = i_x + W_BUBBLE_DIST * cos(i_yaw)
cy = i_y + W_BUBBLE_DIST * sin(i_yaw)
cx = i_x + BUBBLE_DIST * cos(i_yaw)
cy = i_y + BUBBLE_DIST * sin(i_yaw)
ids = kd_tree.query_ball_point([cx, cy], W_BUBBLE_R)
ids = kd_tree.query_ball_point([cx, cy], BUBBLE_R)
if not ids:
continue

6
PathPlanning/HybridAStar/hybrid_a_star.py
View File

@@ -20,7 +20,8 @@ sys.path.append(os.path.dirname(os.path.abspath(__file__))
try:
from dynamic_programming_heuristic import calc_distance_heuristic
import reeds_shepp_path_planning as rs
from car import move, check_car_collision, MAX_STEER, WB, plot_car
from car import move, check_car_collision, MAX_STEER, WB, plot_car,\
BUBBLE_R
except Exception:
raise
@@ -28,7 +29,6 @@ XY_GRID_RESOLUTION = 2.0 # [m]
YAW_GRID_RESOLUTION = np.deg2rad(15.0) # [rad]
MOTION_RESOLUTION = 0.1 # [m] path interpolate resolution
N_STEER = 20 # number of steer command
VR = 1.0 # robot radius
SB_COST = 100.0 # switch back penalty cost
BACK_COST = 5.0 # backward penalty cost
@@ -276,7 +276,7 @@ def hybrid_a_star_planning(start, goal, ox, oy, xy_resolution, yaw_resolution):
h_dp = calc_distance_heuristic(
goal_node.x_list[-1], goal_node.y_list[-1],
ox, oy, xy_resolution, VR)
ox, oy, xy_resolution, BUBBLE_R)
pq = []
openList[calc_index(start_node, config)] = start_node