diff --git a/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py index 260fb21a..cb9085fa 100644 --- a/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py +++ b/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py @@ -68,11 +68,14 @@ def reader_thread(self): #Publish to odom topic x and y coord odom = ROSOdom() # convert to long lat - lat, long = World.utm_to_gps(packet.y, packet.x) - odom.pose.pose.position.x = long - odom.pose.pose.position.y = lat + try: + lat, long = World.utm_to_gps(packet.y, packet.x) + odom.pose.pose.position.x = long + odom.pose.pose.position.y = lat + self.odom_publisher.publish(odom) + except Exception as e: + rospy.logwarn("Unable to convert other buggy position to lon lat" + e) - self.odom_publisher.publish(odom) elif isinstance(packet, tuple): #Are there any other packet that is a tuple # print(packet) self.rc_steering_angle_publisher.publish(Float64(packet[0])) @@ -87,8 +90,6 @@ def reader_thread(self): self.read_rate.sleep() - - def loop(self): p1 = threading.Thread(target=self.writer_thread) p2 = threading.Thread(target=self.reader_thread)