Skip to content

fix: use np.deg2rad for obstacle yaw angles instead of np.rad2deg #149

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
dhyey-gosa wants to merge 1 commit into from
+103 −30
Closed

fix: use np.deg2rad for obstacle yaw angles instead of np.rad2deg #149

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 3 additions & 3 deletions src/simulations/mapping/binary_grid_map_construction/binary_grid_map_construction.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
6 changes: 3 additions & 3 deletions src/simulations/mapping/cost_grid_map_construction/cost_grid_map_construction.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
6 changes: 3 additions & 3 deletions src/simulations/mapping/ndt_map_construction/ndt_map_construction.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
6 changes: 3 additions & 3 deletions src/simulations/mapping/potential_field_map_construction/potential_field_map_construction.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
6 changes: 3 additions & 3 deletions src/simulations/path_planning/astar_hybrid_path_planning/astar_hybrid_path_planning.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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))

Expand Down
6 changes: 3 additions & 3 deletions src/simulations/path_planning/astar_path_planning/astar_path_planning.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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))

Expand Down
6 changes: 3 additions & 3 deletions src/simulations/path_planning/dijkstra_path_planning/dijkstra_path_planning.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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))

Expand Down
6 changes: 3 additions & 3 deletions src/simulations/path_planning/rrt_path_planning/rrt_path_planning.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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))

Expand Down
6 changes: 3 additions & 3 deletions ...ath_planning/rrt_star_bidirectional_path_planning/rrt_star_bidirectional_path_planning.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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))

Expand Down
6 changes: 3 additions & 3 deletions src/simulations/path_planning/rrt_star_path_planning/rrt_star_path_planning.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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))

Expand Down
73 changes: 73 additions & 0 deletions test/test_obstacle_yaw_angle_units.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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)