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

Test code clean up (#456)

Browse Source 
* Test code clean up

* Test code clean up

* Test code clean up

* Test code clean up

* Test code clean up

* Test code clean up

* Test code clean up

* Test code clean up

* Test code clean up

* Test code clean up

* Test code clean up

* Test code clean up

* Test code clean up

* Test code clean up

* Test code clean up

* Test code clean up

* Test code clean up

* Test code clean up

* Test code clean up

* Test code clean up

* Test code clean up

* Test code clean up

* Test code clean up

* Test code clean up

* Test code clean up
This commit is contained in:
Atsushi Sakai
2021-01-12 22:13:46 +09:00
committed by GitHub
parent 8861977e8c
commit 67d7d5c610
88 changed files with 831 additions and 1291 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
ArmNavigation/n_joint_arm_3d/__init__py.py → AerialNavigation/__init__.py
View File

4
AerialNavigation/drone_3d_trajectory_following/__init__.py Normal file
View File

@@ -0,0 +1,4 @@
import os
import sys
sys.path.append(os.path.dirname(os.path.abspath(__file__)))

3
ArmNavigation/n_joint_arm_3d/__init__.py Normal file
View File

@@ -0,0 +1,3 @@
import sys
import os
sys.path.append(os.path.dirname(os.path.abspath(__file__)))

2
ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py
View File

@@ -6,7 +6,7 @@ Author: Daniel Ingram (daniel-s-ingram)
"""
import numpy as np
from NLinkArm import NLinkArm
from ArmNavigation.n_joint_arm_to_point_control.NLinkArm import NLinkArm
# Simulation parameters
Kp = 2

0
InvertedPendulumCart/inverted_pendulum_mpc_control.py → InvertedPendulumCart/InvertedPendulumMPCControl/inverted_pendulum_mpc_control.py
View File

0
tests/.gitkeep → InvertedPendulumCart/__init__.py
View File

2
LICENSE
View File

@@ -1,6 +1,6 @@
The MIT License (MIT)
Copyright (c) 2016 Atsushi Sakai
Copyright (c) 2016 - 2021 Atsushi Sakai
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal

0
Localization/__init__.py Normal file
View File

0
Mapping/__init__.py Normal file
View File

0
Mapping/grid_map_lib/__init__.py Normal file
View File

4
Mapping/rectangle_fitting/__init_.py Normal file
View File

@@ -0,0 +1,4 @@
import os
import sys
sys.path.append(os.path.dirname(os.path.abspath(__file__)))

3
Mapping/rectangle_fitting/rectangle_fitting.py
View File

@@ -18,7 +18,8 @@ import itertools
from enum import Enum
from scipy.spatial.transform import Rotation as Rot
from simulator import VehicleSimulator, LidarSimulator
from Mapping.rectangle_fitting.simulator \
import VehicleSimulator, LidarSimulator
show_animation = True

8
PathPlanning/ClosedLoopRRTStar/__init__.py
View File

@@ -0,0 +1,8 @@
import os
import sys
sys.path.append(os.path.dirname(os.path.abspath(__file__)))
sys.path.append(os.path.dirname(
os.path.abspath(__file__)) + "/../ReedsSheppPath/")
sys.path.append(os.path.dirname(
os.path.abspath(__file__)) + "/../RRTStarReedsShepp/")

24
PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py
View File

@@ -6,25 +6,13 @@ author: AtsushiSakai(@Atsushi_twi)
"""
import os
import sys
import matplotlib.pyplot as plt
import numpy as np
import reeds_shepp_path_planning
from rrt_star_reeds_shepp import RRTStarReedsShepp
import pure_pursuit
sys.path.append(os.path.dirname(
os.path.abspath(__file__)) + "/../ReedsSheppPath/")
sys.path.append(os.path.dirname(
os.path.abspath(__file__)) + "/../RRTStarReedsShepp/")
try:
import reeds_shepp_path_planning
import unicycle_model
from rrt_star_reeds_shepp import RRTStarReedsShepp
except ImportError:
raise
import unicycle_model
show_animation = True
@@ -77,7 +65,8 @@ class ClosedLoopRRTStar(RRTStarReedsShepp):
for ind in path_indexs:
path = self.generate_final_course(ind)
flag, x, y, yaw, v, t, a, d = self.check_tracking_path_is_feasible(path)
flag, x, y, yaw, v, t, a, d = self.check_tracking_path_is_feasible(
path)
if flag and best_time >= t[-1]:
print("feasible path is found")
@@ -186,7 +175,8 @@ def main(gx=6.0, gy=7.0, gyaw=np.deg2rad(90.0), max_iter=100):
obstacle_list,
[-2.0, 20.0],
max_iter=max_iter)
flag, x, y, yaw, v, t, a, d = closed_loop_rrt_star.planning(animation=show_animation)
flag, x, y, yaw, v, t, a, d = closed_loop_rrt_star.planning(
animation=show_animation)
if not flag:
print("cannot find feasible path")

7
PathPlanning/GridBasedSweepCPP/__init__.py Normal file
View File

@@ -0,0 +1,7 @@
import os
import sys
GRID_MAP_LIB = os.path.dirname(os.path.abspath(__file__)) + \
"/../../Mapping/"
sys.path.append(GRID_MAP_LIB)

9
PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py
View File

