diff --git a/src/simulations/mapping/binary_grid_map_construction/binary_grid_map_construction.py b/src/simulations/mapping/binary_grid_map_construction/binary_grid_map_construction.py index 30fd5791..215a1d7d 100644 --- a/src/simulations/mapping/binary_grid_map_construction/binary_grid_map_construction.py +++ b/src/simulations/mapping/binary_grid_map_construction/binary_grid_map_construction.py @@ -66,9 +66,9 @@ def main(): obst_list = ObstacleList() obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=15.0), length_m=10, width_m=8)) obst_list.add_obstacle(Obstacle(State(x_m=40.0, y_m=0.0), length_m=2, width_m=10)) - obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=-10.0, yaw_rad=np.rad2deg(45)), length_m=5, width_m=5)) - obst_list.add_obstacle(Obstacle(State(x_m=30.0, y_m=15.0, yaw_rad=np.rad2deg(10)), length_m=5, width_m=2)) - obst_list.add_obstacle(Obstacle(State(x_m=50.0, y_m=15.0, yaw_rad=np.rad2deg(15)), length_m=5, width_m=2)) + obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=-10.0, yaw_rad=np.deg2rad(45)), length_m=5, width_m=5)) + obst_list.add_obstacle(Obstacle(State(x_m=30.0, y_m=15.0, yaw_rad=np.deg2rad(10)), length_m=5, width_m=2)) + obst_list.add_obstacle(Obstacle(State(x_m=50.0, y_m=15.0, yaw_rad=np.deg2rad(15)), length_m=5, width_m=2)) obst_list.add_obstacle(Obstacle(State(x_m=25.0, y_m=0.0), length_m=2, width_m=2)) obst_list.add_obstacle(Obstacle(State(x_m=35.0, y_m=-15.0), length_m=7, width_m=2)) vis.add_object(obst_list) diff --git a/src/simulations/mapping/cost_grid_map_construction/cost_grid_map_construction.py b/src/simulations/mapping/cost_grid_map_construction/cost_grid_map_construction.py index d5e2c86d..1efcbf72 100644 --- a/src/simulations/mapping/cost_grid_map_construction/cost_grid_map_construction.py +++ b/src/simulations/mapping/cost_grid_map_construction/cost_grid_map_construction.py @@ -66,9 +66,9 @@ def main(): obst_list = ObstacleList() obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=15.0), length_m=10, width_m=8)) obst_list.add_obstacle(Obstacle(State(x_m=40.0, y_m=0.0), length_m=2, width_m=10)) - obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=-10.0, yaw_rad=np.rad2deg(45)), length_m=5, width_m=5)) - obst_list.add_obstacle(Obstacle(State(x_m=30.0, y_m=15.0, yaw_rad=np.rad2deg(10)), length_m=5, width_m=2)) - obst_list.add_obstacle(Obstacle(State(x_m=50.0, y_m=15.0, yaw_rad=np.rad2deg(15)), length_m=5, width_m=2)) + obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=-10.0, yaw_rad=np.deg2rad(45)), length_m=5, width_m=5)) + obst_list.add_obstacle(Obstacle(State(x_m=30.0, y_m=15.0, yaw_rad=np.deg2rad(10)), length_m=5, width_m=2)) + obst_list.add_obstacle(Obstacle(State(x_m=50.0, y_m=15.0, yaw_rad=np.deg2rad(15)), length_m=5, width_m=2)) obst_list.add_obstacle(Obstacle(State(x_m=25.0, y_m=0.0), length_m=2, width_m=2)) obst_list.add_obstacle(Obstacle(State(x_m=35.0, y_m=-15.0), length_m=7, width_m=2)) vis.add_object(obst_list) diff --git a/src/simulations/mapping/ndt_map_construction/ndt_map_construction.py b/src/simulations/mapping/ndt_map_construction/ndt_map_construction.py index a8902718..99ae7b63 100644 --- a/src/simulations/mapping/ndt_map_construction/ndt_map_construction.py +++ b/src/simulations/mapping/ndt_map_construction/ndt_map_construction.py @@ -66,9 +66,9 @@ def main(): obst_list = ObstacleList() obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=15.0), length_m=10, width_m=8)) obst_list.add_obstacle(Obstacle(State(x_m=40.0, y_m=0.0), length_m=2, width_m=10)) - obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=-10.0, yaw_rad=np.rad2deg(45)), length_m=5, width_m=5)) - obst_list.add_obstacle(Obstacle(State(x_m=30.0, y_m=15.0, yaw_rad=np.rad2deg(10)), length_m=5, width_m=2)) - obst_list.add_obstacle(Obstacle(State(x_m=50.0, y_m=15.0, yaw_rad=np.rad2deg(15)), length_m=5, width_m=2)) + obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=-10.0, yaw_rad=np.deg2rad(45)), length_m=5, width_m=5)) + obst_list.add_obstacle(Obstacle(State(x_m=30.0, y_m=15.0, yaw_rad=np.deg2rad(10)), length_m=5, width_m=2)) + obst_list.add_obstacle(Obstacle(State(x_m=50.0, y_m=15.0, yaw_rad=np.deg2rad(15)), length_m=5, width_m=2)) obst_list.add_obstacle(Obstacle(State(x_m=25.0, y_m=0.0), length_m=2, width_m=2)) obst_list.add_obstacle(Obstacle(State(x_m=35.0, y_m=-15.0), length_m=7, width_m=2)) vis.add_object(obst_list) diff --git a/src/simulations/mapping/potential_field_map_construction/potential_field_map_construction.py b/src/simulations/mapping/potential_field_map_construction/potential_field_map_construction.py index 7dce7a85..4af9fa6e 100644 --- a/src/simulations/mapping/potential_field_map_construction/potential_field_map_construction.py +++ b/src/simulations/mapping/potential_field_map_construction/potential_field_map_construction.py @@ -66,9 +66,9 @@ def main(): obst_list = ObstacleList() obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=15.0), length_m=10, width_m=8)) obst_list.add_obstacle(Obstacle(State(x_m=40.0, y_m=0.0), length_m=2, width_m=10)) - obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=-10.0, yaw_rad=np.rad2deg(45)), length_m=5, width_m=5)) - obst_list.add_obstacle(Obstacle(State(x_m=30.0, y_m=15.0, yaw_rad=np.rad2deg(10)), length_m=5, width_m=2)) - obst_list.add_obstacle(Obstacle(State(x_m=50.0, y_m=15.0, yaw_rad=np.rad2deg(15)), length_m=5, width_m=2)) + obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=-10.0, yaw_rad=np.deg2rad(45)), length_m=5, width_m=5)) + obst_list.add_obstacle(Obstacle(State(x_m=30.0, y_m=15.0, yaw_rad=np.deg2rad(10)), length_m=5, width_m=2)) + obst_list.add_obstacle(Obstacle(State(x_m=50.0, y_m=15.0, yaw_rad=np.deg2rad(15)), length_m=5, width_m=2)) obst_list.add_obstacle(Obstacle(State(x_m=25.0, y_m=0.0), length_m=2, width_m=2)) obst_list.add_obstacle(Obstacle(State(x_m=35.0, y_m=-15.0), length_m=7, width_m=2)) vis.add_object(obst_list) diff --git a/src/simulations/path_planning/astar_hybrid_path_planning/astar_hybrid_path_planning.py b/src/simulations/path_planning/astar_hybrid_path_planning/astar_hybrid_path_planning.py index 76da5a17..139d2259 100644 --- a/src/simulations/path_planning/astar_hybrid_path_planning/astar_hybrid_path_planning.py +++ b/src/simulations/path_planning/astar_hybrid_path_planning/astar_hybrid_path_planning.py @@ -66,9 +66,9 @@ def main(): obst_list = ObstacleList() obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=15.0), length_m=10, width_m=8)) obst_list.add_obstacle(Obstacle(State(x_m=40.0, y_m=0.0), length_m=2, width_m=10)) - obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=-10.0, yaw_rad=np.rad2deg(45)), length_m=5, width_m=5)) - obst_list.add_obstacle(Obstacle(State(x_m=30.0, y_m=15.0, yaw_rad=np.rad2deg(10)), length_m=5, width_m=2)) - obst_list.add_obstacle(Obstacle(State(x_m=50.0, y_m=15.0, yaw_rad=np.rad2deg(15)), length_m=5, width_m=2)) + obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=-10.0, yaw_rad=np.deg2rad(45)), length_m=5, width_m=5)) + obst_list.add_obstacle(Obstacle(State(x_m=30.0, y_m=15.0, yaw_rad=np.deg2rad(10)), length_m=5, width_m=2)) + obst_list.add_obstacle(Obstacle(State(x_m=50.0, y_m=15.0, yaw_rad=np.deg2rad(15)), length_m=5, width_m=2)) obst_list.add_obstacle(Obstacle(State(x_m=25.0, y_m=0.0), length_m=2, width_m=2)) obst_list.add_obstacle(Obstacle(State(x_m=35.0, y_m=-15.0), length_m=7, width_m=2)) diff --git a/src/simulations/path_planning/astar_path_planning/astar_path_planning.py b/src/simulations/path_planning/astar_path_planning/astar_path_planning.py index 52c34560..4690f0b4 100644 --- a/src/simulations/path_planning/astar_path_planning/astar_path_planning.py +++ b/src/simulations/path_planning/astar_path_planning/astar_path_planning.py @@ -66,9 +66,9 @@ def main(): obst_list = ObstacleList() obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=15.0), length_m=10, width_m=8)) obst_list.add_obstacle(Obstacle(State(x_m=40.0, y_m=0.0), length_m=2, width_m=10)) - # obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=-10.0, yaw_rad=np.rad2deg(45)), length_m=5, width_m=5)) - # obst_list.add_obstacle(Obstacle(State(x_m=30.0, y_m=15.0, yaw_rad=np.rad2deg(10)), length_m=5, width_m=2)) - # obst_list.add_obstacle(Obstacle(State(x_m=50.0, y_m=15.0, yaw_rad=np.rad2deg(15)), length_m=5, width_m=2)) + # obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=-10.0, yaw_rad=np.deg2rad(45)), length_m=5, width_m=5)) + # obst_list.add_obstacle(Obstacle(State(x_m=30.0, y_m=15.0, yaw_rad=np.deg2rad(10)), length_m=5, width_m=2)) + # obst_list.add_obstacle(Obstacle(State(x_m=50.0, y_m=15.0, yaw_rad=np.deg2rad(15)), length_m=5, width_m=2)) # obst_list.add_obstacle(Obstacle(State(x_m=25.0, y_m=0.0), length_m=2, width_m=2)) # obst_list.add_obstacle(Obstacle(State(x_m=35.0, y_m=-15.0), length_m=7, width_m=2)) diff --git a/src/simulations/path_planning/dijkstra_path_planning/dijkstra_path_planning.py b/src/simulations/path_planning/dijkstra_path_planning/dijkstra_path_planning.py index f80d1ae2..be51e853 100644 --- a/src/simulations/path_planning/dijkstra_path_planning/dijkstra_path_planning.py +++ b/src/simulations/path_planning/dijkstra_path_planning/dijkstra_path_planning.py @@ -66,9 +66,9 @@ def main(): obst_list = ObstacleList() obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=15.0), length_m=10, width_m=8)) obst_list.add_obstacle(Obstacle(State(x_m=40.0, y_m=0.0), length_m=2, width_m=10)) - # obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=-10.0, yaw_rad=np.rad2deg(45)), length_m=5, width_m=5)) - # obst_list.add_obstacle(Obstacle(State(x_m=30.0, y_m=15.0, yaw_rad=np.rad2deg(10)), length_m=5, width_m=2)) - # obst_list.add_obstacle(Obstacle(State(x_m=50.0, y_m=15.0, yaw_rad=np.rad2deg(15)), length_m=5, width_m=2)) + # obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=-10.0, yaw_rad=np.deg2rad(45)), length_m=5, width_m=5)) + # obst_list.add_obstacle(Obstacle(State(x_m=30.0, y_m=15.0, yaw_rad=np.deg2rad(10)), length_m=5, width_m=2)) + # obst_list.add_obstacle(Obstacle(State(x_m=50.0, y_m=15.0, yaw_rad=np.deg2rad(15)), length_m=5, width_m=2)) # obst_list.add_obstacle(Obstacle(State(x_m=25.0, y_m=0.0), length_m=2, width_m=2)) # obst_list.add_obstacle(Obstacle(State(x_m=35.0, y_m=-15.0), length_m=7, width_m=2)) diff --git a/src/simulations/path_planning/rrt_path_planning/rrt_path_planning.py b/src/simulations/path_planning/rrt_path_planning/rrt_path_planning.py index 8ddc1b13..bcc29a19 100644 --- a/src/simulations/path_planning/rrt_path_planning/rrt_path_planning.py +++ b/src/simulations/path_planning/rrt_path_planning/rrt_path_planning.py @@ -66,9 +66,9 @@ def main(): obst_list = ObstacleList() obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=15.0), length_m=10, width_m=8)) obst_list.add_obstacle(Obstacle(State(x_m=40.0, y_m=0.0), length_m=2, width_m=10)) - obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=-10.0, yaw_rad=np.rad2deg(45)), length_m=5, width_m=5)) - obst_list.add_obstacle(Obstacle(State(x_m=30.0, y_m=15.0, yaw_rad=np.rad2deg(10)), length_m=5, width_m=2)) - obst_list.add_obstacle(Obstacle(State(x_m=50.0, y_m=15.0, yaw_rad=np.rad2deg(15)), length_m=5, width_m=2)) + obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=-10.0, yaw_rad=np.deg2rad(45)), length_m=5, width_m=5)) + obst_list.add_obstacle(Obstacle(State(x_m=30.0, y_m=15.0, yaw_rad=np.deg2rad(10)), length_m=5, width_m=2)) + obst_list.add_obstacle(Obstacle(State(x_m=50.0, y_m=15.0, yaw_rad=np.deg2rad(15)), length_m=5, width_m=2)) obst_list.add_obstacle(Obstacle(State(x_m=25.0, y_m=0.0), length_m=2, width_m=2)) obst_list.add_obstacle(Obstacle(State(x_m=35.0, y_m=-15.0), length_m=7, width_m=2)) diff --git a/src/simulations/path_planning/rrt_star_bidirectional_path_planning/rrt_star_bidirectional_path_planning.py b/src/simulations/path_planning/rrt_star_bidirectional_path_planning/rrt_star_bidirectional_path_planning.py index 2b2403b7..9c294570 100644 --- a/src/simulations/path_planning/rrt_star_bidirectional_path_planning/rrt_star_bidirectional_path_planning.py +++ b/src/simulations/path_planning/rrt_star_bidirectional_path_planning/rrt_star_bidirectional_path_planning.py @@ -66,9 +66,9 @@ def main(): obst_list = ObstacleList() obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=15.0), length_m=10, width_m=8)) obst_list.add_obstacle(Obstacle(State(x_m=40.0, y_m=0.0), length_m=2, width_m=10)) - obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=-10.0, yaw_rad=np.rad2deg(45)), length_m=5, width_m=5)) - obst_list.add_obstacle(Obstacle(State(x_m=30.0, y_m=15.0, yaw_rad=np.rad2deg(10)), length_m=5, width_m=2)) - obst_list.add_obstacle(Obstacle(State(x_m=50.0, y_m=15.0, yaw_rad=np.rad2deg(15)), length_m=5, width_m=2)) + obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=-10.0, yaw_rad=np.deg2rad(45)), length_m=5, width_m=5)) + obst_list.add_obstacle(Obstacle(State(x_m=30.0, y_m=15.0, yaw_rad=np.deg2rad(10)), length_m=5, width_m=2)) + obst_list.add_obstacle(Obstacle(State(x_m=50.0, y_m=15.0, yaw_rad=np.deg2rad(15)), length_m=5, width_m=2)) obst_list.add_obstacle(Obstacle(State(x_m=25.0, y_m=0.0), length_m=2, width_m=2)) obst_list.add_obstacle(Obstacle(State(x_m=35.0, y_m=-15.0), length_m=7, width_m=2)) diff --git a/src/simulations/path_planning/rrt_star_path_planning/rrt_star_path_planning.py b/src/simulations/path_planning/rrt_star_path_planning/rrt_star_path_planning.py index 1391a53e..a8bc6043 100644 --- a/src/simulations/path_planning/rrt_star_path_planning/rrt_star_path_planning.py +++ b/src/simulations/path_planning/rrt_star_path_planning/rrt_star_path_planning.py @@ -66,9 +66,9 @@ def main(): obst_list = ObstacleList() obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=15.0), length_m=10, width_m=8)) obst_list.add_obstacle(Obstacle(State(x_m=40.0, y_m=0.0), length_m=2, width_m=10)) - obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=-10.0, yaw_rad=np.rad2deg(45)), length_m=5, width_m=5)) - obst_list.add_obstacle(Obstacle(State(x_m=30.0, y_m=15.0, yaw_rad=np.rad2deg(10)), length_m=5, width_m=2)) - obst_list.add_obstacle(Obstacle(State(x_m=50.0, y_m=15.0, yaw_rad=np.rad2deg(15)), length_m=5, width_m=2)) + obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=-10.0, yaw_rad=np.deg2rad(45)), length_m=5, width_m=5)) + obst_list.add_obstacle(Obstacle(State(x_m=30.0, y_m=15.0, yaw_rad=np.deg2rad(10)), length_m=5, width_m=2)) + obst_list.add_obstacle(Obstacle(State(x_m=50.0, y_m=15.0, yaw_rad=np.deg2rad(15)), length_m=5, width_m=2)) obst_list.add_obstacle(Obstacle(State(x_m=25.0, y_m=0.0), length_m=2, width_m=2)) obst_list.add_obstacle(Obstacle(State(x_m=35.0, y_m=-15.0), length_m=7, width_m=2)) diff --git a/test/test_obstacle_yaw_angle_units.py b/test/test_obstacle_yaw_angle_units.py new file mode 100644 index 00000000..96430cfb --- /dev/null +++ b/test/test_obstacle_yaw_angle_units.py @@ -0,0 +1,73 @@ +""" +Test that obstacle yaw angles are converted from degrees to radians + +Author: Dhyey Gosa +""" + +from pathlib import Path +import numpy as np +import pytest + +SIMULATION_DIR = Path(__file__).absolute().parent / "../src/simulations" + +# Obstacle yaw angles were written as yaw_rad=np.rad2deg(45), which converts +# 45 radians into about 2578 degrees instead of turning 45 degrees into radians. +# State.yaw_rad expects radians, so the conversion has to be np.deg2rad(). +SIMULATION_SCRIPTS = sorted(SIMULATION_DIR.rglob("*.py")) + + +def _script_with_yaw_angles(): + """ + Function to collect the simulation scripts that convert an angle into the yaw_rad argument + """ + # Only scripts that feed a numpy conversion into yaw_rad are of interest here. + # Plain literals such as yaw_rad=0.0 appear in every simulation and are correct. + return [script for script in SIMULATION_SCRIPTS + if "yaw_rad=np." in script.read_text(encoding="utf-8")] + + +def test_simulation_scripts_exist(): + """ + Test to check the simulation scripts were found, otherwise the checks below are meaningless + """ + assert len(_script_with_yaw_angles()) > 0 + + +def test_yaw_rad_is_never_built_with_rad2deg(): + """ + Test to check no simulation script converts with np.rad2deg() inside a yaw_rad argument + """ + offenders = [str(script.relative_to(SIMULATION_DIR)) for script in _script_with_yaw_angles() + if "yaw_rad=np.rad2deg" in script.read_text(encoding="utf-8")] + + assert offenders == [], f"yaw_rad expects radians, but np.rad2deg() was used in: {offenders}" + + +def test_yaw_rad_is_built_with_deg2rad(): + """ + Test to check the scripts that set an obstacle yaw angle use np.deg2rad() + """ + scripts = _script_with_yaw_angles() + + assert len(scripts) > 0 + for script in scripts: + assert "yaw_rad=np.deg2rad" in script.read_text(encoding="utf-8"), \ + f"{script.relative_to(SIMULATION_DIR)} sets yaw_rad but does not use np.deg2rad()" + + +@pytest.mark.parametrize("angle_deg", [10.0, 15.0, 45.0, 90.0, 180.0]) +def test_deg2rad_yaw_stays_in_radian_range(angle_deg): + """ + Test to check a degree input converted with np.deg2rad() stays inside the valid radian range + """ + yaw_rad = np.deg2rad(angle_deg) + + assert -np.pi <= yaw_rad <= np.pi + assert yaw_rad != np.rad2deg(angle_deg) + + +def test_rad2deg_would_be_out_of_range(): + """ + Test to check the old np.rad2deg() call really produced an out of range radian value + """ + assert not (-np.pi <= np.rad2deg(45.0) <= np.pi) \ No newline at end of file