From 36f170e1573edeb9c52c2e3edaa5c368555edde2 Mon Sep 17 00:00:00 2001 From: DHYEY-GOSA Date: Sat, 10 Oct 2026 16:41:27 +0530 Subject: [PATCH] fix: use np.deg2rad for obstacle yaw angles instead of np.rad2deg State.yaw_rad expects radians, but the obstacle definitions passed np.rad2deg(45), which converts 45 radians into about 2578 degrees. The result is an angle far outside the valid radian range, so the obstacles are rotated by hundreds of full turns instead of 45 degrees. Replaced the call with np.deg2rad in the 10 affected simulation scripts, including the two commented out blocks, so that no yaw_rad=np.rad2deg remains. Added test/test_obstacle_yaw_angle_units.py as a regression test, since a defect fix is expected to come with a unit test. --- .../binary_grid_map_construction.py | 6 +- .../cost_grid_map_construction.py | 6 +- .../ndt_map_construction.py | 6 +- .../potential_field_map_construction.py | 6 +- .../astar_hybrid_path_planning.py | 6 +- .../astar_path_planning.py | 6 +- .../dijkstra_path_planning.py | 6 +- .../rrt_path_planning/rrt_path_planning.py | 6 +- .../rrt_star_bidirectional_path_planning.py | 6 +- .../rrt_star_path_planning.py | 6 +- test/test_obstacle_yaw_angle_units.py | 73 +++++++++++++++++++ 11 files changed, 103 insertions(+), 30 deletions(-) create mode 100644 test/test_obstacle_yaw_angle_units.py 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