Sign convention

rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py

@@ -18,6 +18,7 @@
 
 from world import World
 from pose import Pose
+import numpy as np
 
 class Translator:
     def __init__(self, self_name, other_name, teensy_name):
@@ -80,7 +81,7 @@ def reader_thread(self):
         while True:
             packet = self.comms.read_packet()
             # print("trying to read odom")
-            if isinstance(packet, Odometry):
+            if isinstance(packet, Odometry) and self_name == "SC":
                 rospy.logdebug("packet", packet)
                 #Publish to odom topic x and y coord
                 odom = ROSOdom()
@@ -93,15 +94,15 @@ def reader_thread(self):
                 except Exception as e:
                     rospy.logwarn("Unable to convert other buggy position to lon lat" + e)
 
-            elif isinstance(packet, BnyaTelemetry):
+            elif isinstance(packet, BnyaTelemetry) and self_name == "NAND":
                 rospy.logdebug("packet", packet)
                 odom = ROSOdom()
                 try:
                     lat, long = World.utm_to_gps(packet.y, packet.x)
                     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)
+                    odom.pose.pose.orientation = Pose.heading_to_quaternion(packet.heading + np.pi/2)
 
                     self.odom_publisher.publish(odom)
                 except Exception as e: