def test1(self):
start_x = 1.0 # [m]
start_y = 1.0 # [m]
start_yaw = np.deg2rad(45.0) # [rad]
end_x = -3.0 # [m]
end_y = -3.0 # [m]
end_yaw = np.deg2rad(-45.0) # [rad]
curvature = 1.0
px, py, pyaw, mode, clen = dubins_path_planning.dubins_path_planning(
start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature)
self.check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw,
end_x, end_y, end_yaw)
def test1(self):
start_x = 1.0 # [m]
start_y = 1.0 # [m]
start_yaw = np.deg2rad(45.0) # [rad]
end_x = -3.0 # [m]
end_y = -3.0 # [m]
end_yaw = np.deg2rad(-45.0) # [rad]
curvature = 1.0
px, py, pyaw, mode, clen = dubins_path_planning.dubins_path_planning(
start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature)
assert (abs(px[-1] - end_x) <= 0.1)
assert (abs(py[-1] - end_y) <= 0.1)
assert (abs(pyaw[-1] - end_yaw) <= 0.1)
def test_1():
start_x = 1.0 # [m]
start_y = 1.0 # [m]
start_yaw = np.deg2rad(45.0) # [rad]
end_x = -3.0 # [m]
end_y = -3.0 # [m]
end_yaw = np.deg2rad(-45.0) # [rad]
curvature = 1.0
px, py, pyaw, mode, lengths = dubins_path_planning.dubins_path_planning(
start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature)
check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, end_x,
end_y, end_yaw)
check_path_length(px, py, lengths)
def test1(self):
start_x = 1.0 # [m]
start_y = 1.0 # [m]
start_yaw = np.deg2rad(45.0) # [rad]
end_x = -3.0 # [m]
end_y = -3.0 # [m]
end_yaw = np.deg2rad(-45.0) # [rad]
curvature = 1.0
px, py, pyaw, mode, clen = dubins_path_planning.dubins_path_planning(
start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature)
assert(abs(px[-1] - end_x) <= 0.1)
assert(abs(py[-1] - end_y) <= 0.1)
assert(abs(pyaw[-1] - end_yaw) <= 0.1)
def test3(self):
N_TEST = 10
for i in range(N_TEST):
start_x = (np.random.rand() - 0.5) * 10.0 # [m]
start_y = (np.random.rand() - 0.5) * 10.0 # [m]
start_yaw = np.deg2rad((np.random.rand() - 0.5) * 180.0) # [rad]
end_x = (np.random.rand() - 0.5) * 10.0 # [m]
end_y = (np.random.rand() - 0.5) * 10.0 # [m]
end_yaw = np.deg2rad((np.random.rand() - 0.5) * 180.0) # [rad]
curvature = 1.0 / (np.random.rand() * 5.0)
px, py, pyaw, mode, clen = dubins_path_planning.dubins_path_planning(
start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature)
self.check_edge_condition(px, py, pyaw, start_x, start_y,
start_yaw, end_x, end_y, end_yaw)
