diff --git a/ArmNavigation/n_joint_arm_3d/__init__py.py b/AerialNavigation/__init__.py similarity index 100% rename from ArmNavigation/n_joint_arm_3d/__init__py.py rename to AerialNavigation/__init__.py diff --git a/AerialNavigation/drone_3d_trajectory_following/__init__.py b/AerialNavigation/drone_3d_trajectory_following/__init__.py new file mode 100644 index 00000000000..087dab646e1 --- /dev/null +++ b/AerialNavigation/drone_3d_trajectory_following/__init__.py @@ -0,0 +1,4 @@ +import os +import sys + +sys.path.append(os.path.dirname(os.path.abspath(__file__))) diff --git a/ArmNavigation/n_joint_arm_3d/__init__.py b/ArmNavigation/n_joint_arm_3d/__init__.py new file mode 100644 index 00000000000..ba5dac65f66 --- /dev/null +++ b/ArmNavigation/n_joint_arm_3d/__init__.py @@ -0,0 +1,3 @@ +import sys +import os +sys.path.append(os.path.dirname(os.path.abspath(__file__))) diff --git a/ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py b/ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py index 3972474b8d5..b7834ee98bd 100644 --- a/ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py +++ b/ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py @@ -6,7 +6,7 @@ """ import numpy as np -from NLinkArm import NLinkArm +from ArmNavigation.n_joint_arm_to_point_control.NLinkArm import NLinkArm # Simulation parameters Kp = 2 diff --git a/InvertedPendulumCart/inverted_pendulum_mpc_control.py b/InvertedPendulumCart/InvertedPendulumMPCControl/inverted_pendulum_mpc_control.py similarity index 100% rename from InvertedPendulumCart/inverted_pendulum_mpc_control.py rename to InvertedPendulumCart/InvertedPendulumMPCControl/inverted_pendulum_mpc_control.py diff --git a/tests/.gitkeep b/InvertedPendulumCart/__init__.py similarity index 100% rename from tests/.gitkeep rename to InvertedPendulumCart/__init__.py diff --git a/LICENSE b/LICENSE index 9d60d1828f7..2867142fc71 100644 --- a/LICENSE +++ b/LICENSE @@ -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 diff --git a/Localization/__init__.py b/Localization/__init__.py new file mode 100644 index 00000000000..e69de29bb2d diff --git a/Mapping/__init__.py b/Mapping/__init__.py new file mode 100644 index 00000000000..e69de29bb2d diff --git a/Mapping/grid_map_lib/__init__.py b/Mapping/grid_map_lib/__init__.py new file mode 100644 index 00000000000..e69de29bb2d diff --git a/Mapping/rectangle_fitting/__init_.py b/Mapping/rectangle_fitting/__init_.py new file mode 100644 index 00000000000..087dab646e1 --- /dev/null +++ b/Mapping/rectangle_fitting/__init_.py @@ -0,0 +1,4 @@ +import os +import sys + +sys.path.append(os.path.dirname(os.path.abspath(__file__))) diff --git a/Mapping/rectangle_fitting/rectangle_fitting.py b/Mapping/rectangle_fitting/rectangle_fitting.py index a60a6fbfd18..078916f3b87 100644 --- a/Mapping/rectangle_fitting/rectangle_fitting.py +++ b/Mapping/rectangle_fitting/rectangle_fitting.py @@ -18,7 +18,8 @@ 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 diff --git a/PathPlanning/ClosedLoopRRTStar/__init__.py b/PathPlanning/ClosedLoopRRTStar/__init__.py index e69de29bb2d..0e49cd14d3d 100644 --- a/PathPlanning/ClosedLoopRRTStar/__init__.py +++ b/PathPlanning/ClosedLoopRRTStar/__init__.py @@ -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/") diff --git a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py index aea0080112c..740fc5590af 100644 --- a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py +++ b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py @@ -6,25 +6,13 @@ """ -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 @@ def search_best_feasible_path(self, path_indexs): 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") diff --git a/PathPlanning/GridBasedSweepCPP/__init__.py b/PathPlanning/GridBasedSweepCPP/__init__.py new file mode 100644 index 00000000000..f1c41ce4330 --- /dev/null +++ b/PathPlanning/GridBasedSweepCPP/__init__.py @@ -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) diff --git a/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py b/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py index 09df21c4c82..ca318ad5967 100644 --- a/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py +++ b/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py @@ -5,19 +5,12 @@ """ 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 diff --git a/PathPlanning/HybridAStar/__init__.py b/PathPlanning/HybridAStar/__init__.py index e69de29bb2d..087dab646e1 100644 --- a/PathPlanning/HybridAStar/__init__.py +++ b/PathPlanning/HybridAStar/__init__.py @@ -0,0 +1,4 @@ +import os +import sys + +sys.path.append(os.path.dirname(os.path.abspath(__file__))) diff --git a/PathPlanning/VoronoiRoadMap/__init__.py b/PathPlanning/VoronoiRoadMap/__init__.py new file mode 100644 index 00000000000..087dab646e1 --- /dev/null +++ b/PathPlanning/VoronoiRoadMap/__init__.py @@ -0,0 +1,4 @@ +import os +import sys + +sys.path.append(os.path.dirname(os.path.abspath(__file__))) diff --git a/PathTracking/__init__.py b/PathTracking/__init__.py new file mode 100644 index 00000000000..e69de29bb2d diff --git a/__init__.py b/__init__.py new file mode 100644 index 00000000000..e69de29bb2d diff --git a/runtests.sh b/runtests.sh index d9abc370a92..12d1b804534 100755 --- a/runtests.sh +++ b/runtests.sh @@ -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 diff --git a/tests/conftest.py b/tests/conftest.py new file mode 100644 index 00000000000..3485fe81502 --- /dev/null +++ b/tests/conftest.py @@ -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)]) diff --git a/tests/test_LQR_planner.py b/tests/test_LQR_planner.py index 2bcf828c387..b535b99baf8 100644 --- a/tests/test_LQR_planner.py +++ b/tests/test_LQR_planner.py @@ -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__) diff --git a/tests/test_a_star.py b/tests/test_a_star.py index 61276e6a313..82f76401ad2 100644 --- a/tests/test_a_star.py +++ b/tests/test_a_star.py @@ -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 test_1(): + m.show_animation = False + m.main() - 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__) diff --git a/tests/test_a_star_searching_two_side.py b/tests/test_a_star_searching_two_side.py index 013e9253b86..ad90ebc509a 100644 --- a/tests/test_a_star_searching_two_side.py +++ b/tests/test_a_star_searching_two_side.py @@ -1,27 +1,16 @@ -from unittest import TestCase -import os -import sys +import conftest +from PathPlanning.AStar import a_star_searching_from_two_side as m -sys.path.append(os.path.dirname(__file__) + '/../') -try: - from PathPlanning.AStar import a_star_searching_from_two_side as m -except ImportError: - raise +def test1(): + m.show_animation = False + m.main(800) -class Test(TestCase): - - 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__) diff --git a/tests/test_a_star_variants.py b/tests/test_a_star_variants.py index 13009c1365e..b9ef7914026 100644 --- a/tests/test_a_star_variants.py +++ b/tests/test_a_star_variants.py @@ -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__) + "/../") - - -class Test(TestCase): - - def test(self): - # A* with beam search - astar.show_animation = False - - astar.use_beam_search = True - astar.main() - self.reset_all() - - # A* with iterative deepening - astar.use_iterative_deepening = True - astar.main() - self.reset_all() - - # A* with dynamic weighting - astar.use_dynamic_weighting = True - astar.main() - self.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 - - -if __name__ == '__main__': # pragma: no cover - test = Test() - test.test() +import PathPlanning.AStar.a_star_variants as a_star +import conftest + + +def test_1(): + # A* with beam search + a_star.show_animation = False + + a_star.use_beam_search = True + a_star.main() + reset_all() + + # A* with iterative deepening + a_star.use_iterative_deepening = True + a_star.main() + reset_all() + + # A* with dynamic weighting + a_star.use_dynamic_weighting = True + a_star.main() + reset_all() + + # theta* + a_star.use_theta_star = True + a_star.main() + reset_all() + + # A* with jump point + a_star.use_jump_point = True + a_star.main() + reset_all() + + +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__) diff --git a/tests/test_batch_informed_rrt_star.py b/tests/test_batch_informed_rrt_star.py index bac8941460b..3ad78bdb231 100644 --- a/tests/test_batch_informed_rrt_star.py +++ b/tests/test_batch_informed_rrt_star.py @@ -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__) +import conftest +from PathPlanning.BatchInformedRRTStar import batch_informed_rrtstar as m -random.seed(12345) +def test_1(): + m.show_animation = False + random.seed(12345) + m.main(maxIter=10) -class Test(TestCase): - def test1(self): - m.show_animation = False - m.main(maxIter=10) - - -if __name__ == '__main__': # pragma: no cover - test = Test() - test.test1() +if __name__ == '__main__': + conftest.run_this_test(__file__) diff --git a/tests/test_bezier_path.py b/tests/test_bezier_path.py index 94ec8f5c31f..67a5d520de8 100644 --- a/tests/test_bezier_path.py +++ b/tests/test_bezier_path.py @@ -1,16 +1,16 @@ -from unittest import TestCase +import conftest +from PathPlanning.BezierPath import bezier_path as m -import sys -sys.path.append("./PathPlanning/BezierPath/") -from PathPlanning.BezierPath import bezier_path as m +def test_1(): + m.show_animation = False + m.main() -print(__file__) +def test_2(): + m.show_animation = False + m.main2() -class Test(TestCase): - def test1(self): - m.show_animation = False - m.main() - m.main2() +if __name__ == '__main__': + conftest.run_this_test(__file__) diff --git a/tests/test_bipedal_planner.py b/tests/test_bipedal_planner.py index 66049c68ec0..818957bcdcb 100644 --- a/tests/test_bipedal_planner.py +++ b/tests/test_bipedal_planner.py @@ -1,24 +1,18 @@ -from unittest import TestCase +import conftest +from Bipedal.bipedal_planner import bipedal_planner as m -import sys -sys.path.append("./Bipedal/bipedal_planner/") -try: - from Bipedal.bipedal_planner import bipedal_planner as m -except Exception: - raise -print(__file__) +def test_1(): + 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) -class Test(TestCase): - 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) +if __name__ == '__main__': + conftest.run_this_test(__file__) diff --git a/tests/test_breadth_first_search.py b/tests/test_breadth_first_search.py index 7dc36b2860a..14c1ac69882 100644 --- a/tests/test_breadth_first_search.py +++ b/tests/test_breadth_first_search.py @@ -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__) diff --git a/tests/test_bug.py b/tests/test_bug.py index 31d8b2c297d..f94794a45e4 100644 --- a/tests/test_bug.py +++ b/tests/test_bug.py @@ -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_1(): + m.show_animation = False + m.main(bug_0=True, bug_1=True, bug_2=True) - def test(self): - b.show_animation = False - b.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__) diff --git a/tests/test_cgmres_nmpc.py b/tests/test_cgmres_nmpc.py index 8420ca1a5a6..db3ada5065c 100644 --- a/tests/test_cgmres_nmpc.py +++ b/tests/test_cgmres_nmpc.py @@ -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__) diff --git a/tests/test_circle_fitting.py b/tests/test_circle_fitting.py index d184548227c..b3888d7cc31 100644 --- a/tests/test_circle_fitting.py +++ b/tests/test_circle_fitting.py @@ -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__) diff --git a/tests/test_closed_loop_rrt_star_car.py b/tests/test_closed_loop_rrt_star_car.py index 060389d6a2a..c33e413e926 100644 --- a/tests/test_closed_loop_rrt_star_car.py +++ b/tests/test_closed_loop_rrt_star_car.py @@ -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) +def test_1(): + random.seed(12345) + m.show_animation = False + m.main(gx=1.0, gy=0.0, gyaw=0.0, max_iter=5) -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) - - -if __name__ == '__main__': # pragma: no cover - test = Test() - test.test1() +if __name__ == '__main__': + conftest.run_this_test(__file__) diff --git a/tests/test_cubature_kalman_filter.py b/tests/test_cubature_kalman_filter.py index e6ac71a8eec..00f9d8bc5ff 100644 --- a/tests/test_cubature_kalman_filter.py +++ b/tests/test_cubature_kalman_filter.py @@ -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__) diff --git a/tests/test_depth_first_search.py b/tests/test_depth_first_search.py index 15aa7a0a53c..8dd009278f1 100644 --- a/tests/test_depth_first_search.py +++ b/tests/test_depth_first_search.py @@ -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__) diff --git a/tests/test_dijkstra.py b/tests/test_dijkstra.py index 7650eee8029..40404153d8b 100644 --- a/tests/test_dijkstra.py +++ b/tests/test_dijkstra.py @@ -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__) diff --git a/tests/test_drone_3d_trajectory_following.py b/tests/test_drone_3d_trajectory_following.py index dfadcdaee41..e3fd4170247 100644 --- a/tests/test_drone_3d_trajectory_following.py +++ b/tests/test_drone_3d_trajectory_following.py @@ -1,16 +1,12 @@ -import os -import sys -from unittest import TestCase +import conftest +from AerialNavigation.drone_3d_trajectory_following \ + import drone_3d_trajectory_following as m -sys.path.append(os.path.dirname(__file__) + "/../AerialNavigation/drone_3d_trajectory_following/") -import drone_3d_trajectory_following 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__) diff --git a/tests/test_dubins_path_planning.py b/tests/test_dubins_path_planning.py index 1be70e390ea..99fbd291002 100644 --- a/tests/test_dubins_path_planning.py +++ b/tests/test_dubins_path_planning.py @@ -1,60 +1,65 @@ -from unittest import TestCase - +import conftest import numpy as np +from PathPlanning.DubinsPath import dubins_path_planning + np.random.seed(12345) -from PathPlanning.DubinsPath import dubins_path_planning +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) -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 test_1(): + start_x = 1.0 # [m] + start_y = 1.0 # [m] + start_yaw = np.deg2rad(45.0) # [rad] - 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] - end_x = -3.0 # [m] - end_y = -3.0 # [m] - end_yaw = np.deg2rad(-45.0) # [rad] + curvature = 1.0 - 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) - 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) - self.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 test_2(): + 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] +def test_3(): + 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] + + 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) - curvature = 1.0 / (np.random.rand() * 5.0) + check_edge_condition(px, py, pyaw, + start_x, start_y, start_yaw, + end_x, end_y, end_yaw) - 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__) diff --git a/tests/test_dynamic_window_approach.py b/tests/test_dynamic_window_approach.py index 93deb6846dd..fdb452b4a7a 100644 --- a/tests/test_dynamic_window_approach.py +++ b/tests/test_dynamic_window_approach.py @@ -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__) - - -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) - - -if __name__ == '__main__': # pragma: no cover - test = TestDynamicWindowApproach() - test.test_main1() - test.test_main2() - test.test_stuck_main() + +from PathPlanning.DynamicWindowApproach import dynamic_window_approach as m + + +def test_main1(): + m.show_animation = False + m.main(gx=1.0, gy=1.0) + + +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__) diff --git a/tests/test_ekf_slam.py b/tests/test_ekf_slam.py index e651e0079c3..4181bb64bae 100644 --- a/tests/test_ekf_slam.py +++ b/tests/test_ekf_slam.py @@ -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__) diff --git a/tests/test_eta3_spline_path.py b/tests/test_eta3_spline_path.py index 33a4223d310..7fa3883ea59 100644 --- a/tests/test_eta3_spline_path.py +++ b/tests/test_eta3_spline_path.py @@ -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__) diff --git a/tests/test_extended_kalman_filter.py b/tests/test_extended_kalman_filter.py index 299ee71b0a8..d9ad6437a8e 100644 --- a/tests/test_extended_kalman_filter.py +++ b/tests/test_extended_kalman_filter.py @@ -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__) diff --git a/tests/test_fast_slam1.py b/tests/test_fast_slam1.py index 4b15e65c2d8..b72ab6b9efa 100644 --- a/tests/test_fast_slam1.py +++ b/tests/test_fast_slam1.py @@ -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__) diff --git a/tests/test_fast_slam2.py b/tests/test_fast_slam2.py index 4c1c967a41c..95cdc69d424 100644 --- a/tests/test_fast_slam2.py +++ b/tests/test_fast_slam2.py @@ -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__) diff --git a/tests/test_flow_field.py b/tests/test_flow_field.py index bf8fe6b8024..d049731fe59 100644 --- a/tests/test_flow_field.py +++ b/tests/test_flow_field.py @@ -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(): + flow_field.show_animation = False + flow_field.main() - def test(self): - flowfield.show_animation = False - flowfield.main() - -if __name__ == '__main__': # pragma: no cover - test = Test() - test.test() +if __name__ == '__main__': + conftest.run_this_test(__file__) diff --git a/tests/test_frenet_optimal_trajectory.py b/tests/test_frenet_optimal_trajectory.py index 7a0ba5179ec..72bb7a8341f 100644 --- a/tests/test_frenet_optimal_trajectory.py +++ b/tests/test_frenet_optimal_trajectory.py @@ -1,26 +1,12 @@ -from unittest import TestCase +import conftest +from PathPlanning.FrenetOptimalTrajectory import frenet_optimal_trajectory as m -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 +def test1(): + m.show_animation = False + m.SIM_LOOP = 5 + m.main() -print(__file__) - -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__) diff --git a/tests/test_gaussian_grid_map.py b/tests/test_gaussian_grid_map.py index d6a6cf53b58..af1e138721b 100644 --- a/tests/test_gaussian_grid_map.py +++ b/tests/test_gaussian_grid_map.py @@ -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__) diff --git a/tests/test_graph_based_slam.py b/tests/test_graph_based_slam.py index 89a6e14dfd3..67d75f0f858 100644 --- a/tests/test_graph_based_slam.py +++ b/tests/test_graph_based_slam.py @@ -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__) diff --git a/tests/test_grid_based_sweep_coverage_path_planner.py b/tests/test_grid_based_sweep_coverage_path_planner.py index 12ef98df39b..8cbe36eb497 100644 --- a/tests/test_grid_based_sweep_coverage_path_planner.py +++ b/tests/test_grid_based_sweep_coverage_path_planner.py @@ -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 - - -class TestPlanning(TestCase): - - 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 - - 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=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=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=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(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=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=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=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.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) - - 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=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.DOWN, - ) - self.assertGreater(len(px), 5) +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 + + +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 + + 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=LEFT, + sweeping_direction=DOWN, + ) + assert 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=RIGHT, + sweeping_direction=UP, + ) + assert 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 + + 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=LEFT, + sweeping_direction=DOWN, + ) + assert 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=RIGHT, + sweeping_direction=DOWN, + ) + assert 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=LEFT, + sweeping_direction=DOWN, + ) + assert 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=RIGHT, + sweeping_direction=DOWN, + ) + assert len(px) >= 5 + + +if __name__ == '__main__': + conftest.run_this_test(__file__) diff --git a/tests/test_grid_map_lib.py b/tests/test_grid_map_lib.py index 63b3cc86b75..ffdb72b1ae9 100644 --- a/tests/test_grid_map_lib.py +++ b/tests/test_grid_map_lib.py @@ -1,38 +1,25 @@ -import os -import sys -import unittest +from Mapping.grid_map_lib.grid_map_lib import GridMap -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../Mapping/grid_map_lib") -try: - from grid_map_lib import GridMap -except ImportError: - raise +def test_position_set(): + grid_map = GridMap(100, 120, 0.5, 10.0, -0.5) -class MyTestCase(unittest.TestCase): + 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) - 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) +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] - self.assertEqual(True, True) + grid_map = GridMap(600, 290, 0.7, 60.0, 30.5) - 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.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__) diff --git a/tests/test_histogram_filter.py b/tests/test_histogram_filter.py index 6d7df824e05..4474ead0979 100644 --- a/tests/test_histogram_filter.py +++ b/tests/test_histogram_filter.py @@ -1,25 +1,12 @@ -from unittest import TestCase +import conftest +from Localization.histogram_filter import histogram_filter as m -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 - -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__) diff --git a/tests/test_hybrid_a_star.py b/tests/test_hybrid_a_star.py index 8cf5c30ad67..dbcf3ba9f43 100644 --- a/tests/test_hybrid_a_star.py +++ b/tests/test_hybrid_a_star.py @@ -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(): + m.show_animation = False + m.main() - 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__) diff --git a/tests/test_informed_rrt_star.py b/tests/test_informed_rrt_star.py index a19f2465f2f..10974ecfcb5 100644 --- a/tests/test_informed_rrt_star.py +++ b/tests/test_informed_rrt_star.py @@ -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 +import conftest +from PathPlanning.InformedRRTStar import informed_rrt_star 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__) diff --git a/tests/test_inverted_pendulum_mpc_control.py b/tests/test_inverted_pendulum_mpc_control.py index bf6c8576e40..d88d4d7915c 100644 --- a/tests/test_inverted_pendulum_mpc_control.py +++ b/tests/test_inverted_pendulum_mpc_control.py @@ -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__) diff --git a/tests/test_iterative_closest_point.py b/tests/test_iterative_closest_point.py index 9c71688ec73..3f212f72986 100644 --- a/tests/test_iterative_closest_point.py +++ b/tests/test_iterative_closest_point.py @@ -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__) diff --git a/tests/test_kmeans_clustering.py b/tests/test_kmeans_clustering.py index b5b2cb7a605..5e39d64ae6e 100644 --- a/tests/test_kmeans_clustering.py +++ b/tests/test_kmeans_clustering.py @@ -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__) diff --git a/tests/test_lqr_rrt_star.py b/tests/test_lqr_rrt_star.py index 0b59475b3dd..444b4616b85 100644 --- a/tests/test_lqr_rrt_star.py +++ b/tests/test_lqr_rrt_star.py @@ -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__) diff --git a/tests/test_lqr_speed_steer_control.py b/tests/test_lqr_speed_steer_control.py index fb1a14af54c..9e921895ba7 100644 --- a/tests/test_lqr_speed_steer_control.py +++ b/tests/test_lqr_speed_steer_control.py @@ -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() diff --git a/tests/test_lqr_steer_control.py b/tests/test_lqr_steer_control.py index 4807afef6c0..24427a8ffdd 100644 --- a/tests/test_lqr_steer_control.py +++ b/tests/test_lqr_steer_control.py @@ -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__) diff --git a/tests/test_model_predictive_speed_and_steer_control.py b/tests/test_model_predictive_speed_and_steer_control.py index 4356fd40e26..27b6b79cf5d 100644 --- a/tests/test_model_predictive_speed_and_steer_control.py +++ b/tests/test_model_predictive_speed_and_steer_control.py @@ -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 +if 'cvxpy' in sys.modules: # pragma: no cover - print(__file__) + from PathTracking.model_predictive_speed_and_steer_control \ + import model_predictive_speed_and_steer_control as m - class Test(TestCase): + def test_1(): + m.show_animation = False + m.main() - def test1(self): - m.show_animation = False - m.main() - m.main2() + def test_2(): + m.show_animation = False + m.main2() diff --git a/tests/test_move_to_pose.py b/tests/test_move_to_pose.py index 0125792039b..6a5a4cf4fba 100644 --- a/tests/test_move_to_pose.py +++ b/tests/test_move_to_pose.py @@ -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() diff --git a/tests/test_n_joint_arm_to_point_control.py b/tests/test_n_joint_arm_to_point_control.py index deb30f83be6..a87271b4717 100644 --- a/tests/test_n_joint_arm_to_point_control.py +++ b/tests/test_n_joint_arm_to_point_control.py @@ -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() diff --git a/tests/test_particle_filter.py b/tests/test_particle_filter.py index 3e655039760..dad823ee9e5 100644 --- a/tests/test_particle_filter.py +++ b/tests/test_particle_filter.py @@ -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() diff --git a/tests/test_potential_field_planning.py b/tests/test_potential_field_planning.py index 58b960363e9..c473a23968b 100644 --- a/tests/test_potential_field_planning.py +++ b/tests/test_potential_field_planning.py @@ -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() diff --git a/tests/test_probabilistic_road_map.py b/tests/test_probabilistic_road_map.py index 7b2f1fc5579..aa58e28bce3 100644 --- a/tests/test_probabilistic_road_map.py +++ b/tests/test_probabilistic_road_map.py @@ -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__) diff --git a/tests/test_pure_pursuit.py b/tests/test_pure_pursuit.py index 8803bba458b..2f0b83ae36d 100644 --- a/tests/test_pure_pursuit.py +++ b/tests/test_pure_pursuit.py @@ -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() diff --git a/tests/test_quintic_polynomials_planner.py b/tests/test_quintic_polynomials_planner.py index 0f8d781071c..bee383a7eaa 100644 --- a/tests/test_quintic_polynomials_planner.py +++ b/tests/test_quintic_polynomials_planner.py @@ -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() diff --git a/tests/test_raycasting_grid_map.py b/tests/test_raycasting_grid_map.py index 3c77096bd0b..60ecf769021 100644 --- a/tests/test_raycasting_grid_map.py +++ b/tests/test_raycasting_grid_map.py @@ -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() diff --git a/tests/test_rear_wheel_feedback.py b/tests/test_rear_wheel_feedback.py index 7fba28bd6d3..895eb188b32 100644 --- a/tests/test_rear_wheel_feedback.py +++ b/tests/test_rear_wheel_feedback.py @@ -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__) diff --git a/tests/test_rectangle_fitting.py b/tests/test_rectangle_fitting.py index 2a283206260..20dc9cafbff 100644 --- a/tests/test_rectangle_fitting.py +++ b/tests/test_rectangle_fitting.py @@ -1,22 +1,7 @@ -from unittest import TestCase +import conftest # Add root path to sys.path +from Mapping.rectangle_fitting import rectangle_fitting as m -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 - - -print(__file__) - - -class Test(TestCase): - - def test1(self): - m.show_animation = False - m.main() +def test1(): + m.show_animation = False + m.main() diff --git a/tests/test_reeds_shepp_path_planning.py b/tests/test_reeds_shepp_path_planning.py index 2c3b38dc31f..150f84fb35b 100644 --- a/tests/test_reeds_shepp_path_planning.py +++ b/tests/test_reeds_shepp_path_planning.py @@ -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(): + m.show_animation = False + m.main() - def test1(self): - m.show_animation = False - m.main() - def test2(self): - N_TEST = 10 +def test2(): + 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] + 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] + 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) + curvature = 1.0 / (np.random.rand() * 5.0) - px, py, pyaw, mode, clen = m.reeds_shepp_path_planning( - start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature) + px, py, pyaw, mode, clen = m.reeds_shepp_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) diff --git a/tests/test_rocket_powered_landing.py b/tests/test_rocket_powered_landing.py index 96cebbfe96a..01d2f4f832c 100644 --- a/tests/test_rocket_powered_landing.py +++ b/tests/test_rocket_powered_landing.py @@ -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() diff --git a/tests/test_rrt.py b/tests/test_rrt.py index b7c03d25cfd..14a81759315 100644 --- a/tests/test_rrt.py +++ b/tests/test_rrt.py @@ -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(): + m.show_animation = False + m.main(gx=1.0, gy=1.0) - 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 test2(): + m1.show_animation = False + m1.main() -if __name__ == '__main__': # pragma: no cover - test = Test() - test.test1() - test.test2() +if __name__ == '__main__': + conftest.run_this_test(__file__) diff --git a/tests/test_rrt_dubins.py b/tests/test_rrt_dubins.py index 19983252efa..66130484bc1 100644 --- a/tests/test_rrt_dubins.py +++ b/tests/test_rrt_dubins.py @@ -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/") +import conftest # Add root path to sys.path +from PathPlanning.RRTDubins import rrt_dubins as m -try: - import rrt_dubins as m -except: - raise +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__': # pragma: no cover - test = Test() - test.test1() +if __name__ == '__main__': + conftest.run_this_test(__file__) diff --git a/tests/test_rrt_star.py b/tests/test_rrt_star.py index d3661610ff5..5327f4ca206 100644 --- a/tests/test_rrt_star.py +++ b/tests/test_rrt_star.py @@ -1,37 +1,23 @@ -import os -import sys -from unittest import TestCase +import conftest # Add root path to sys.path +from PathPlanning.RRTStar import rrt_star as m -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../PathPlanning/RRTStar/") -try: - import rrt_star as m -except ImportError: - raise +def test1(): + m.show_animation = False + m.main() -print(__file__) +def test_no_obstacle(): + 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 -class Test(TestCase): - def test1(self): - m.show_animation = False - m.main() - - 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__) diff --git a/tests/test_rrt_star_dubins.py b/tests/test_rrt_star_dubins.py index db1ca2bfa48..c55ca23e43d 100644 --- a/tests/test_rrt_star_dubins.py +++ b/tests/test_rrt_star_dubins.py @@ -1,26 +1,11 @@ -from unittest import TestCase +import conftest # Add root path to sys.path +from PathPlanning.RRTStarDubins import rrt_star_dubins as m -import sys -import os -sys.path.append(os.path.dirname(os.path.abspath(__file__)) - + "/../PathPlanning/RRTStarDubins/") +def test1(): + m.show_animation = False + m.main() -try: - import rrt_star_dubins as m -except: - raise -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__) diff --git a/tests/test_rrt_star_reeds_shepp.py b/tests/test_rrt_star_reeds_shepp.py index f7eeae3e6bf..0a4663056ac 100644 --- a/tests/test_rrt_star_reeds_shepp.py +++ b/tests/test_rrt_star_reeds_shepp.py @@ -1,28 +1,11 @@ -import os -import sys -from unittest import TestCase +import conftest # Add root path to sys.path +import rrt_star_reeds_shepp as m -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 +def test1(): + m.show_animation = False + m.main(max_iter=5) -print(__file__) - - -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__) diff --git a/tests/test_rrt_star_seven_joint_arm.py b/tests/test_rrt_star_seven_joint_arm.py index 0788b3675ad..2f6622cf6cf 100644 --- a/tests/test_rrt_star_seven_joint_arm.py +++ b/tests/test_rrt_star_seven_joint_arm.py @@ -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__) - -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__) diff --git a/tests/test_rrt_with_sobol_sampler.py b/tests/test_rrt_with_sobol_sampler.py index 43f25e85bce..affe2b165a6 100644 --- a/tests/test_rrt_with_sobol_sampler.py +++ b/tests/test_rrt_with_sobol_sampler.py @@ -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__) diff --git a/tests/test_spiral_spanning_tree_coverage_path_planner.py b/tests/test_spiral_spanning_tree_coverage_path_planner.py index 622b6ba18c8..44170fa4cc3 100644 --- a/tests/test_spiral_spanning_tree_coverage_path_planner.py +++ b/tests/test_spiral_spanning_tree_coverage_path_planner.py @@ -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] - - STC_planner = spiral_spanning_tree_coverage_path_planner.\ - SpiralSpanningTreeCoveragePlanner(img) - - edge, route, path = STC_planner.plan(start) - - 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) - - 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_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 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) + + edge, route, path = STC_planner.plan(start) + + covered_nodes = set() + for p, q in edge: + covered_nodes.add(p) + covered_nodes.add(q) + + # assert complete coverage + assert len(covered_nodes) == num_free / 4 + + +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_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__) diff --git a/tests/test_stanley_controller.py b/tests/test_stanley_controller.py index 5aa24acb972..7920273db46 100644 --- a/tests/test_stanley_controller.py +++ b/tests/test_stanley_controller.py @@ -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() diff --git a/tests/test_state_lattice_planner.py b/tests/test_state_lattice_planner.py index 9bf90952826..9e9e40c8928 100644 --- a/tests/test_state_lattice_planner.py +++ b/tests/test_state_lattice_planner.py @@ -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() diff --git a/tests/test_two_joint_arm_to_point_control.py b/tests/test_two_joint_arm_to_point_control.py index e2b9c21ac39..24cc26f118a 100644 --- a/tests/test_two_joint_arm_to_point_control.py +++ b/tests/test_two_joint_arm_to_point_control.py @@ -1,12 +1,8 @@ -from unittest import TestCase +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 -from ArmNavigation.two_joint_arm_to_point_control import two_joint_arm_to_point_control as m -print(__file__) - - -class Test(TestCase): - - def test1(self): - m.show_animation = False - m.animation() +def test1(): + m.show_animation = False + m.animation() diff --git a/tests/test_unscented_kalman_filter.py b/tests/test_unscented_kalman_filter.py index 8788043e8a6..c4f79e8906a 100644 --- a/tests/test_unscented_kalman_filter.py +++ b/tests/test_unscented_kalman_filter.py @@ -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() diff --git a/tests/test_visibility_road_map_planner.py b/tests/test_visibility_road_map_planner.py index 5295769b409..bd758da5f4c 100644 --- a/tests/test_visibility_road_map_planner.py +++ b/tests/test_visibility_road_map_planner.py @@ -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() diff --git a/tests/test_voronoi_road_map_planner.py b/tests/test_voronoi_road_map_planner.py index a1f860a108e..51fe38ea33b 100644 --- a/tests/test_voronoi_road_map_planner.py +++ b/tests/test_voronoi_road_map_planner.py @@ -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() diff --git a/tests/test_wavefront_coverage_path_planner.py b/tests/test_wavefront_coverage_path_planner.py index a72818c2593..28bb5987e7f 100644 --- a/tests/test_wavefront_coverage_path_planner.py +++ b/tests/test_wavefront_coverage_path_planner.py @@ -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) + 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) + assert len(PT_path) == num_free # assert complete coverage + + +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 + + start = (43, 0) + goal = (0, 0) - 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 + wavefront_cpp(img, start, goal) - 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 - 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 +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 - start = (43, 0) - goal = (0, 0) + start = (10, 0) + goal = (10, 40) - self.wavefront_cpp(img, start, goal) + wavefront_cpp(img, start, goal) - 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 - start = (10, 0) - goal = (10, 40) +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 - self.wavefront_cpp(img, start, goal) + start = (0, 0) + goal = (30, 30) - 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 + wavefront_cpp(img, start, goal) - start = (0, 0) - goal = (30, 30) - self.wavefront_cpp(img, start, goal) +if __name__ == '__main__': + conftest.run_this_test(__file__)