diff --git a/src/navigation/odom_transformer/odom_transformer/__init__.py b/src/navigation/odom_transformer/odom_transformer/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/navigation/odom_transformer/odom_transformer/node_odom_transformer.py b/src/navigation/odom_transformer/odom_transformer/node_odom_transformer.py new file mode 100644 index 000000000..3f44db0be --- /dev/null +++ b/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() diff --git a/src/navigation/odom_transformer/package.xml b/src/navigation/odom_transformer/package.xml new file mode 100644 index 000000000..8ce5b6c71 --- /dev/null +++ b/src/navigation/odom_transformer/package.xml @@ -0,0 +1,17 @@ + + + + odom_transformer + 0.0.0 + SBG UTM to odom frame transform + Alastair Bradford + MIT + + + geometry_msgs + nav_msgs + + + ament_python + + diff --git a/src/navigation/odom_transformer/resource/odom_transformer b/src/navigation/odom_transformer/resource/odom_transformer new file mode 100644 index 000000000..e69de29bb diff --git a/src/navigation/odom_transformer/setup.cfg b/src/navigation/odom_transformer/setup.cfg new file mode 100644 index 000000000..7279c6374 --- /dev/null +++ b/src/navigation/odom_transformer/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/odom_transformer +[install] +install_scripts=$base/lib/odom_transformer diff --git a/src/navigation/odom_transformer/setup.py b/src/navigation/odom_transformer/setup.py new file mode 100644 index 000000000..f42f17e33 --- /dev/null +++ b/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="team@qutmotorsport.com", + description="SBG UTM to odom frame transform", + license="MIT", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "odom_transformer = odom_transformer.node_odom_transformer:main", + ], + }, +)