Telematics Rewrite (#11)

rb_ws/src/buggy/buggy/visualization/telematics.py

@@ -0,0 +1,63 @@
+#! /usr/bin/env python3
+# Runs the conversion script for all telematics data
+
+import rclpy
+import utm
+from nav_msgs.msg import Odometry
+from rclpy.node import Node
+from sensor_msgs.msg import NavSatFix
+
+
+class Telematics(Node):
+    """
+    Converts subscribers and publishers that need to be reformated, so that they are readible.
+    """
+
+    def __init__(self):
+        """Generate all the subscribers and publishers that need to be reformatted.
+        """
+        super().__init__('telematics')
+
+        # Implements behavior of callback_args from rospy.Subscriber
+        def wrap_args(callback, callback_args):
+            return lambda msg: callback(msg, callback_args)
+
+        self.self_publisher = self.create_publisher(NavSatFix, "/self/state_navsatfix", 10)
+        self.self_subscriber = self.create_subscription(Odometry, "/self/state", wrap_args(self.convert_buggystate, self.self_publisher), 1)
+
+        self.other_publisher = self.create_publisher(NavSatFix, "/other/state_navsatfix", 10)
+        self.other_subscriber = self.create_subscription(Odometry, "/other/state", wrap_args(self.convert_buggystate, self.other_publisher), 1)
+
+    # TODO Make this a static method?
+    def convert_buggystate(self, msg, publisher):
+        """Converts BuggyState/Odometry message to NavSatFix and publishes to specified publisher
+        
+        Args:
+            msg (Odometry): Buggy state to convert
+            publisher (Publisher): Publisher to send NavSatFix message to
+        """
+        try:
+            y = msg.pose.pose.position.y
+            x = msg.pose.pose.position.x
+            lat, long = utm.to_latlon(x, y, 17, "T")
+            down = msg.pose.pose.position.z
+            new_msg = NavSatFix()
+            new_msg.header = msg.header
+            new_msg.latitude = lat
+            new_msg.longitude = long
+            new_msg.altitude = down
+            publisher.publish(new_msg)
+
+        except Exception as e:
+            self.get_logger().warn(
+                "Unable to convert other buggy position to lon lat" + str(e)
+            )
+
+
+if __name__ == "__main__":
+    rclpy.init()
+    telem = Telematics()
+    rclpy.spin(telem)
+
+    telem.destroy_node()
+    rclpy.shutdown()