Made a new node that converts single-float representing compass angle…

… into a quaternion.

src/compass/compass/compass_listener.py

@@ -4,6 +4,7 @@
 from constants import COMPASS_TOPIC, QOS
 
 
+
 class CompassListener(Node):
     def __init__(self):
         super().__init__("compass_listener")

src/compass/compass/quaternion_publisher.py

@@ -0,0 +1,39 @@
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Float64
+from geometry_msgs.msg import Quaternion
+from constants import COMPASS_TOPIC, QOS
+import math
+
+class QuaternionPublisher(Node):
+    def __init__(self):
+        super().__init__('quaternion_publisher')
+
+        self.create_subscription(Float64, COMPASS_TOPIC, self.float_callback, 10)
+
+        self.publisher_ = self.create_publisher(Quaternion, 'quaternion_topic', 10)
+        self.timer = self.create_timer(1.0, self.timer_callback)
+
+    def float_callback(self, msg):
+        #convert the float to a quaternion
+        quaternion = self.convert_float_to_quaternion(msg.data)
+
+        self.publisher_.publish(quaternion)
+
+        self.get_logger().info(f'Publishing Quaternion: "{quaternion}"')
+
+    def convert_float_to_quaternion(self, angle):
+        angle = math.radians(angle)
+        q = Quaternion()
+        q.x = 0.0
+        q.y = 0.0
+        q.z = math.sin(angle/2.0)
+        q.w = 1-(q.z*q.z)
+        return q
+
+def main(args=None):
+    rclpy.init(args=args)
+    try:
+        rclpy.spin(QuaternionPublisher())
+    except KeyboardInterrupt:
+        print("Shutting down")

src/compass/setup.py

@@ -21,6 +21,8 @@
         "console_scripts": [
             "compass = compass.main:main",
             "compass_listener = compass.compass_listener:main",
+
+            "quaternion_publisher = compass.quaternion_publisher:main",
         ],
     },
 )

src/urc_intelsys_2024/launch/sensor_launch.py

@@ -13,6 +13,7 @@ def generate_launch_description():  # all launch files need a function with this
     # can't use the value in the parameter until we return the launch description.
     # thus, the lists, allow us to say "substitute `compass_type` with the actual value at runtime"
     compass_node = Node(package="compass", executable="compass", parameters=[config])
+    quaternion_compass_node = Node(package="compass", executable="quaternion_compass", parameters=[config])
     gps_node = Node(package="gps", executable="gps", parameters=[config])
 
     # create launch description for the luxonis depthai ros driver
@@ -23,4 +24,4 @@ def generate_launch_description():  # all launch files need a function with this
         ),
     )
 
-    return LaunchDescription([gps_node, compass_node, camera_launch])
+    return LaunchDescription([gps_node, compass_node, quaternion_compass_node, camera_launch])

src/urc_intelsys_2024_msgs/msg/COMPASS_QUA.msg

@@ -0,0 +1,4 @@
+Float64 x;
+Float64 y;
+Float64 z;
+Float64 a;