fixed bnyatelem (#88)

* fixed bnyatelem * unused import * fixed orientation --------- Co-authored-by: Jackack <[email protected]>
CMU-Robotics-Club · Apr 6, 2024 · 41d59a8 · 41d59a8
1 parent 1a398d0
commit 41d59a8
Showing 1 changed file with 13 additions and 3 deletions.
diff --git a/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py
@@ -18,7 +18,6 @@
 
 from world import World
 from pose import Pose
-import numpy as np
 
 class Translator:
     def __init__(self, self_name, other_name, teensy_name):
@@ -103,8 +102,19 @@ def reader_thread(self):
                     odom.pose.pose.position.x = long
                     odom.pose.pose.position.y = lat
                     odom.twist.twist.angular.z = packet.heading_rate
-                    odom.pose.pose.orientation = Pose.heading_to_quaternion(packet.heading + np.pi/2)
-
+                    heading = Pose.heading_to_quaternion(packet.heading)
+
+                    odom.pose.pose.orientation.x = heading[0]
+                    odom.pose.pose.orientation.y = heading[1]
+                    odom.pose.pose.orientation.z = heading[2]
+                    odom.pose.pose.orientation.w = heading[3]
+
+                    speed = packet.velocity
+                    # TODO: fix this
+                    # why this works: autonsystem only cares about magnitude of velocity
+                    # so setting an arbitrary axis to the speed and leave other at 0
+                    # works. However, this should be done properly ASAP
+                    odom.twist.twist.linear.x = speed
                     self.odom_publisher.publish(odom)
                 except Exception as e:
                     rospy.logwarn("Unable to convert other buggy position to lon lat" + str(e))