Skip to content

Commit

Permalink
Removed import alias for SciPy Rotation in ros_to_bnyahaj.py, fixed p…
Browse files Browse the repository at this point in the history
…ossible swapping of packet x and y for ROS odom topic publishing.
  • Loading branch information
rk012 committed Dec 18, 2024
1 parent ed35e5a commit a57c316
Showing 1 changed file with 4 additions and 3 deletions.
7 changes: 4 additions & 3 deletions rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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)
Expand All @@ -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]
Expand Down

0 comments on commit a57c316

Please sign in to comment.