Unify heading conversions

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):

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):

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

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
         )

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."""

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)

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
 
 

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)