From 634810afef1919ceabeff65dcba6f8b0321f8a22 Mon Sep 17 00:00:00 2001
From: ktro2828 <kotaro.uetake@tier4.jp>
Date: Sat, 16 Nov 2024 17:29:50 +0900
Subject: [PATCH] feat: update ego pose table

Signed-off-by: ktro2828 <kotaro.uetake@tier4.jp>
---
 t4_devkit/schema/tables/ego_pose.py | 24 +++++++++++++++++++++++-
 t4_devkit/typing.py                 |  4 +++-
 tests/schema/conftest.py            |  3 +++
 3 files changed, 29 insertions(+), 2 deletions(-)

diff --git a/t4_devkit/schema/tables/ego_pose.py b/t4_devkit/schema/tables/ego_pose.py
index 774a9e2..e5b0d31 100644
--- a/t4_devkit/schema/tables/ego_pose.py
+++ b/t4_devkit/schema/tables/ego_pose.py
@@ -12,7 +12,13 @@
 from .registry import SCHEMAS
 
 if TYPE_CHECKING:
-    from t4_devkit.typing import RotationType, TranslationType
+    from t4_devkit.typing import (
+        AccelerationType,
+        GeoCoordinateType,
+        RotationType,
+        TranslationType,
+        TwistType,
+    )
 
 __all__ = ["EgoPose"]
 
@@ -27,8 +33,24 @@ class EgoPose(SchemaBase):
         translation (TranslationType): Coordinate system origin given as [x, y, z] in [m].
         rotation (RotationType): Coordinate system orientation given as quaternion [w, x, y, z].
         timestamp (int): Unix time stamp.
+        twist (TwistType | None): Linear and angular velocities in the local coordinate system of
+            the ego vehicle (in m/s for linear and rad/s for angular), in the order of
+            (vx, vy, vz, yaw_rate, pitch_rate, roll_rate).
+        acceleration (AccelerationType | None): Acceleration in the local coordinate system of
+            the ego vehicle (in m/s2), in the order of (ax, ay, az).
+        geocoordinate (GeoCoordinateType | None): Coordinates in the WGS 84 reference ellipsoid
+            (latitude, longitude, altitude) in degrees and meters.
     """
 
     translation: TranslationType = field(converter=np.asarray)
     rotation: RotationType = field(converter=as_quaternion)
     timestamp: int
+    twist: TwistType = field(
+        default=None, converter=lambda x: np.asarray(x) if x is not None else x
+    )
+    acceleration: AccelerationType = field(
+        default=None, converter=lambda x: np.asarray(x) if x is not None else x
+    )
+    geocoordinate: GeoCoordinateType = field(
+        default=None, converter=lambda x: np.asarray(x) if x is not None else x
+    )
diff --git a/t4_devkit/typing.py b/t4_devkit/typing.py
index 155fd0f..cde72a8 100644
--- a/t4_devkit/typing.py
+++ b/t4_devkit/typing.py
@@ -18,9 +18,9 @@
     "NDArrayFloat",
     "TranslationType",
     "VelocityType",
+    "TwistType",
     "AccelerationType",
     "RotationType",
-    "VelocityType",
     "SizeType",
     "TrajectoryType",
     "CamIntrinsicType",
@@ -44,10 +44,12 @@
 # 3D
 TranslationType = NewType("TranslationType", NDArrayF64)
 VelocityType = NewType("VelocityType", NDArrayF64)
+TwistType = NewType("TwistType", NDArrayF64)
 AccelerationType = NewType("AccelerationType", NDArrayF64)
 RotationType = NewType("RotationType", Quaternion)
 SizeType = NewType("SizeType", NDArrayF64)
 TrajectoryType = NewType("TrajectoryType", NDArrayF64)
+GeoCoordinateType = NewType("GeoCoordinateType", NDArrayF64)
 CamIntrinsicType = NewType("CamIntrinsicType", NDArrayF64)
 CamDistortionType = NewType("CamDistortionType", NDArrayF64)
 
diff --git a/tests/schema/conftest.py b/tests/schema/conftest.py
index ff15255..ff686cf 100644
--- a/tests/schema/conftest.py
+++ b/tests/schema/conftest.py
@@ -75,6 +75,9 @@ def ego_pose_dict() -> dict:
         "translation": [1.0, 1.0, 1.0],
         "rotation": [1.0, 0.0, 0.0, 0.0],
         "timestamp": 1603452042983183,
+        "twist": [1.0, 1.0, 1.0, 1.0, 1.0, 1.0],
+        "acceleration": [1.0, 1.0, 1.0],
+        "geocoordinate": [35.0, 140.0, 5.0],
     }