From 2a1f2d595a83c94cb0bb1cc5f9215d88db3ea12f Mon Sep 17 00:00:00 2001 From: Saransh Agrawal Date: Wed, 23 Oct 2024 23:08:53 -0400 Subject: [PATCH] initial commit for buggy_state_converter --- python-requirements.txt | 2 - .../src/buggy/buggy/buggy_state_converter.py | 110 ------------------ rb_ws/src/buggy/setup.py | 30 ++--- 3 files changed, 15 insertions(+), 127 deletions(-) delete mode 100644 rb_ws/src/buggy/buggy/buggy_state_converter.py diff --git a/python-requirements.txt b/python-requirements.txt index 71c2c61..68b0c6f 100644 --- a/python-requirements.txt +++ b/python-requirements.txt @@ -5,8 +5,6 @@ numpy osqp pandas pymap3d -pyproj -rclpy scipy setuptools==58.2.0 trimesh diff --git a/rb_ws/src/buggy/buggy/buggy_state_converter.py b/rb_ws/src/buggy/buggy/buggy_state_converter.py deleted file mode 100644 index 3d5c442..0000000 --- a/rb_ws/src/buggy/buggy/buggy_state_converter.py +++ /dev/null @@ -1,110 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -from rclpy.node import Node -from nav_msgs.msg import Odometry -import numpy as np -import pyproj - - -class BuggyStateConverter(Node): - def __init__(self): - super().__init__("buggy_state_converter") - - # ROS2 Subscriber: /ekf/odometry_earth - self.subscription = self.create_subscription( - Odometry, "/ekf/odometry_earth", self.ekf_odometry_callback, 10 - ) - - # ROS2 Publisher: /nav/odometry_earth - self.publisher_ = self.create_publisher(Odometry, "/nav/odometry_earth", 10) - - # Initialize pyproj Transformer for ECEF -> UTM conversion - self.ecef_to_utm_transformer = pyproj.Transformer.from_crs( - "epsg:4978", "epsg:32633", always_xy=True - ) # Change EPSG as required - - def ekf_odometry_callback(self, msg): - """Callback function to process incoming odometry message, convert it, and publish the transformed message""" - - # Create a new Odometry message for the output - converted_msg = Odometry() - converted_msg.header = msg.header - - # ---- 1. Convert ECEF Position to UTM Coordinates ---- - ecef_x = msg.pose.pose.position.x - ecef_y = msg.pose.pose.position.y - ecef_z = msg.pose.pose.position.z - - # Convert ECEF to UTM using pyproj - utm_x, utm_y, _ = self.ecef_to_utm_transformer.transform(ecef_x, ecef_y, ecef_z) - - # Set the converted UTM position in the new message - converted_msg.pose.pose.position.x = utm_x # UTM Easting - converted_msg.pose.pose.position.y = utm_y # UTM Northing - converted_msg.pose.pose.position.z = 0.0 # Ignored in this context - - # ---- 2. Convert Quaternion to Heading (Radians) ---- - qx = msg.pose.pose.orientation.x - qy = msg.pose.pose.orientation.y - qz = msg.pose.pose.orientation.z - qw = msg.pose.pose.orientation.w - - # Convert quaternion to euler angles (roll, pitch, yaw) - yaw = self.quaternion_to_yaw(qx, qy, qz, qw) - - # Store the heading in the x component of the orientation (0 is East) - converted_msg.pose.pose.orientation.x = yaw - - # ---- 3. Copy Covariances (Unchanged) ---- - converted_msg.pose.covariance = msg.pose.covariance - converted_msg.twist.covariance = msg.twist.covariance - - # ---- 4. Convert Linear Velocities ---- - converted_msg.twist.twist.linear.x = ( - msg.twist.twist.linear.x - ) # m/s in x-direction - converted_msg.twist.twist.linear.y = ( - msg.twist.twist.linear.y - ) # m/s in y-direction - converted_msg.twist.twist.linear.z = ( - msg.twist.twist.linear.z - ) # Keep original Z velocity (??) - - # ---- 5. Convert Angular Velocities (rad/s for heading rate of change) ---- - converted_msg.twist.twist.angular.x = ( - msg.twist.twist.angular.z - ) # rad/s for heading change rate - converted_msg.twist.twist.angular.y = ( - msg.twist.twist.angular.y - ) # Keep original Y angular velocity (??) - converted_msg.twist.twist.angular.z = ( - msg.twist.twist.angular.z - ) # Keep original Z angular velocity (??) - - # Publish the converted message - self.publisher_.publish(converted_msg) - - def quaternion_to_yaw(self, qx, qy, qz, qw): - """Convert a quaternion to yaw (heading) in radians.""" - # Extract yaw (z-axis rotation) from quaternion - siny_cosp = 2.0 * (qw * qz + qx * qy) - cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz) - return np.arctan2(siny_cosp, cosy_cosp) - - -def main(args=None): - rclpy.init(args=args) - - # Create the BuggyStateConverter node and spin it - state_converter = BuggyStateConverter() - # while rclpy.ok(): <- is this needed? - rclpy.spin(state_converter) - - # Shutdown when done - state_converter.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/rb_ws/src/buggy/setup.py b/rb_ws/src/buggy/setup.py index a817c9d..4373639 100644 --- a/rb_ws/src/buggy/setup.py +++ b/rb_ws/src/buggy/setup.py @@ -1,30 +1,30 @@ -from glob import glob +from glob import glob import os from setuptools import find_packages, setup -package_name = "buggy" +package_name = 'buggy' setup( name=package_name, - version="0.0.0", - packages=find_packages(exclude=["test"]), + version='0.0.0', + packages=find_packages(exclude=['test']), data_files=[ - ("share/ament_index/resource_index/packages", ["resource/" + package_name]), - ("share/" + package_name, ["package.xml"]), + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), (os.path.join("share", package_name), glob("launch/*.xml")), ], - install_requires=["setuptools"], + install_requires=['setuptools'], zip_safe=True, - maintainer="root", - maintainer_email="root@todo.todo", - description="TODO: Package description", - license="TODO: License declaration", - tests_require=["pytest"], + maintainer='root', + maintainer_email='root@todo.todo', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], entry_points={ - "console_scripts": [ - "hello_world = buggy.hello_world:main", - "buggy_state_converter = buggy.buggy_state_converter:main", + 'console_scripts': [ + 'hello_world = buggy.hello_world:main' ], }, )