Transformation functions + tests

code/mapping/ext_modules/mapping_common/entity.py

@@ -33,7 +33,8 @@ class Motion2D:
     angular_velocity: float = 0.0
     """Angular velocity in radians/s
 
-    TODO: define if is cw or ccw depending on sign
+    - angle > 0: CCW
+    - angle < 0: CW
     """
 
     @staticmethod

code/mapping/ext_modules/mapping_common/shape.py

@@ -65,13 +65,12 @@ def to_marker(self, transform: Transform2D) -> Marker:
         m.pose.position.x = transl.x()
         m.pose.position.y = transl.y()
         m.pose.position.z = 0.0
-        # TODO: apply rotation to marker
         (
             m.pose.orientation.x,
             m.pose.orientation.y,
             m.pose.orientation.z,
             m.pose.orientation.w,
-        ) = quaternion_from_euler(0, 0, 0)
+        ) = quaternion_from_euler(0, 0, shape_transform.rotation())
 
         m.scale.z = 1.0
         return m

code/mapping/ext_modules/mapping_common/transform.pxd

@@ -4,17 +4,23 @@ cdef class _Coord2():
     cdef readonly np.ndarray _matrix
 
     cpdef double x(self)
-    cpdef set_x(self, value: double)
+    cpdef set_x(self, double value)
     cpdef double y(self)
-    cpdef void set_y(self, value: double)
+    cpdef void set_y(self, double value)
 
 cdef class Point2(_Coord2):
     pass
 
 cdef class Vector2(_Coord2):
-    pass
+
+    cpdef double length(self)
+    cpdef Vector2 normalized(self)
+    cpdef double angle_to(self, Vector2 other)
 
 cdef class Transform2D:
     cdef readonly np.ndarray _matrix
 
-    cpdef Vector2 translation(self)
+    cpdef Vector2 translation(self)
+    cpdef double rotation(self)
+
+    cpdef Transform2D inverse(self)

code/mapping/ext_modules/mapping_common/transform.py

@@ -1,5 +1,4 @@
 from dataclasses import dataclass
-import cython
 
 import numpy as np
 import numpy.typing as npt
@@ -26,16 +25,16 @@ def __init__(self, matrix: npt.NDArray[np.float64]) -> None:
         ), f"{type(self).__name__} matrix must have shape (3,)"
         self._matrix = matrix
 
-    def x(self) -> cython.double:
+    def x(self) -> float:
         return self._matrix[0]
 
-    def set_x(self, value: cython.double):
+    def set_x(self, value: float):
         self._matrix[0] = value
 
-    def y(self) -> cython.double:
+    def y(self) -> float:
         return self._matrix[1]
 
-    def set_y(self, value: cython.double):
+    def set_y(self, value: float):
         self._matrix[1] = value
 
     def __eq__(self, value) -> bool:
@@ -50,6 +49,9 @@ class Point2(_Coord2):
 
     Receives both rotation and translation when transformed with a Transform2D"""
 
+    def vector(self) -> "Vector2":
+        return Vector2(self._matrix)
+
     @staticmethod
     def new(x: float, y: float) -> "Point2":
         m = np.array([x, y, 1.0], dtype=np.float64)
@@ -70,6 +72,15 @@ def from_ros_msg(m: geometry_msgs.Point) -> "Point2":
     def to_ros_msg(self) -> geometry_msgs.Point:
         return geometry_msgs.Point(x=self.x(), y=self.y(), z=0.0)
 
+    def __add__(self, other):
+        if isinstance(other, Vector2):
+            matrix = self._matrix + other._matrix
+            matrix[2] = 1.0
+            return Point2(matrix)
+        raise TypeError(
+            f"Unsupported operand types for *: '{type(self)}' and '{type(other)}'"
+        )
+
 
 @dataclass(init=False, eq=False)
 class Vector2(_Coord2):
@@ -95,11 +106,19 @@ def angle_to(self, other: "Vector2") -> float:
             other (Vector2): _description_
 
         Returns:
-            float: angle in radians. Always in interval [-pi,pi].
-            TODO: define if angle is cw or ccw depending on sign
+            float: signed angle in radians. Always in interval [-pi,pi].
+
+            - angle > 0: CCW
+            - angle < 0: CW
         """
