diff --git a/src/scenic/core/geometry.py b/src/scenic/core/geometry.py index b60a68b2d..111f3f12e 100644 --- a/src/scenic/core/geometry.py +++ b/src/scenic/core/geometry.py @@ -44,12 +44,8 @@ def min(*args, **kwargs): @distributionFunction def normalizeAngle(angle) -> float: - while angle > math.pi: - angle -= math.tau - while angle < -math.pi: - angle += math.tau - assert -math.pi <= angle <= math.pi - return angle + """Normalize *angle* (radians) to the half-open interval [−π, π).""" + return (angle + math.pi) % math.tau - math.pi def averageVectors(a, b, weight=0.5): diff --git a/src/scenic/simulators/gta/interface.py b/src/scenic/simulators/gta/interface.py index 6bee5d842..46e2c1b0d 100644 --- a/src/scenic/simulators/gta/interface.py +++ b/src/scenic/simulators/gta/interface.py @@ -8,6 +8,11 @@ import numpy import scipy.spatial +from scenic.simulators.utils.coordinates import ( + rep103ToScenicHeading, + scenicToRep103Heading, +) + try: import PIL except ModuleNotFoundError as e: @@ -187,14 +192,14 @@ def gridToScenicCoords(self, point): return ((self.Ax * x) + self.Bx, (self.Ay * y) + self.By) def gridToScenicHeading(self, heading): - return heading - (math.pi / 2) + return rep103ToScenicHeading(heading) def scenicToGridCoords(self, point): x, y = point[0], point[1] return ((x - self.Bx) / self.Ax, (y - self.By) / self.Ay) def scenicToGridHeading(self, heading): - return heading + (math.pi / 2) + return scenicToRep103Heading(heading) @distributionMethod def roadHeadingAt(self, point): diff --git a/src/scenic/simulators/metadrive/model.scenic b/src/scenic/simulators/metadrive/model.scenic index 078c6fc33..b24ea126a 100644 --- a/src/scenic/simulators/metadrive/model.scenic +++ b/src/scenic/simulators/metadrive/model.scenic @@ -44,7 +44,8 @@ from scenic.simulators.metadrive.sensors import MetaDriveSSSensor as SSSensor try: from scenic.simulators.metadrive.simulator import MetaDriveSimulator - from scenic.simulators.metadrive.utils import scenicToMetaDriveHeading, scenicToMetaDrivePosition + from scenic.simulators.metadrive.utils import scenicToMetaDrivePosition + from scenic.simulators.utils.coordinates import scenicToRep103Heading except ModuleNotFoundError: # for convenience when testing without the metadrive package from scenic.core.simulators import SimulatorInterfaceWarning @@ -184,7 +185,7 @@ class Pedestrian(Pedestrian, MetaDriveActor, Walks): return True def setWalkingDirection(self, heading): - self._walking_direction = scenicToMetaDriveHeading(heading) + self._walking_direction = scenicToRep103Heading(heading) def setWalkingSpeed(self, speed): self._walking_speed = speed diff --git a/src/scenic/simulators/metadrive/simulator.py b/src/scenic/simulators/metadrive/simulator.py index 76607d5bc..9c1af7389 100644 --- a/src/scenic/simulators/metadrive/simulator.py +++ b/src/scenic/simulators/metadrive/simulator.py @@ -27,6 +27,10 @@ from scenic.domains.driving.simulators import DrivingSimulation, DrivingSimulator from scenic.simulators.metadrive.sensors import MetaDriveRGBSensor, MetaDriveSSSensor import scenic.simulators.metadrive.utils as utils +from scenic.simulators.utils.coordinates import ( + rep103ToScenicHeading, + scenicToRep103Heading, +) class MetaDriveSimulator(DrivingSimulator): @@ -195,7 +199,7 @@ def createObjectInSimulator(self, obj): converted_position = utils.scenicToMetaDrivePosition( obj.position, self.scenic_offset ) - converted_heading = utils.scenicToMetaDriveHeading(obj.heading) + converted_heading = scenicToRep103Heading(obj.heading) vehicle_config = {} if obj.isVehicle: @@ -283,7 +287,7 @@ def executeActions(self, allActions): else: # For Pedestrians if obj._walking_direction is None: - obj._walking_direction = utils.scenicToMetaDriveHeading(obj.heading) + obj._walking_direction = scenicToRep103Heading(obj.heading) if obj._walking_speed is None: obj._walking_speed = obj.speed direction = [ @@ -351,7 +355,7 @@ def getProperties(self, obj, properties): md_ang_vel = metaDriveActor.body.getAngularVelocity() angularVelocity = Vector(*md_ang_vel) angularSpeed = math.hypot(*md_ang_vel) - converted_heading = utils.metaDriveToScenicHeading(metaDriveActor.heading_theta) + converted_heading = rep103ToScenicHeading(metaDriveActor.heading_theta) yaw, pitch, roll = obj.parentOrientation.globalToLocalAngles( converted_heading, 0, 0 ) diff --git a/src/scenic/simulators/metadrive/utils.py b/src/scenic/simulators/metadrive/utils.py index 6891295f5..d902a276a 100644 --- a/src/scenic/simulators/metadrive/utils.py +++ b/src/scenic/simulators/metadrive/utils.py @@ -99,25 +99,6 @@ def scenicToMetaDrivePosition(vec, scenic_offset): return adjusted_x, adjusted_y -def scenicToMetaDriveHeading(scenicHeading): - """ - Converts Scenic heading to MetaDrive heading by adding π/2 (90 degrees). - - Scenic's coordinate system has 0 radians pointing North, while MetaDrive uses - 0 radians pointing East. This function shifts the heading to align with MetaDrive's system. - """ - metadriveHeading = scenicHeading + (math.pi / 2) - # Normalize to [-π, π] - return (metadriveHeading + math.pi) % (2 * math.pi) - math.pi - - -def metaDriveToScenicHeading(metaDriveHeading): - """Converts MetaDrive heading to Scenic heading by subtracting π/2 (90 degrees).""" - scenicHeading = metaDriveHeading - (math.pi / 2) - # Normalize to [-π, π] - return (scenicHeading + math.pi) % (2 * math.pi) - math.pi - - class DriveEnv(BaseEnv): def reward_function(self, agent): """Dummy reward function.""" diff --git a/src/scenic/simulators/utils/coordinates.py b/src/scenic/simulators/utils/coordinates.py new file mode 100644 index 000000000..b71b35fc5 --- /dev/null +++ b/src/scenic/simulators/utils/coordinates.py @@ -0,0 +1,26 @@ +"""Coordinate-convention utilities shared across simulator adapters. + +Scenic headings are measured CCW from North: +0 = North, π/2 = West, -π/2 = East. +ROS2 REP-103 yaw (ENU frame) is measured CCW from East: +0 = East, π/2 = North. + +MetaDrive and GTA's internal grid both use the same East-zero convention as REP-103. + +Conversion: scenic_heading = rep103_yaw - π/2 + rep103_yaw = scenic_heading + π/2 +""" + +import math + +from scenic.core.geometry import normalizeAngle + + +def rep103ToScenicHeading(yaw: float) -> float: + """ROS2 REP-103 yaw (CCW from East, radians) → Scenic heading (CCW from North, radians).""" + return normalizeAngle(yaw - math.pi / 2) + + +def scenicToRep103Heading(heading: float) -> float: + """Scenic heading (CCW from North, radians) → ROS2 REP-103 yaw (CCW from East, radians).""" + return normalizeAngle(heading + math.pi / 2) diff --git a/tests/core/test_geometry.py b/tests/core/test_geometry.py index 203020fe4..f551b4796 100644 --- a/tests/core/test_geometry.py +++ b/tests/core/test_geometry.py @@ -6,9 +6,46 @@ import trimesh import scenic.core.geometry as geometry +from scenic.core.geometry import normalizeAngle from scenic.core.object_types import Object from scenic.core.shapes import ConeShape + +## normalizeAngle + + +def test_normalizeAngle_zero(): + assert normalizeAngle(0) == 0.0 + + +def test_normalizeAngle_in_range(): + assert normalizeAngle(math.pi / 4) == pytest.approx(math.pi / 4) + assert normalizeAngle(-math.pi / 3) == pytest.approx(-math.pi / 3) + + +def test_normalizeAngle_positive_overflow(): + assert normalizeAngle(3 * math.pi / 2) == pytest.approx(-math.pi / 2) + assert normalizeAngle(2 * math.pi) == pytest.approx(0.0) + assert normalizeAngle(3 * math.pi) == pytest.approx(-math.pi) + + +def test_normalizeAngle_negative_overflow(): + assert normalizeAngle(-3 * math.pi / 2) == pytest.approx(math.pi / 2) + assert normalizeAngle(-2 * math.pi) == pytest.approx(0.0) + assert normalizeAngle(-3 * math.pi) == pytest.approx(-math.pi) + + +def test_normalizeAngle_boundary(): + assert normalizeAngle(math.pi) == pytest.approx(-math.pi) + assert normalizeAngle(-math.pi) == pytest.approx(-math.pi) + + +def test_normalizeAngle_large_multiple(): + for n in range(1, 6): + assert normalizeAngle(2 * n * math.pi) == pytest.approx(0.0) + assert normalizeAngle(-2 * n * math.pi) == pytest.approx(0.0) + + ## Triangulation diff --git a/tests/simulators/utils/test_coordinates.py b/tests/simulators/utils/test_coordinates.py new file mode 100644 index 000000000..ba5b19356 --- /dev/null +++ b/tests/simulators/utils/test_coordinates.py @@ -0,0 +1,72 @@ +import math + +import pytest + +from scenic.simulators.utils.coordinates import rep103ToScenicHeading, scenicToRep103Heading + + +## rep103ToScenicHeading: REP-103 yaw (CCW from East) -> Scenic heading (CCW from North) + + +def test_rep103_north(): + # REP-103 yaw = π/2 (pointing North) -> Scenic heading = 0 + assert rep103ToScenicHeading(math.pi / 2) == pytest.approx(0.0) + + +def test_rep103_east(): + # REP-103 yaw = 0 (pointing East) -> Scenic heading = -π/2 + assert rep103ToScenicHeading(0.0) == pytest.approx(-math.pi / 2) + + +def test_rep103_west(): + # REP-103 yaw = π (pointing West) -> Scenic heading = π/2 + assert rep103ToScenicHeading(math.pi) == pytest.approx(-math.pi / 2 + math.pi) + + +def test_rep103_south(): + # REP-103 yaw = -π/2 (pointing South) -> Scenic heading = -π (or equivalently π) + result = rep103ToScenicHeading(-math.pi / 2) + assert abs(abs(result) - math.pi) < 1e-10 + + +def test_rep103_output_normalized(): + # Output is always in [-π, π) + for yaw in [0, math.pi / 3, math.pi, -math.pi, 3 * math.pi, -3 * math.pi / 2]: + h = rep103ToScenicHeading(yaw) + assert -math.pi <= h < math.pi, f"yaw={yaw}: heading {h} out of range" + + +## scenicToRep103Heading: Scenic heading (CCW from North) -> REP-103 yaw (CCW from East) + + +def test_scenic_north(): + # Scenic heading = 0 (pointing North) -> REP-103 yaw = π/2 + assert scenicToRep103Heading(0.0) == pytest.approx(math.pi / 2) + + +def test_scenic_east(): + # Scenic heading = -π/2 (pointing East) -> REP-103 yaw = 0 + assert scenicToRep103Heading(-math.pi / 2) == pytest.approx(0.0) + + +def test_scenic_west(): + # Scenic heading = π/2 (pointing West) -> REP-103 yaw = π (or -π) + result = scenicToRep103Heading(math.pi / 2) + assert abs(abs(result) - math.pi) < 1e-10 + + +def test_scenic_output_normalized(): + for h in [0, math.pi / 3, math.pi / 2, -math.pi / 2, 3 * math.pi, -3 * math.pi]: + y = scenicToRep103Heading(h) + assert -math.pi <= y < math.pi, f"heading={h}: yaw {y} out of range" + + +## Round-trip + + +@pytest.mark.parametrize("heading", [ + 0, math.pi / 6, math.pi / 4, math.pi / 2, + -math.pi / 3, -math.pi / 2, math.pi * 0.9, +]) +def test_round_trip(heading): + assert rep103ToScenicHeading(scenicToRep103Heading(heading)) == pytest.approx(heading)