<reponame>Pythocrates/PythonRobotics
import conftest
import numpy as np
from PathPlanning.DubinsPath import dubins_path_planning

np.random.seed(12345)


def check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw,
                         end_x, end_y, end_yaw):
    assert (abs(px[0] - start_x) <= 0.01)
    assert (abs(py[0] - start_y) <= 0.01)
    assert (abs(pyaw[0] - start_yaw) <= 0.01)
    assert (abs(px[-1] - end_x) <= 0.01)
    assert (abs(py[-1] - end_y) <= 0.01)
    assert (abs(pyaw[-1] - end_yaw) <= 0.01)


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, clen = 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)


def test_2():
    dubins_path_planning.show_animation = False
    dubins_path_planning.main()


def test_3():
    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)

        check_edge_condition(px, py, pyaw,
                             start_x, start_y, start_yaw,
                             end_x, end_y, end_yaw)


if __name__ == '__main__':
    conftest.run_this_test(__file__)

import conftest
import numpy as np
from PathPlanning.DubinsPath import dubins_path_planning

np.random.seed(12345)


def check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw,
                         end_x, end_y, end_yaw):
    assert (abs(px[0] - start_x) <= 0.01)
    assert (abs(py[0] - start_y) <= 0.01)
    assert (abs(pyaw[0] - start_yaw) <= 0.01)
    assert (abs(px[-1] - end_x) <= 0.01)
    assert (abs(py[-1] - end_y) <= 0.01)
    assert (abs(pyaw[-1] - end_yaw) <= 0.01)


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, clen = 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)


def test_2():
    dubins_path_planning.show_animation = False
    dubins_path_planning.main()


def test_3():
    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)

        check_edge_condition(px, py, pyaw,
                             start_x, start_y, start_yaw,
                             end_x, end_y, end_yaw)


if __name__ == '__main__':
    conftest.run_this_test(__file__)
# [m] # [m] # [rad] # [m] # [m] # [rad] # [m] # [m] # [rad] # [m] # [m] # [rad]