-        div = self._matrix.dot(other._matrix) / self.length() * other.length()
-        return math.acos(div)
+        # Based on https://stackoverflow.com/questions/21483999/
+        # using-atan2-to-find-angle-between-two-vectors/21486462#21486462
+        cross = np.cross(self._matrix[:2], other._matrix[:2])
+        dot = np.dot(self._matrix[:2], other._matrix[:2])
+        return math.atan2(cross, dot)
+
+    def point(self) -> Point2:
+        return Point2(self._matrix)
 
     @staticmethod
     def new(x: float, y: float) -> "Vector2":
@@ -121,12 +140,41 @@ def from_ros_msg(m: geometry_msgs.Vector3) -> "Vector2":
     def to_ros_msg(self) -> geometry_msgs.Vector3:
         return geometry_msgs.Vector3(x=self.x(), y=self.y(), z=0.0)
 
+    def __mul__(self, other):
+        if isinstance(other, float):
+            matrix = self._matrix * other
+            matrix[2] = 1.0
+            return Vector2(matrix)
+        raise TypeError(
+            f"Unsupported operand types for *: '{type(self)}' and '{type(other)}'"
+        )
+
+    def __add__(self, other):
+        if isinstance(other, Vector2):
+            matrix = self._matrix + other._matrix
+            matrix[2] = 1.0
+            return Vector2(matrix)
+        raise TypeError(
+            f"Unsupported operand types for *: '{type(self)}' and '{type(other)}'"
+        )
+
 
 @dataclass(init=False, eq=False)
 class Transform2D:
     """Homogeneous 2 dimensional transformation matrix
 
     Based on https://alexsm.com/homogeneous-transforms/
+
+    ## Examples:
+    ### Transform a Vector2
+    ```python
+    v = Vector2.new(1.0, 0.0)
+    t = Transform2D.new_rotation(math.pi/2.0)
+    v_transformed = t * v
+    ```
+    v_transformed is (0.0, 1.0)
+
+    Note that Vectors are only directions/offsets and ignore translations.
     """
 
     # Matrix with shape (3, 3)
@@ -149,6 +197,20 @@ def translation(self) -> Vector2:
         m = m / m[2]
         return Vector2(m)
 
+    def rotation(self) -> float:
+        """Returns only the rotation that this Transform applies
+
+        Returns:
+            float: rotation angle in radians
+
+            - angle > 0: CCW
+            - angle < 0: CW
+        """
+        # Not the most efficient solution
+        v = Vector2.new(1.0, 0.0)
+        v_rot: Vector2 = self * v
+        return v.angle_to(v_rot)
+
     def inverse(self) -> "Transform2D":
         """Returns an inverted Transformation matrix
 
@@ -172,7 +234,9 @@ def new_rotation(angle: float) -> "Transform2D":
 
         Args:
             angle (float): Rotation angle in radians
-            TODO: define if angle is cw or ccw depending on sign
+
+            - angle > 0: CCW
+            - angle < 0: CW
         """
         c = np.cos(angle)
         s = np.sin(angle)
@@ -200,7 +264,9 @@ def new_rotation_translation(angle: float, v: Vector2) -> "Transform2D":
 
         Args:
             angle (float): Rotation angle in radians
