From a57c3168a01960035ad6be8c056e57eaa92f4143 Mon Sep 17 00:00:00 2001 From: Rishi Kumar Date: Wed, 18 Dec 2024 01:05:52 +0000 Subject: [PATCH] Removed import alias for SciPy Rotation in ros_to_bnyahaj.py, fixed possible swapping of packet x and y for ROS odom topic publishing. --- rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py index 17f5ae7..ca4bbea 100644 --- a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py +++ b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py @@ -4,7 +4,7 @@ import rclpy from rclpy import Node -from scipy.spatial.transform import Rotation as R +from scipy.spatial.transform import Rotation import utm from std_msgs.msg import Float64, Int8, UInt8, Bool @@ -150,7 +150,8 @@ def reader_thread(self): odom = ROSOdom() # convert to long lat try: - lat, long = utm.to_latlon(packet.x, packet.y, 17, "T") + # TODO check if the order of y and x here is accurate + lat, long = utm.to_latlon(packet.y, packet.x, 17, "T") odom.pose.pose.position.x = long odom.pose.pose.position.y = lat self.odom_publisher.publish(odom) @@ -168,7 +169,7 @@ def reader_thread(self): odom.pose.pose.position.x = long odom.pose.pose.position.y = lat odom.twist.twist.angular.z = packet.heading_rate - heading = R.from_euler('x', packet.heading, degrees=False).as_quat() + heading = Rotation.from_euler('x', packet.heading, degrees=False).as_quat() odom.pose.pose.orientation.x = heading[0] odom.pose.pose.orientation.y = heading[1]