From af2061a1e784a6779bdc08976a43157ddae17884 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Mon, 11 Apr 2022 23:06:13 +0900
Subject: [PATCH] fix doc artifact link in CI (#660)

---
 PathPlanning/HybridAStar/car.py           | 14 +++++++-------
 PathPlanning/HybridAStar/hybrid_a_star.py |  6 +++---
 2 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/PathPlanning/HybridAStar/car.py b/PathPlanning/HybridAStar/car.py
index cc7529b9..b378447f 100644
--- a/PathPlanning/HybridAStar/car.py
+++ b/PathPlanning/HybridAStar/car.py
@@ -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
diff --git a/PathPlanning/HybridAStar/hybrid_a_star.py b/PathPlanning/HybridAStar/hybrid_a_star.py
index 9679e397..4ef0c1dd 100644
--- a/PathPlanning/HybridAStar/hybrid_a_star.py
+++ b/PathPlanning/HybridAStar/hybrid_a_star.py
@@ -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