@@ -5,19 +5,12 @@ author: Atsushi Sakai
"""
import math
import os
import sys
from enum import IntEnum
import matplotlib.pyplot as plt
import numpy as np
from scipy.spatial.transform import Rotation as Rot
sys.path.append(os.path.relpath("../../Mapping/grid_map_lib/"))
try:
from grid_map_lib import GridMap
except ImportError:
raise
from Mapping.grid_map_lib.grid_map_lib import GridMap
do_animation = True

4
PathPlanning/HybridAStar/__init__.py
View File

@@ -0,0 +1,4 @@
import os
import sys
sys.path.append(os.path.dirname(os.path.abspath(__file__)))

4
PathPlanning/VoronoiRoadMap/__init__.py Normal file
View File

@@ -0,0 +1,4 @@
import os
import sys
sys.path.append(os.path.dirname(os.path.abspath(__file__)))

0
PathTracking/__init__.py Normal file
View File

0
__init__.py Normal file
View File

6
runtests.sh
View File

@@ -1,6 +1,8 @@
#!/usr/bin/env bash
echo "Run test suites! "
# tests: include unittest based tests
# === pytest based test runner ===
# -Werror: warning as error
# --durations=0: show ranking of test durations
pytest tests -Werror --durations=0
# -l (--showlocals); show local variables when test failed
pytest tests -l -Werror --durations=0

13
tests/conftest.py Normal file
View File

@@ -0,0 +1,13 @@
"""Path hack to make tests work."""
import sys
import os
import pytest
TEST_DIR = os.path.dirname(os.path.abspath(__file__))
sys.path.append(TEST_DIR) # to import this file from test code.
ROOT_DIR = os.path.dirname(TEST_DIR)
sys.path.append(ROOT_DIR)
def run_this_test(file):
pytest.main([os.path.abspath(file)])

18
tests/test_LQR_planner.py
View File

@@ -1,15 +1,11 @@
import sys
from unittest import TestCase
sys.path.append("./PathPlanning/LQRPlanner")
import conftest # Add root path to sys.path
from PathPlanning.LQRPlanner import LQRplanner as m
print(__file__)
def test_1():
m.SHOW_ANIMATION = False
m.main()
class Test(TestCase):
def test1(self):
m.SHOW_ANIMATION = False
m.main()
if __name__ == '__main__':
conftest.run_this_test(__file__)

23
tests/test_a_star.py
View File

@@ -1,20 +1,11 @@
from unittest import TestCase
import sys
import os
sys.path.append(os.path.dirname(__file__) + "/../")
try:
from PathPlanning.AStar import a_star as m
except:
raise
import conftest
from PathPlanning.AStar import a_star as m
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
def test_1():
m.show_animation = False
m.main()
if __name__ == '__main__': # pragma: no cover
test = Test()
test.test1()
if __name__ == '__main__':
conftest.run_this_test(__file__)

29
tests/test_a_star_searching_two_side.py
View File

@@ -1,27 +1,16 @@
from unittest import TestCase
import os
import sys
sys.path.append(os.path.dirname(__file__) + '/../')
try:
from PathPlanning.AStar import a_star_searching_from_two_side as m
except ImportError:
raise
import conftest
from PathPlanning.AStar import a_star_searching_from_two_side as m
class Test(TestCase):
def test1():
m.show_animation = False
m.main(800)
def test1(self):
m.show_animation = False
m.main(800)
def test2(self):
m.show_animation = False
m.main(5000) # increase obstacle number, block path
def test2():
m.show_animation = False
m.main(5000) # increase obstacle number, block path
if __name__ == '__main__':
test = Test()
test.test1()
test.test2()
conftest.run_this_test(__file__)

76
tests/test_a_star_variants.py
View File

@@ -1,50 +1,44 @@
import PathPlanning.AStar.a_star_variants as astar
from unittest import TestCase
import sys
import os
sys.path.append(os.path.dirname(__file__) + "/../")
import PathPlanning.AStar.a_star_variants as a_star
import conftest
class Test(TestCase):
def test_1():
# A* with beam search
a_star.show_animation = False
def test(self):
# A* with beam search
astar.show_animation = False
a_star.use_beam_search = True
a_star.main()
reset_all()
astar.use_beam_search = True
astar.main()
self.reset_all()
# A* with iterative deepening
a_star.use_iterative_deepening = True
a_star.main()
reset_all()
# A* with iterative deepening
astar.use_iterative_deepening = True
astar.main()
self.reset_all()
# A* with dynamic weighting
a_star.use_dynamic_weighting = True
a_star.main()
reset_all()
# A* with dynamic weighting
astar.use_dynamic_weighting = True
astar.main()
self.reset_all()
# theta*
a_star.use_theta_star = True
a_star.main()
reset_all()
# theta*
astar.use_theta_star = True
astar.main()
self.reset_all()
# A* with jump point
astar.use_jump_point = True
astar.main()
self.reset_all()
@staticmethod
def reset_all():
astar.show_animation = False
astar.use_beam_search = False
astar.use_iterative_deepening = False
astar.use_dynamic_weighting = False
astar.use_theta_star = False
astar.use_jump_point = False
# A* with jump point
a_star.use_jump_point = True
a_star.main()
reset_all()
if __name__ == '__main__': # pragma: no cover
test = Test()
test.test()
def reset_all():
a_star.show_animation = False
a_star.use_beam_search = False
a_star.use_iterative_deepening = False
a_star.use_dynamic_weighting = False
a_star.use_theta_star = False
a_star.use_jump_point = False
if __name__ == '__main__':
conftest.run_this_test(__file__)

27
tests/test_batch_informed_rrt_star.py
View File

@@ -1,25 +1,14 @@
from unittest import TestCase
import sys
import os
import random
sys.path.append(os.path.dirname(__file__) + "/../")
try:
from PathPlanning.BatchInformedRRTStar import batch_informed_rrtstar as m
except ImportError:
raise
print(__file__)
random.seed(12345)
import conftest
from PathPlanning.BatchInformedRRTStar import batch_informed_rrtstar as m
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main(maxIter=10)
def test_1():
m.show_animation = False
random.seed(12345)
m.main(maxIter=10)
if __name__ == '__main__': # pragma: no cover
test = Test()
test.test1()
if __name__ == '__main__':
conftest.run_this_test(__file__)

22
tests/test_bezier_path.py
View File

@@ -1,16 +1,16 @@
from unittest import TestCase
import sys
sys.path.append("./PathPlanning/BezierPath/")
import conftest
from PathPlanning.BezierPath import bezier_path as m
print(__file__)
def test_1():
m.show_animation = False
m.main()
class Test(TestCase):
def test_2():
m.show_animation = False
m.main2()
def test1(self):
m.show_animation = False
m.main()
m.main2()
if __name__ == '__main__':
conftest.run_this_test(__file__)

34
tests/test_bipedal_planner.py
View File

@@ -1,24 +1,18 @@
from unittest import TestCase
import sys
sys.path.append("./Bipedal/bipedal_planner/")
try:
from Bipedal.bipedal_planner import bipedal_planner as m
except Exception:
raise
print(__file__)
import conftest
from Bipedal.bipedal_planner import bipedal_planner as m
class Test(TestCase):
def test_1():
bipedal_planner = m.BipedalPlanner()
def test(self):
bipedal_planner = m.BipedalPlanner()
footsteps = [[0.0, 0.2, 0.0],
[0.3, 0.2, 0.0],
[0.3, 0.2, 0.2],
[0.3, 0.2, 0.2],
[0.0, 0.2, 0.2]]
bipedal_planner.set_ref_footsteps(footsteps)
bipedal_planner.walk(plot=False)
footsteps = [[0.0, 0.2, 0.0],
[0.3, 0.2, 0.0],
[0.3, 0.2, 0.2],
[0.3, 0.2, 0.2],
[0.0, 0.2, 0.2]]
bipedal_planner.set_ref_footsteps(footsteps)
bipedal_planner.walk(plot=False)
if __name__ == '__main__':
conftest.run_this_test(__file__)

29
tests/test_breadth_first_search.py
View File

@@ -1,26 +1,11 @@
from unittest import TestCase
import sys
import os
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../PathPlanning/BreadthFirstSearch/")
import conftest
from PathPlanning.BreadthFirstSearch import breadth_first_search as m
try:
import breadth_first_search as m
except ImportError:
raise
def test_1():
m.show_animation = False
m.main()
print(__file__)
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
if __name__ == '__main__': # pragma: no cover
test = Test()
test.test1()
if __name__ == '__main__':
conftest.run_this_test(__file__)

20
tests/test_bug.py
View File

@@ -1,17 +1,11 @@
from PathPlanning.BugPlanning import bug as b
from unittest import TestCase
import sys
import os
sys.path.append(os.path.dirname(__file__) + "/../")
import conftest
from PathPlanning.BugPlanning import bug as m
class Test(TestCase):
def test(self):
b.show_animation = False
b.main(bug_0=True, bug_1=True, bug_2=True)
def test_1():
m.show_animation = False
m.main(bug_0=True, bug_1=True, bug_2=True)
if __name__ == '__main__': # pragma: no cover
test = Test()
test.test()
if __name__ == '__main__':
conftest.run_this_test(__file__)

18
tests/test_cgmres_nmpc.py
View File

@@ -1,15 +1,11 @@
from unittest import TestCase
import conftest
from PathTracking.cgmres_nmpc import cgmres_nmpc as m
import sys
if 'cvxpy' in sys.modules: # pragma: no cover
sys.path.append("./PathTracking/cgmres_nmpc/")
from PathTracking.cgmres_nmpc import cgmres_nmpc as m
def test1():
m.show_animation = False
m.main()
print(__file__)
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
if __name__ == '__main__':
conftest.run_this_test(__file__)

15
tests/test_circle_fitting.py
View File

@@ -1,12 +1,11 @@
from unittest import TestCase
import conftest
from Mapping.circle_fitting import circle_fitting as m
print(__file__)
def test_1():
m.show_animation = False
m.main()
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
if __name__ == '__main__':
conftest.run_this_test(__file__)

31
tests/test_closed_loop_rrt_star_car.py
View File

@@ -1,28 +1,13 @@
import os
import sys
import conftest
from PathPlanning.ClosedLoopRRTStar import closed_loop_rrt_star_car as m
import random
from unittest import TestCase
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../")
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../PathPlanning/ClosedLoopRRTStar/")
try:
from PathPlanning.ClosedLoopRRTStar import closed_loop_rrt_star_car as m
except ImportError:
raise
random.seed(12345)
print(__file__)
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main(gx=1.0, gy=0.0, gyaw=0.0, max_iter=5)
def test_1():
random.seed(12345)
m.show_animation = False
m.main(gx=1.0, gy=0.0, gyaw=0.0, max_iter=5)
if __name__ == '__main__': # pragma: no cover
test = Test()
test.test1()
if __name__ == '__main__':
conftest.run_this_test(__file__)

19
tests/test_cubature_kalman_filter.py
View File

@@ -1,14 +1,13 @@
from unittest import TestCase
import conftest
from Localization.cubature_kalman_filter import cubature_kalman_filter as m
print(__file__)
def test1():
m.show_final = False
m.show_animation = False
m.show_ellipse = False
m.main()
class Test(TestCase):
def test1(self):
m.show_final = False
m.show_animation = False
m.show_ellipse = False
m.main()
if __name__ == '__main__':
conftest.run_this_test(__file__)

29
tests/test_depth_first_search.py
View File

@@ -1,26 +1,11 @@
from unittest import TestCase
import sys
import os
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../PathPlanning/DepthFirstSearch/")
import conftest
from PathPlanning.DepthFirstSearch import depth_first_search as m
try:
import depth_first_search as m
except ImportError:
raise
def test_1():
m.show_animation = False
m.main()
print(__file__)
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
if __name__ == '__main__': # pragma: no cover
test = Test()
test.test1()
if __name__ == '__main__':
conftest.run_this_test(__file__)

29
tests/test_dijkstra.py
View File

@@ -1,26 +1,11 @@
from unittest import TestCase
import sys
import os
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../PathPlanning/Dijkstra/")
import conftest
from PathPlanning.Dijkstra import dijkstra as m
try:
import dijkstra as m
except ImportError:
raise
def test_1():
m.show_animation = False
m.main()
print(__file__)
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
if __name__ == '__main__': # pragma: no cover
test = Test()
test.test1()
if __name__ == '__main__':
conftest.run_this_test(__file__)

22
tests/test_drone_3d_trajectory_following.py
View File

@@ -1,16 +1,12 @@
import os
import sys
from unittest import TestCase
sys.path.append(os.path.dirname(__file__) + "/../AerialNavigation/drone_3d_trajectory_following/")
import drone_3d_trajectory_following as m
print(__file__)
import conftest
from AerialNavigation.drone_3d_trajectory_following \
import drone_3d_trajectory_following as m
class Test(TestCase):
def test1():
m.show_animation = False
m.main()
def test1(self):
m.show_animation = False
m.main()
if __name__ == '__main__':
conftest.run_this_test(__file__)

97
tests/test_dubins_path_planning.py
View File

@@ -1,60 +1,65 @@
from unittest import TestCase
import conftest
import numpy as np
np.random.seed(12345)
from PathPlanning.DubinsPath import dubins_path_planning
np.random.seed(12345)
class Test(TestCase):
@staticmethod
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)
print("x", px[-1], end_x)
assert (abs(px[-1] - end_x) <= 0.01)
print("y", py[-1], end_y)
assert (abs(py[-1] - end_y) <= 0.01)
print("yaw", pyaw[-1], end_yaw)
assert (abs(pyaw[-1] - end_yaw) <= 0.01)
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 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]
def test_1():
start_x = 1.0 # [m]
start_y = 1.0 # [m]
start_yaw = np.deg2rad(45.0) # [rad]
curvature = 1.0
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)
self.check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, end_x, end_y, end_yaw)
check_edge_condition(px, py, pyaw,
start_x, start_y, start_yaw,
end_x, end_y, end_yaw)
def test2(self):
dubins_path_planning.show_animation = False
dubins_path_planning.main()
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)
if __name__ == '__main__':
conftest.run_this_test(__file__)

94
tests/test_dynamic_window_approach.py
View File

@@ -1,58 +1,48 @@
import os
import sys
import conftest
import numpy as np
from unittest import TestCase
sys.path.append(os.path.dirname(__file__) + "/../")
try:
from PathPlanning.DynamicWindowApproach import dynamic_window_approach as m
except ImportError:
raise
print(__file__)
from PathPlanning.DynamicWindowApproach import dynamic_window_approach as m
class TestDynamicWindowApproach(TestCase):
def test_main1(self):
m.show_animation = False
m.main(gx=1.0, gy=1.0)
def test_main2(self):
m.show_animation = False
m.main(gx=1.0, gy=1.0, robot_type=m.RobotType.rectangle)
def test_stuck_main(self):
m.show_animation = False
# adjust cost
m.config.to_goal_cost_gain = 0.2
m.config.obstacle_cost_gain = 2.0
# obstacles and goals for stuck condition
m.config.ob = -1 * np.array([[-1.0, -1.0],
[0.0, 2.0],
[2.0, 6.0],
[2.0, 8.0],
[3.0, 9.27],
[3.79, 9.39],
[7.25, 8.97],
[7.0, 2.0],
[3.0, 4.0],
[6.0, 5.0],
[3.5, 5.8],
[6.0, 9.0],
[8.8, 9.0],
[5.0, 9.0],
[7.5, 3.0],
[9.0, 8.0],
[5.8, 4.4],
[12.0, 12.0],
[3.0, 2.0],
[13.0, 13.0]
])
m.main(gx=-5.0, gy=-7.0)
def test_main1():
m.show_animation = False
m.main(gx=1.0, gy=1.0)
if __name__ == '__main__': # pragma: no cover
test = TestDynamicWindowApproach()
test.test_main1()
test.test_main2()
test.test_stuck_main()
def test_main2():
m.show_animation = False
m.main(gx=1.0, gy=1.0, robot_type=m.RobotType.rectangle)
def test_stuck_main():
m.show_animation = False
# adjust cost
m.config.to_goal_cost_gain = 0.2
m.config.obstacle_cost_gain = 2.0
# obstacles and goals for stuck condition
m.config.ob = -1 * np.array([[-1.0, -1.0],
[0.0, 2.0],
[2.0, 6.0],
[2.0, 8.0],
[3.0, 9.27],
[3.79, 9.39],
[7.25, 8.97],
[7.0, 2.0],
[3.0, 4.0],
[6.0, 5.0],
[3.5, 5.8],
[6.0, 9.0],
[8.8, 9.0],
[5.0, 9.0],
[7.5, 3.0],
[9.0, 8.0],
[5.8, 4.4],
[12.0, 12.0],
[3.0, 2.0],
[13.0, 13.0]
])
m.main(gx=-5.0, gy=-7.0)
if __name__ == '__main__':
conftest.run_this_test(__file__)

15
tests/test_ekf_slam.py
View File

@@ -1,12 +1,11 @@
from unittest import TestCase
import conftest
from SLAM.EKFSLAM import ekf_slam as m
print(__file__)
def test_1():
m.show_animation = False
m.main()
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
if __name__ == '__main__':
conftest.run_this_test(__file__)

13
tests/test_eta3_spline_path.py
View File

@@ -1,10 +1,11 @@
from unittest import TestCase
import conftest
from PathPlanning.Eta3SplinePath import eta3_spline_path as m
class Test(TestCase):
def test_1():
m.show_animation = False
m.main()
def test1(self):
m.show_animation = False
m.main()
if __name__ == '__main__':
conftest.run_this_test(__file__)

15
tests/test_extended_kalman_filter.py
View File

@@ -1,12 +1,11 @@
from unittest import TestCase
import conftest
from Localization.extended_kalman_filter import extended_kalman_filter as m
print(__file__)
def test_1():
m.show_animation = False
m.main()
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
if __name__ == '__main__':
conftest.run_this_test(__file__)

28
tests/test_fast_slam1.py
View File

@@ -1,24 +1,12 @@
from unittest import TestCase
import sys
import os
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../")
try:
from SLAM.FastSLAM1 import fast_slam1 as m
except:
raise
import conftest
from SLAM.FastSLAM1 import fast_slam1 as m
print(__file__)
def test1():
m.show_animation = False
m.SIM_TIME = 3.0
m.main()
class Test(TestCase):
def test1(self):
m.show_animation = False
m.SIM_TIME = 3.0
m.main()
if __name__ == '__main__': # pragma: no cover
test = Test()
test.test1()
if __name__ == '__main__':
conftest.run_this_test(__file__)

28
tests/test_fast_slam2.py
View File

@@ -1,24 +1,12 @@
from unittest import TestCase
import sys
import os
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../")
try:
from SLAM.FastSLAM2 import fast_slam2 as m
except:
raise
import conftest
from SLAM.FastSLAM2 import fast_slam2 as m
print(__file__)
def test1():
m.show_animation = False
m.SIM_TIME = 3.0
m.main()
class Test(TestCase):
def test1(self):
m.show_animation = False
m.SIM_TIME = 3.0
m.main()
if __name__ == '__main__': # pragma: no cover
test = Test()
test.test1()
if __name__ == '__main__':
conftest.run_this_test(__file__)

20
tests/test_flow_field.py
View File

@@ -1,17 +1,11 @@
import PathPlanning.FlowField.flowfield as flowfield
from unittest import TestCase
import sys
import os
sys.path.append(os.path.dirname(__file__) + "/../")
import conftest
import PathPlanning.FlowField.flowfield as flow_field
class Test(TestCase):
def test(self):
flowfield.show_animation = False
flowfield.main()
def test():
flow_field.show_animation = False
flow_field.main()
if __name__ == '__main__': # pragma: no cover
test = Test()
test.test()
if __name__ == '__main__':
conftest.run_this_test(__file__)

30
tests/test_frenet_optimal_trajectory.py
View File

@@ -1,26 +1,12 @@
from unittest import TestCase
import sys
import os
sys.path.append("./PathPlanning/FrenetOptimalTrajectory/")
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../")
try:
from PathPlanning.FrenetOptimalTrajectory import frenet_optimal_trajectory as m
except:
raise
import conftest
from PathPlanning.FrenetOptimalTrajectory import frenet_optimal_trajectory as m
print(__file__)
def test1():
m.show_animation = False
m.SIM_LOOP = 5
m.main()
class Test(TestCase):
def test1(self):
m.show_animation = False
m.SIM_LOOP = 5
m.main()
if __name__ == '__main__': # pragma: no cover
test = Test()
test.test1()
if __name__ == '__main__':
conftest.run_this_test(__file__)

15
tests/test_gaussian_grid_map.py
View File

@@ -1,12 +1,11 @@
from unittest import TestCase
import conftest
from Mapping.gaussian_grid_map import gaussian_grid_map as m
print(__file__)
def test1():
m.show_animation = False
m.main()
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
if __name__ == '__main__':
conftest.run_this_test(__file__)

28
tests/test_graph_based_slam.py
View File

@@ -1,24 +1,12 @@
from unittest import TestCase
import sys
import os
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../")
try:
from SLAM.GraphBasedSLAM import graph_based_slam as m
except:
raise
import conftest
from SLAM.GraphBasedSLAM import graph_based_slam as m
print(__file__)
def test_1():
m.show_animation = False
m.SIM_TIME = 20.0
m.main()
class Test(TestCase):
def test1(self):
m.show_animation = False
m.SIM_TIME = 20.0
m.main()
if __name__ == '__main__': # pragma: no cover
test = Test()
test.test1()
if __name__ == '__main__':
conftest.run_this_test(__file__)

202
tests/test_grid_based_sweep_coverage_path_planner.py
View File

@@ -1,124 +1,118 @@
import os
import sys
from unittest import TestCase
sys.path.append(os.path.dirname(
os.path.abspath(__file__)) + "/../PathPlanning/GridBasedSweepCPP")
sys.path.append(
os.path.dirname(os.path.abspath(__file__)) + "/../Mapping/grid_map_lib")
try:
import conftest
from PathPlanning.GridBasedSweepCPP \
import grid_based_sweep_coverage_path_planner
except ImportError:
raise
grid_based_sweep_coverage_path_planner.do_animation = False
RIGHT = grid_based_sweep_coverage_path_planner. \
SweepSearcher.MovingDirection.RIGHT
LEFT = grid_based_sweep_coverage_path_planner. \
SweepSearcher.MovingDirection.LEFT
UP = grid_based_sweep_coverage_path_planner. \
SweepSearcher.SweepDirection.UP
DOWN = grid_based_sweep_coverage_path_planner. \
SweepSearcher.SweepDirection.DOWN
class TestPlanning(TestCase):
def test_planning1():
ox = [0.0, 20.0, 50.0, 100.0, 130.0, 40.0, 0.0]
oy = [0.0, -20.0, 0.0, 30.0, 60.0, 80.0, 0.0]
resolution = 5.0
RIGHT = grid_based_sweep_coverage_path_planner.\
SweepSearcher.MovingDirection.RIGHT
LEFT = grid_based_sweep_coverage_path_planner. \
SweepSearcher.MovingDirection.LEFT
UP = grid_based_sweep_coverage_path_planner. \
SweepSearcher.SweepDirection.UP
DOWN = grid_based_sweep_coverage_path_planner. \
SweepSearcher.SweepDirection.DOWN
px, py = grid_based_sweep_coverage_path_planner.planning(
ox, oy, resolution,
moving_direction=RIGHT,
sweeping_direction=DOWN,
)
assert len(px) >= 5
def test_planning1(self):
ox = [0.0, 20.0, 50.0, 100.0, 130.0, 40.0, 0.0]
oy = [0.0, -20.0, 0.0, 30.0, 60.0, 80.0, 0.0]
resolution = 5.0
px, py = grid_based_sweep_coverage_path_planner.planning(
ox, oy, resolution,
moving_direction=LEFT,
sweeping_direction=DOWN,
)
assert len(px) >= 5
px, py = grid_based_sweep_coverage_path_planner.planning(
ox, oy, resolution,
moving_direction=self.RIGHT,
sweeping_direction=self.DOWN,
)
self.assertGreater(len(px), 5)
px, py = grid_based_sweep_coverage_path_planner.planning(
ox, oy, resolution,
moving_direction=RIGHT,
sweeping_direction=UP,
)
assert len(px) >= 5
px, py = grid_based_sweep_coverage_path_planner.planning(
ox, oy, resolution,
moving_direction=self.LEFT,
sweeping_direction=self.DOWN,
)
self.assertGreater(len(px), 5)
px, py = grid_based_sweep_coverage_path_planner.planning(
ox, oy, resolution,
moving_direction=RIGHT,
sweeping_direction=UP,
)
assert len(px) >= 5
px, py = grid_based_sweep_coverage_path_planner.planning(
ox, oy, resolution,
moving_direction=self.RIGHT,
sweeping_direction=self.UP,
)
self.assertGreater(len(px), 5)
px, py = grid_based_sweep_coverage_path_planner.planning(
ox, oy, resolution,
moving_direction=self.RIGHT,
sweeping_direction=self.UP,
)
self.assertGreater(len(px), 5)
def test_planning2():
ox = [0.0, 50.0, 50.0, 0.0, 0.0]
oy = [0.0, 0.0, 30.0, 30.0, 0.0]
resolution = 1.3
def test_planning2(self):
ox = [0.0, 50.0, 50.0, 0.0, 0.0]
oy = [0.0, 0.0, 30.0, 30.0, 0.0]
resolution = 1.3
px, py = grid_based_sweep_coverage_path_planner.planning(
ox, oy, resolution,
moving_direction=RIGHT,
sweeping_direction=DOWN,
)
assert len(px) >= 5
px, py = grid_based_sweep_coverage_path_planner.planning(
ox, oy, resolution,
moving_direction=self.RIGHT,
sweeping_direction=self.DOWN,
)
self.assertGreater(len(px), 5)
px, py = grid_based_sweep_coverage_path_planner.planning(
ox, oy, resolution,
moving_direction=LEFT,
sweeping_direction=DOWN,
)
assert len(px) >= 5
px, py = grid_based_sweep_coverage_path_planner.planning(
ox, oy, resolution,
moving_direction=self.LEFT,
sweeping_direction=self.DOWN,
)
self.assertGreater(len(px), 5)
px, py = grid_based_sweep_coverage_path_planner.planning(
ox, oy, resolution,
moving_direction=RIGHT,
sweeping_direction=UP,
)
assert len(px) >= 5
px, py = grid_based_sweep_coverage_path_planner.planning(
ox, oy, resolution,
moving_direction=self.RIGHT,
sweeping_direction=self.UP,
)
self.assertGreater(len(px), 5)
px, py = grid_based_sweep_coverage_path_planner.planning(
ox, oy, resolution,
moving_direction=RIGHT,
sweeping_direction=DOWN,
)
assert len(px) >= 5
px, py = grid_based_sweep_coverage_path_planner.planning(
ox, oy, resolution,
moving_direction=self.RIGHT,
sweeping_direction=self.DOWN,
)
self.assertGreater(len(px), 5)
def test_planning3(self):
ox = [0.0, 20.0, 50.0, 200.0, 130.0, 40.0, 0.0]
oy = [0.0, -80.0, 0.0, 30.0, 60.0, 80.0, 0.0]
resolution = 5.1
px, py = grid_based_sweep_coverage_path_planner.planning(
ox, oy, resolution,
moving_direction=self.RIGHT,
sweeping_direction=self.DOWN,
)
self.assertGreater(len(px), 5)
def test_planning3():
ox = [0.0, 20.0, 50.0, 200.0, 130.0, 40.0, 0.0]
oy = [0.0, -80.0, 0.0, 30.0, 60.0, 80.0, 0.0]
resolution = 5.1
px, py = grid_based_sweep_coverage_path_planner.planning(
ox, oy, resolution,
moving_direction=RIGHT,
sweeping_direction=DOWN,
)
assert len(px) >= 5
px, py = grid_based_sweep_coverage_path_planner.planning(
ox, oy, resolution,
moving_direction=self.LEFT,
sweeping_direction=self.DOWN,
)
self.assertGreater(len(px), 5)
px, py = grid_based_sweep_coverage_path_planner.planning(
ox, oy, resolution,
moving_direction=LEFT,
sweeping_direction=DOWN,
)
assert len(px) >= 5
px, py = grid_based_sweep_coverage_path_planner.planning(
ox, oy, resolution,
moving_direction=self.RIGHT,
sweeping_direction=self.UP,
)
self.assertGreater(len(px), 5)
px, py = grid_based_sweep_coverage_path_planner.planning(
ox, oy, resolution,
moving_direction=RIGHT,
sweeping_direction=UP,
)
assert len(px) >= 5
px, py = grid_based_sweep_coverage_path_planner.planning(
ox, oy, resolution,
moving_direction=self.RIGHT,
sweeping_direction=self.DOWN,
)
self.assertGreater(len(px), 5)
px, py = grid_based_sweep_coverage_path_planner.planning(
ox, oy, resolution,
moving_direction=RIGHT,
sweeping_direction=DOWN,
)
assert len(px) >= 5
if __name__ == '__main__':
conftest.run_this_test(__file__)

43
tests/test_grid_map_lib.py
View File

@@ -1,38 +1,25 @@
import os
import sys
import unittest
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../Mapping/grid_map_lib")
try:
from grid_map_lib import GridMap
except ImportError:
raise
from Mapping.grid_map_lib.grid_map_lib import GridMap
class MyTestCase(unittest.TestCase):
def test_position_set():
grid_map = GridMap(100, 120, 0.5, 10.0, -0.5)
def test_position_set(self):
grid_map = GridMap(100, 120, 0.5, 10.0, -0.5)
grid_map.set_value_from_xy_pos(10.1, -1.1, 1.0)
grid_map.set_value_from_xy_pos(10.1, -0.1, 1.0)
grid_map.set_value_from_xy_pos(10.1, 1.1, 1.0)
grid_map.set_value_from_xy_pos(11.1, 0.1, 1.0)
grid_map.set_value_from_xy_pos(10.1, 0.1, 1.0)
grid_map.set_value_from_xy_pos(9.1, 0.1, 1.0)
grid_map.set_value_from_xy_pos(10.1, -1.1, 1.0)
grid_map.set_value_from_xy_pos(10.1, -0.1, 1.0)
grid_map.set_value_from_xy_pos(10.1, 1.1, 1.0)
grid_map.set_value_from_xy_pos(11.1, 0.1, 1.0)
grid_map.set_value_from_xy_pos(10.1, 0.1, 1.0)
grid_map.set_value_from_xy_pos(9.1, 0.1, 1.0)
self.assertEqual(True, True)
def test_polygon_set():
ox = [0.0, 20.0, 50.0, 100.0, 130.0, 40.0]
oy = [0.0, -20.0, 0.0, 30.0, 60.0, 80.0]
def test_polygon_set(self):
ox = [0.0, 20.0, 50.0, 100.0, 130.0, 40.0]
oy = [0.0, -20.0, 0.0, 30.0, 60.0, 80.0]
grid_map = GridMap(600, 290, 0.7, 60.0, 30.5)
grid_map = GridMap(600, 290, 0.7, 60.0, 30.5)
grid_map.set_value_from_polygon(ox, oy, 1.0, inside=False)
self.assertEqual(True, True)
grid_map.set_value_from_polygon(ox, oy, 1.0, inside=False)
if __name__ == '__main__':
unittest.main()
conftest.run_this_test(__file__)

27
tests/test_histogram_filter.py
View File

@@ -1,25 +1,12 @@
from unittest import TestCase
import sys
import os
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../")
try:
from Localization.histogram_filter import histogram_filter as m
except:
raise
import conftest
from Localization.histogram_filter import histogram_filter as m
print(__file__)
class Test(TestCase):
def test1(self):
m.show_animation = False
m.SIM_TIME = 1.0
m.main()
def test1():
m.show_animation = False
m.SIM_TIME = 1.0
m.main()
if __name__ == '__main__':
test = Test()
test.test1()
conftest.run_this_test(__file__)

25
tests/test_hybrid_a_star.py
View File

@@ -1,22 +1,11 @@
from unittest import TestCase
import sys
import os
sys.path.append(os.path.dirname(__file__) + "/../")
sys.path.append(os.path.dirname(os.path.abspath(__file__))
+ "/../PathPlanning/HybridAStar")
try:
from PathPlanning.HybridAStar import hybrid_a_star as m
except Exception:
raise
import conftest
from PathPlanning.HybridAStar import hybrid_a_star as m
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
def test1():
m.show_animation = False
m.main()
if __name__ == '__main__': # pragma: no cover
test = Test()
test.test1()
if __name__ == '__main__':
conftest.run_this_test(__file__)

27
tests/test_informed_rrt_star.py
View File

@@ -1,24 +1,11 @@
from unittest import TestCase
import sys
import os
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../")
sys.path.append(os.path.dirname(os.path.abspath(__file__))
+ "/../PathPlanning/InformedRRTStar/")
try:
from PathPlanning.InformedRRTStar import informed_rrt_star as m
except:
raise
print(__file__)
import conftest
from PathPlanning.InformedRRTStar import informed_rrt_star as m
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
def test1():
m.show_animation = False
m.main()
if __name__ == '__main__': # pragma: no cover
test = Test()
test.test1()
if __name__ == '__main__':
conftest.run_this_test(__file__)

17
tests/test_inverted_pendulum_mpc_control.py
View File

@@ -1,15 +1,14 @@
from unittest import TestCase
import conftest
import sys
if 'cvxpy' in sys.modules: # pragma: no cover
sys.path.append("./InvertedPendulumCart/inverted_pendulum_mpc_control/")
import inverted_pendulum_mpc_control as m
from InvertedPendulumCart.InvertedPendulumMPCControl \
import inverted_pendulum_mpc_control as m
print(__file__)
def test1():
m.show_animation = False
m.main()
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
if __name__ == '__main__':
conftest.run_this_test(__file__)

15
tests/test_iterative_closest_point.py
View File

@@ -1,12 +1,11 @@
from unittest import TestCase
import conftest
from SLAM.iterative_closest_point import iterative_closest_point as m
print(__file__)
def test_1():
m.show_animation = False
m.main()
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
if __name__ == '__main__':
conftest.run_this_test(__file__)

15
tests/test_kmeans_clustering.py
View File

@@ -1,12 +1,11 @@
from unittest import TestCase
import conftest
from Mapping.kmeans_clustering import kmeans_clustering as m
print(__file__)
def test_1():
m.show_animation = False
m.main()
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
if __name__ == '__main__':
conftest.run_this_test(__file__)

27
tests/test_lqr_rrt_star.py
View File

@@ -1,27 +1,14 @@
from unittest import TestCase
import sys
import os
import conftest # Add root path to sys.path
from PathPlanning.LQRRRTStar import lqr_rrt_star as m
import random
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../")
sys.path.append(os.path.dirname(os.path.abspath(__file__))
+ "/../PathPlanning/LQRRRTStar/")
try:
from PathPlanning.LQRRRTStar import lqr_rrt_star as m
except ImportError:
raise
print(__file__)
random.seed(12345)
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main(maxIter=5)
def test1():
m.show_animation = False
m.main(maxIter=5)
if __name__ == '__main__': # pragma: no cover
test = Test()
test.test1()
if __name__ == '__main__':
conftest.run_this_test(__file__)

16
tests/test_lqr_speed_steer_control.py
View File

@@ -1,15 +1,7 @@
from unittest import TestCase
import sys
sys.path.append("./PathTracking/lqr_speed_steer_control/")
import conftest # Add root path to sys.path
from PathTracking.lqr_speed_steer_control import lqr_speed_steer_control as m
print(__file__)
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
def test_1():
m.show_animation = False
m.main()

18
tests/test_lqr_steer_control.py
View File

@@ -1,15 +1,11 @@
from unittest import TestCase
import sys
sys.path.append("./PathTracking/lqr_steer_control/")
import conftest # Add root path to sys.path
from PathTracking.lqr_steer_control import lqr_steer_control as m
print(__file__)
def test1():
m.show_animation = False
m.main()
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
if __name__ == '__main__':
conftest.run_this_test(__file__)

20
tests/test_model_predictive_speed_and_steer_control.py
View File

@@ -1,16 +1,14 @@
from unittest import TestCase
import sys
if 'cvxpy' in sys.modules: # pragma: no cover
sys.path.append("./PathTracking/model_predictive_speed_and_steer_control/")
from PathTracking.model_predictive_speed_and_steer_control import model_predictive_speed_and_steer_control as m
from PathTracking.model_predictive_speed_and_steer_control \
import model_predictive_speed_and_steer_control as m
print(__file__)
def test_1():
m.show_animation = False
m.main()
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
m.main2()
def test_2():
m.show_animation = False
m.main2()

13
tests/test_move_to_pose.py
View File

@@ -1,12 +1,7 @@
from unittest import TestCase
import conftest # Add root path to sys.path
from PathTracking.move_to_pose import move_to_pose as m
print(__file__)
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
def test_1():
m.show_animation = False
m.main()

20
tests/test_n_joint_arm_to_point_control.py
View File

@@ -1,19 +1,11 @@
import os
import sys
import conftest # Add root path to sys.path
from ArmNavigation.n_joint_arm_to_point_control\
import n_joint_arm_to_point_control as m
import random
from unittest import TestCase
sys.path.append(os.path.dirname(__file__) + "/../ArmNavigation/n_joint_arm_to_point_control/")
import n_joint_arm_to_point_control as m
print(__file__)
random.seed(12345)
class Test(TestCase):
def test1(self):
m.show_animation = False
m.animation()
def test1():
m.show_animation = False
m.animation()

13
tests/test_particle_filter.py
View File

@@ -1,12 +1,7 @@
from unittest import TestCase
import conftest # Add root path to sys.path
from Localization.particle_filter import particle_filter as m
print(__file__)
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
def test_1():
m.show_animation = False
m.main()

13
tests/test_potential_field_planning.py
View File

@@ -1,12 +1,7 @@
from unittest import TestCase
import conftest # Add root path to sys.path
from PathPlanning.PotentialFieldPlanning import potential_field_planning as m
print(__file__)
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
def test1():
m.show_animation = False
m.main()

12
tests/test_probabilistic_road_map.py
View File

@@ -1,9 +1,11 @@
from unittest import TestCase
import conftest # Add root path to sys.path
from PathPlanning.ProbabilisticRoadMap import probabilistic_road_map
class Test(TestCase):
def test1():
probabilistic_road_map.show_animation = False
probabilistic_road_map.main()
def test1(self):
probabilistic_road_map.show_animation = False
probabilistic_road_map.main()
if __name__ == '__main__':
conftest.run_this_test(__file__)

12
tests/test_pure_pursuit.py
View File

@@ -1,11 +1,7 @@
from unittest import TestCase
import conftest # Add root path to sys.path
from PathTracking.pure_pursuit import pure_pursuit as m
print("pure_pursuit test")
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
def test1():
m.show_animation = False
m.main()

13
tests/test_quintic_polynomials_planner.py
View File

@@ -1,12 +1,7 @@
from unittest import TestCase
import conftest # Add root path to sys.path
from PathPlanning.QuinticPolynomialsPlanner import quintic_polynomials_planner as m
print(__file__)
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
def test1():
m.show_animation = False
m.main()

13
tests/test_raycasting_grid_map.py
View File

@@ -1,12 +1,7 @@
from unittest import TestCase
import conftest # Add root path to sys.path
from Mapping.raycasting_grid_map import raycasting_grid_map as m
print(__file__)
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
def test1():
m.show_animation = False
m.main()

18
tests/test_rear_wheel_feedback.py
View File

@@ -1,15 +1,11 @@
from unittest import TestCase
import sys
sys.path.append("./PathTracking/rear_wheel_feedback/")
import conftest # Add root path to sys.path
from PathTracking.rear_wheel_feedback import rear_wheel_feedback as m
print(__file__)
def test1():
m.show_animation = False
m.main()
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
if __name__ == '__main__':
conftest.run_this_test(__file__)

25
tests/test_rectangle_fitting.py
View File

@@ -1,22 +1,7 @@
from unittest import TestCase
import sys
import os
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../Mapping/rectangle_fitting/")
try:
from Mapping.rectangle_fitting import rectangle_fitting as m
except ImportError:
raise
import conftest # Add root path to sys.path
from Mapping.rectangle_fitting import rectangle_fitting as m
print(__file__)
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
def test1():
m.show_animation = False
m.main()

60
tests/test_reeds_shepp_path_planning.py
View File

@@ -1,42 +1,42 @@
from unittest import TestCase
import conftest # Add root path to sys.path
from PathPlanning.ReedsSheppPath import reeds_shepp_path_planning as m
import numpy as np
class Test(TestCase):
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)
print("x", px[-1], end_x)
assert (abs(px[-1] - end_x) <= 0.01)
print("y", py[-1], end_y)
assert (abs(py[-1] - end_y) <= 0.01)
print("yaw", pyaw[-1], end_yaw)
assert (abs(pyaw[-1] - end_yaw) <= 0.01)
@staticmethod
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)
print("x", px[-1], end_x)
assert (abs(px[-1] - end_x) <= 0.01)
print("y", py[-1], end_y)
assert (abs(py[-1] - end_y) <= 0.01)
print("yaw", pyaw[-1], end_yaw)
assert (abs(pyaw[-1] - end_yaw) <= 0.01)
def test1(self):
m.show_animation = False
m.main()
def test1():
m.show_animation = False
m.main()
def test2(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]
def test2():
N_TEST = 10
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]
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]
curvature = 1.0 / (np.random.rand() * 5.0)
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]
px, py, pyaw, mode, clen = m.reeds_shepp_path_planning(
start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature)
curvature = 1.0 / (np.random.rand() * 5.0)
self.check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, end_x, end_y, end_yaw)
px, py, pyaw, mode, clen = m.reeds_shepp_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)

13
tests/test_rocket_powered_landing.py
View File

@@ -1,15 +1,10 @@
from unittest import TestCase
import conftest # Add root path to sys.path
import sys
if 'cvxpy' in sys.modules: # pragma: no cover
sys.path.append("./AerialNavigation/rocket_powered_landing/")
from AerialNavigation.rocket_powered_landing import rocket_powered_landing as m
print(__file__)
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
def test1():
m.show_animation = False
m.main()

38
tests/test_rrt.py
View File

@@ -1,33 +1,21 @@
import os
import sys
import conftest
import random
from unittest import TestCase
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../")
try:
from PathPlanning.RRT import rrt as m
from PathPlanning.RRT import rrt_with_pathsmoothing as m1
except ImportError:
raise
print(__file__)
from PathPlanning.RRT import rrt as m
from PathPlanning.RRT import rrt_with_pathsmoothing as m1
random.seed(12345)
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main(gx=1.0, gy=1.0)
def test2(self):
m1.show_animation = False
m1.main()
def test1():
m.show_animation = False
m.main(gx=1.0, gy=1.0)
if __name__ == '__main__': # pragma: no cover
test = Test()
test.test1()
test.test2()
def test2():
m1.show_animation = False
m1.main()
if __name__ == '__main__':
conftest.run_this_test(__file__)

30
tests/test_rrt_dubins.py
View File

@@ -1,27 +1,11 @@
from unittest import TestCase
import sys
import os
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../PathPlanning/RRTDubins/")
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../PathPlanning/DubinsPath/")
try:
import rrt_dubins as m
except:
raise
import conftest # Add root path to sys.path
from PathPlanning.RRTDubins import rrt_dubins as m
print(__file__)
def test1():
m.show_animation = False
m.main()
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
if __name__ == '__main__': # pragma: no cover
test = Test()
test.test1()
if __name__ == '__main__':
conftest.run_this_test(__file__)

46
tests/test_rrt_star.py
View File

@@ -1,37 +1,23 @@
import os
import sys
from unittest import TestCase
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../PathPlanning/RRTStar/")
try:
import rrt_star as m
except ImportError:
raise
import conftest # Add root path to sys.path
from PathPlanning.RRTStar import rrt_star as m
print(__file__)
def test1():
m.show_animation = False
m.main()
class Test(TestCase):
def test_no_obstacle():
obstacle_list = []
def test1(self):
m.show_animation = False
m.main()
# Set Initial parameters
rrt_star = m.RRTStar(start=[0, 0],
goal=[6, 10],
rand_area=[-2, 15],
obstacle_list=obstacle_list)
path = rrt_star.planning(animation=False)
assert path is not None
def test_no_obstacle(self):
obstacle_list = []
# Set Initial parameters
rrt_star = m.RRTStar(start=[0, 0],
goal=[6, 10],
rand_area=[-2, 15],
obstacle_list=obstacle_list)
path = rrt_star.planning(animation=False)
assert path is not None
if __name__ == '__main__': # pragma: no cover
test = Test()
test.test1()
test.test_no_obstacle()
if __name__ == '__main__':
conftest.run_this_test(__file__)

29
tests/test_rrt_star_dubins.py
View File

@@ -1,26 +1,11 @@
from unittest import TestCase
import sys
import os
sys.path.append(os.path.dirname(os.path.abspath(__file__))
+ "/../PathPlanning/RRTStarDubins/")
try:
import rrt_star_dubins as m
except:
raise
print(__file__)
import conftest # Add root path to sys.path
from PathPlanning.RRTStarDubins import rrt_star_dubins as m
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
def test1():
m.show_animation = False
m.main()
if __name__ == '__main__': # pragma: no cover
test = Test()
test.test1()
if __name__ == '__main__':
conftest.run_this_test(__file__)

31
tests/test_rrt_star_reeds_shepp.py
View File

@@ -1,28 +1,11 @@
import os
import sys
from unittest import TestCase
sys.path.append(os.path.dirname(os.path.abspath(__file__))
+ "/../PathPlanning/RRTStarReedsShepp/")
sys.path.append(os.path.dirname(os.path.abspath(__file__))
+ "/../PathPlanning/ReedsSheppPath/")
try:
import rrt_star_reeds_shepp as m
except:
raise
import conftest # Add root path to sys.path
import rrt_star_reeds_shepp as m
print(__file__)
def test1():
m.show_animation = False
m.main(max_iter=5)
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main(max_iter=5)
if __name__ == '__main__': # pragma: no cover
test = Test()
test.test1()
if __name__ == '__main__':
conftest.run_this_test(__file__)

28
tests/test_rrt_star_seven_joint_arm.py
View File

@@ -1,26 +1,12 @@
import os
import sys
from unittest import TestCase
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../ArmNavigation/rrt_star_seven_joint_arm_control/")
try:
import conftest # Add root path to sys.path
from ArmNavigation.rrt_star_seven_joint_arm_control \
import rrt_star_seven_joint_arm_control as m
except ImportError:
raise
print(__file__)
def test1():
m.show_animation = False
m.main()
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
if __name__ == '__main__': # pragma: no cover
test = Test()
test.test1()
if __name__ == '__main__':
conftest.run_this_test(__file__)

30
tests/test_rrt_with_sobol_sampler.py
View File

@@ -1,30 +1,14 @@
import os
import sys
import conftest # Add root path to sys.path
from PathPlanning.RRT import rrt_with_sobol_sampler as m
import random
from unittest import TestCase
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../")
sys.path.append(os.path.dirname(
os.path.abspath(__file__)) + "/../PathPlanning/RRT")
try:
from PathPlanning.RRT import rrt_with_sobol_sampler as m
except ImportError:
raise
print(__file__)
random.seed(12345)
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main(gx=1.0, gy=1.0)
def test1():
m.show_animation = False
m.main(gx=1.0, gy=1.0)
if __name__ == '__main__': # pragma: no cover
test = Test()
test.test1()
if __name__ == '__main__':
conftest.run_this_test(__file__)

88
tests/test_spiral_spanning_tree_coverage_path_planner.py
View File

@@ -1,58 +1,58 @@
import conftest # Add root path to sys.path
import os
import sys
import matplotlib.pyplot as plt
from unittest import TestCase
sys.path.append(os.path.dirname(
os.path.abspath(__file__)) + "/../PathPlanning/SpiralSpanningTreeCPP")
try:
from PathPlanning.SpiralSpanningTreeCPP \
import spiral_spanning_tree_coverage_path_planner
except ImportError:
raise
spiral_spanning_tree_coverage_path_planner.do_animation = True
class TestPlanning(TestCase):
def spiral_stc_cpp(self, img, start):
num_free = 0
for i in range(img.shape[0]):
for j in range(img.shape[1]):
num_free += img[i][j]
def spiral_stc_cpp(img, start):
num_free = 0
for i in range(img.shape[0]):
for j in range(img.shape[1]):
num_free += img[i][j]
STC_planner = spiral_spanning_tree_coverage_path_planner.\
SpiralSpanningTreeCoveragePlanner(img)
STC_planner = spiral_spanning_tree_coverage_path_planner.\
SpiralSpanningTreeCoveragePlanner(img)
edge, route, path = STC_planner.plan(start)
edge, route, path = STC_planner.plan(start)
covered_nodes = set()
for p, q in edge:
covered_nodes.add(p)
covered_nodes.add(q)
covered_nodes = set()
for p, q in edge:
covered_nodes.add(p)
covered_nodes.add(q)
# assert complete coverage
self.assertEqual(len(covered_nodes), num_free / 4)
# assert complete coverage
assert len(covered_nodes) == num_free / 4
def test_spiral_stc_cpp_1(self):
img_dir = os.path.dirname(
os.path.abspath(__file__)) + \
"/../PathPlanning/SpiralSpanningTreeCPP"
img = plt.imread(os.path.join(img_dir, 'map', 'test.png'))
start = (0, 0)
self.spiral_stc_cpp(img, start)
def test_spiral_stc_cpp_2(self):
img_dir = os.path.dirname(
os.path.abspath(__file__)) + \
"/../PathPlanning/SpiralSpanningTreeCPP"
img = plt.imread(os.path.join(img_dir, 'map', 'test_2.png'))
start = (10, 0)
self.spiral_stc_cpp(img, start)
def test_spiral_stc_cpp_1():
img_dir = os.path.dirname(
os.path.abspath(__file__)) + \
"/../PathPlanning/SpiralSpanningTreeCPP"
img = plt.imread(os.path.join(img_dir, 'map', 'test.png'))
start = (0, 0)
spiral_stc_cpp(img, start)
def test_spiral_stc_cpp_3(self):
img_dir = os.path.dirname(
os.path.abspath(__file__)) + \
"/../PathPlanning/SpiralSpanningTreeCPP"
img = plt.imread(os.path.join(img_dir, 'map', 'test_3.png'))
start = (0, 0)
self.spiral_stc_cpp(img, start)
def test_spiral_stc_cpp_2():
img_dir = os.path.dirname(
os.path.abspath(__file__)) + \
"/../PathPlanning/SpiralSpanningTreeCPP"
img = plt.imread(os.path.join(img_dir, 'map', 'test_2.png'))
start = (10, 0)
spiral_stc_cpp(img, start)
def test_spiral_stc_cpp_3():
img_dir = os.path.dirname(
os.path.abspath(__file__)) + \
"/../PathPlanning/SpiralSpanningTreeCPP"
img = plt.imread(os.path.join(img_dir, 'map', 'test_3.png'))
start = (0, 0)
spiral_stc_cpp(img, start)
if __name__ == '__main__':
conftest.run_this_test(__file__)

16
tests/test_stanley_controller.py
View File

@@ -1,15 +1,7 @@
from unittest import TestCase
import sys
sys.path.append("./PathTracking/stanley_controller/")
import conftest # Add root path to sys.path
from PathTracking.stanley_controller import stanley_controller as m
print(__file__)
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
def test1():
m.show_animation = False
m.main()

34
tests/test_state_lattice_planner.py
View File

@@ -1,30 +1,10 @@
from unittest import TestCase
import sys
import os
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../PathPlanning/ModelPredictiveTrajectoryGenerator/")
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../PathPlanning/StateLatticePlanner/")
try:
import state_lattice_planner as m
import conftest # Add root path to sys.path
from PathPlanning.StateLatticePlanner import state_lattice_planner as m
from PathPlanning.ModelPredictiveTrajectoryGenerator \
import model_predictive_trajectory_generator as m2
except:
raise
print(__file__)
class Test(TestCase):
def test1(self):
m.show_animation = False
m2.show_animation = False
m.main()
if __name__ == '__main__': # pragma: no cover
test = Test()
test.test1()
def test1():
m.show_animation = False
m2.show_animation = False
m.main()

16
tests/test_two_joint_arm_to_point_control.py
View File

@@ -1,12 +1,8 @@
from unittest import TestCase
from ArmNavigation.two_joint_arm_to_point_control import two_joint_arm_to_point_control as m
print(__file__)
import conftest # Add root path to sys.path
from ArmNavigation.two_joint_arm_to_point_control \
import two_joint_arm_to_point_control as m
class Test(TestCase):
def test1(self):
m.show_animation = False
m.animation()
def test1():
m.show_animation = False
m.animation()

13
tests/test_unscented_kalman_filter.py
View File

@@ -1,12 +1,7 @@
from unittest import TestCase
import conftest # Add root path to sys.path
from Localization.unscented_kalman_filter import unscented_kalman_filter as m
print(__file__)
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
def test1():
m.show_animation = False
m.main()

18
tests/test_visibility_road_map_planner.py
View File

@@ -1,17 +1,7 @@
import sys
import os
sys.path.append(os.path.dirname(os.path.abspath(__file__))
+ "/../PathPlanning/VoronoiRoadMap/")
from unittest import TestCase
import conftest # Add root path to sys.path
from PathPlanning.VoronoiRoadMap import voronoi_road_map as m
print(__file__)
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
def test1():
m.show_animation = False
m.main()

18
tests/test_voronoi_road_map_planner.py
View File

@@ -1,17 +1,7 @@
import os
import sys
sys.path.append(os.path.dirname(os.path.abspath(__file__))
+ "/../PathPlanning/VisibilityRoadMap/")
from unittest import TestCase
import conftest # Add root path to sys.path
from PathPlanning.VisibilityRoadMap import visibility_road_map as m
print(__file__)
class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
def test1():
m.show_animation = False
m.main()

93
tests/test_wavefront_coverage_path_planner.py
View File

@@ -1,64 +1,63 @@
import conftest # Add root path to sys.path
import os
import sys
import matplotlib.pyplot as plt
from unittest import TestCase
sys.path.append(os.path.dirname(
os.path.abspath(__file__)) + "/../PathPlanning/WavefrontCPP")
try:
import wavefront_coverage_path_planner
except ImportError:
raise
from PathPlanning.WavefrontCPP import wavefront_coverage_path_planner
wavefront_coverage_path_planner.do_animation = False
class TestPlanning(TestCase):
def wavefront_cpp(self, img, start, goal):
num_free = 0
for i in range(img.shape[0]):
for j in range(img.shape[1]):
num_free += 1 - img[i][j]
def wavefront_cpp(img, start, goal):
num_free = 0
for i in range(img.shape[0]):
for j in range(img.shape[1]):
num_free += 1 - img[i][j]
DT = wavefront_coverage_path_planner.transform(
img, goal, transform_type='distance')
DT_path = wavefront_coverage_path_planner.wavefront(DT, start, goal)
self.assertEqual(len(DT_path), num_free) # assert complete coverage
DT = wavefront_coverage_path_planner.transform(
img, goal, transform_type='distance')
DT_path = wavefront_coverage_path_planner.wavefront(DT, start, goal)
assert len(DT_path) == num_free # assert complete coverage
PT = wavefront_coverage_path_planner.transform(
img, goal, transform_type='path', alpha=0.01)
PT_path = wavefront_coverage_path_planner.wavefront(PT, start, goal)
self.assertEqual(len(PT_path), num_free) # assert complete coverage
PT = wavefront_coverage_path_planner.transform(
img, goal, transform_type='path', alpha=0.01)
PT_path = wavefront_coverage_path_planner.wavefront(PT, start, goal)
assert len(PT_path) == num_free # assert complete coverage
def test_wavefront_CPP_1(self):
img_dir = os.path.dirname(
os.path.abspath(__file__)) + "/../PathPlanning/WavefrontCPP"
img = plt.imread(os.path.join(img_dir, 'map', 'test.png'))
img = 1 - img
start = (43, 0)
goal = (0, 0)
def test_wavefront_CPP_1():
img_dir = os.path.dirname(
os.path.abspath(__file__)) + "/../PathPlanning/WavefrontCPP"
img = plt.imread(os.path.join(img_dir, 'map', 'test.png'))
img = 1 - img
self.wavefront_cpp(img, start, goal)
start = (43, 0)
goal = (0, 0)
def test_wavefront_CPP_2(self):
img_dir = os.path.dirname(
os.path.abspath(__file__)) + "/../PathPlanning/WavefrontCPP"
img = plt.imread(os.path.join(img_dir, 'map', 'test_2.png'))
img = 1 - img
wavefront_cpp(img, start, goal)
start = (10, 0)
goal = (10, 40)
self.wavefront_cpp(img, start, goal)
def test_wavefront_CPP_2():
img_dir = os.path.dirname(
os.path.abspath(__file__)) + "/../PathPlanning/WavefrontCPP"
img = plt.imread(os.path.join(img_dir, 'map', 'test_2.png'))
img = 1 - img
def test_wavefront_CPP_3(self):
img_dir = os.path.dirname(
os.path.abspath(__file__)) + "/../PathPlanning/WavefrontCPP"
img = plt.imread(os.path.join(img_dir, 'map', 'test_3.png'))
img = 1 - img
start = (10, 0)
goal = (10, 40)
start = (0, 0)
goal = (30, 30)
wavefront_cpp(img, start, goal)
self.wavefront_cpp(img, start, goal)
def test_wavefront_CPP_3():
img_dir = os.path.dirname(
os.path.abspath(__file__)) + "/../PathPlanning/WavefrontCPP"
img = plt.imread(os.path.join(img_dir, 'map', 'test_3.png'))
img = 1 - img
start = (0, 0)
goal = (30, 30)
wavefront_cpp(img, start, goal)
if __name__ == '__main__':
conftest.run_this_test(__file__)