add a utm to local coord transformer

src/navigation/odom_transformer/odom_transformer/node_odom_transformer.py

@@ -0,0 +1,80 @@
+import rclpy
+from rclpy.node import Node
+from nav_msgs.msg import Odometry
+from math import cos, sin, radians, pi
+from geometry_msgs.msg import Quaternion
+from transforms3d.euler import euler2quat, quat2euler
+
+def yaw(quat: Quaternion):
+    # Convert quaternion to euler angles
+    return quat2euler([quat.w, quat.x, quat.y, quat.z])[2]  # yaw
+
+def orientation(x, y, z):
+    new_orientation = euler2quat(x, y, z)
+    return Quaternion(w=new_orientation[0], x=new_orientation[1], y=new_orientation[2], z=new_orientation[3])
+
+class OdometryTransformer(Node):
+    def __init__(self):
+        super().__init__('odometry_transformer')
+
+        self.subscription = self.create_subscription(
+            Odometry,
+            'imu/odometry',
+            self.odometry_callback,
+            10)
+
+        self.initialised = False
+        self.initial_easting = 0.0
+        self.initial_northing = 0.0
+        self.initial_heading = 0.0
+
+        self.publisher = self.create_publisher(
+            Odometry,
+            'imu/local_odometry',
+            10)
+
+    def odometry_callback(self, msg: Odometry):
+        if not self.initialised:
+            self.initial_easting = msg.pose.pose.position.x
+            self.initial_northing = msg.pose.pose.position.y
+            # Assuming orientation is in quaternion, converting to heading (yaw)
+            self.initial_heading = yaw(msg.pose.pose.orientation)
+            self.initialised = True
+
+            print(f"Initialised at ({self.initial_easting}, {self.initial_northing}, {self.initial_heading})")
+            return
+
+        current_easting = msg.pose.pose.position.x
+        current_northing = msg.pose.pose.position.y
+        current_heading = yaw(msg.pose.pose.orientation)
+
+        delta_easting = current_easting - self.initial_easting
+        delta_northing = current_northing - self.initial_northing
+
+        # relative_heading = -current_heading - self.initial_heading + pi / 2  # use when ENU is false (eg recorded data)
+        relative_heading = current_heading - self.initial_heading
+
+        x = delta_easting * cos(self.initial_heading) + delta_northing * sin(self.initial_heading)
+        y = -delta_easting * sin(self.initial_heading) + delta_northing * cos(self.initial_heading)
+
+        # Create a new Odometry message for the local frame
+        local_odom = Odometry()
+        local_odom.header = msg.header
+        local_odom.pose.pose.position.x = x
+        local_odom.pose.pose.position.y = y
+        local_odom.pose.pose.position.z = msg.pose.pose.position.z
+        # Adjust the orientation to be relative to the initial heading
+        local_odom.pose.pose.orientation = orientation(0, 0, relative_heading)
+        local_odom.twist = msg.twist
+
+        self.publisher.publish(local_odom)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = OdometryTransformer()
+    rclpy.spin(node)
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/navigation/odom_transformer/package.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+    <name>odom_transformer</name>
+    <version>0.0.0</version>
+    <description>SBG UTM to odom frame transform</description>
+    <maintainer email="[email protected]">Alastair Bradford</maintainer>
+    <license>MIT</license>
+
+    <!-- Messages -->
+    <depend>geometry_msgs</depend>
+    <depend>nav_msgs</depend>
+
+    <export>
+        <build_type>ament_python</build_type>
+    </export>
+</package>

src/navigation/odom_transformer/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/odom_transformer
+[install]
+install_scripts=$base/lib/odom_transformer

src/navigation/odom_transformer/setup.py

@@ -0,0 +1,25 @@
+from setuptools import setup
+
+package_name = "odom_transformer"
+
+setup(
+    name=package_name,
+    version="0.0.0",
+    packages=[package_name],
+    data_files=[
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="Alastair Bradford",
+    maintainer_email="[email protected]",
+    description="SBG UTM to odom frame transform",
+    license="MIT",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "odom_transformer = odom_transformer.node_odom_transformer:main",
+        ],
+    },
+)