-            TODO: define if angle is cw or ccw depending on sign
+            - angle > 0: CCW
+            - angle < 0: CW
+
             v (Vector2): Translation vector
         """
         transform = Transform2D.new_rotation(angle)

code/mapping/tests/mapping_common/test_transform.py

@@ -1,4 +1,29 @@
 from mapping_common.transform import Transform2D, Vector2, Point2
+import math
+
+
+def test_point_conversion():
+    p = Point2.new(1.0, 25.0)
+    msg = p.to_ros_msg()
+    p_conv = Point2.from_ros_msg(msg)
+
+    assert p == p_conv
+
+
+def test_vector_conversion():
+    v = Vector2.new(1.0, 25.0)
+    msg = v.to_ros_msg()
+    v_conv = Vector2.from_ros_msg(msg)
+
+    assert v == v_conv
+
+
+def test_transform_conversion():
+    p = Point2.new(1.0, 25.0)
+    msg = p.to_ros_msg()
+    p_conv = Point2.from_ros_msg(msg)
+
+    assert p == p_conv
 
 
 def test_translation():
@@ -16,3 +41,90 @@ def test_translation2():
     transl = Transform2D.new_translation(transl_v)
 
     assert transl.translation() == transl_v
+
+
+def test_rotation():
+    rot = Transform2D.new_rotation(0.0)
+    assert math.isclose(rot.rotation(), 0.0)
+
+
+def test_rotation2():
+    rot = Transform2D.new_rotation(math.pi / 2)
+    assert math.isclose(rot.rotation(), math.pi / 2)
+
+
+def test_rotation3():
+    rot = Transform2D.new_rotation(-math.pi / 2)
+    assert math.isclose(rot.rotation(), -math.pi / 2)
+
+
+def test_rotation4():
+    rot = Transform2D.new_rotation(-math.pi / 2)
+    v = Vector2.new(1.0, 0.0)
+    v_rot: Vector2 = rot * v
+    assert math.isclose(v_rot.x(), 0.0, abs_tol=1e-15)
+    assert math.isclose(v_rot.y(), -1.0, abs_tol=1e-15)
+
+
+def test_rotation5():
+    rot = Transform2D.new_rotation(math.pi / 2)
+    v = Vector2.new(1.0, 0.0)
+    v_rot: Vector2 = rot * v
+    assert math.isclose(v_rot.x(), 0.0, abs_tol=1e-15)
+    assert math.isclose(v_rot.y(), 1.0, abs_tol=1e-15)
+
+
+def test_rotation_translation_vector():
+    transl = Vector2.new(1.0, 2.0)
+    transf = Transform2D.new_rotation_translation(math.pi / 2, transl)
+    v = Vector2.new(1.0, 0.0)
+    v_rot: Vector2 = transf * v
+    # Vector ignores translation
+    assert math.isclose(v_rot.x(), 0.0, abs_tol=1e-15)
+    assert math.isclose(v_rot.y(), 1.0, abs_tol=1e-15)
+
+
+def test_rotation_translation_point():
+    transl = Vector2.new(1.0, 2.0)
+    transf = Transform2D.new_rotation_translation(math.pi / 2, transl)
+    p = Point2.new(1.0, 0.0)
+    p_trans: Point2 = transf * p
+    assert math.isclose(p_trans.x(), 1.0, abs_tol=1e-15)
+    assert math.isclose(p_trans.y(), 3.0, abs_tol=1e-15)
+
+
+def test_normalize():
+    v = Vector2.new(2.0, 2.0)
+    v_norm = v.normalized()
+
+    assert math.isclose(v_norm.length(), 1.0)
+
+
+def test_vector_scalar_mul():
+    v = Vector2.new(2.0, 2.0)
+    v_norm = v.normalized()
+    v_long = v_norm * 5.0
+
+    assert math.isclose(v_long.length(), 5.0)
+
+
+def test_vector_add():
+    v = Vector2.new(2.0, 2.0)
+    v2 = Vector2.new(1.0, 4.0)
+    v_sum = v + v2
+    v_result = Vector2.new(3.0, 6.0)
+
+    assert v_sum == v_result
+
+
+def test_inverse():
+    transl = Vector2.new(1.0, 2.0)
+    transf = Transform2D.new_rotation_translation(math.pi / 2, transl)
+    inv = transf.inverse()
+
+    p = Point2.new(1.0, 0.0)
+    p_trans: Point2 = transf * p
+    p_ret: Point2 = inv * p_trans
+
+    assert math.isclose(p_ret.x(), p.x(), abs_tol=1e-15)
+    assert math.isclose(p_ret.y(), p.y(), abs_tol=1e-15)