Merge branch 'main' into 20-feature-add-transform-to-convert-gps-latl…

…ong-to-cartesian

src/compass/compass/main.py

@@ -1,5 +1,3 @@
-from compass.actual_compass import ActualCompass
-from compass.fake_compass import FakeCompass
 from std_msgs.msg import Float64
 from constants import COMPASS_TOPIC, QOS
 from rclpy.node import Node
@@ -20,8 +18,12 @@ def __init__(self):
         )
 
         if self.get_parameter("compass_type").value == "actual":
+            from compass.actual_compass import ActualCompass
+
             self.compass = ActualCompass()
         elif self.get_parameter("compass_type").value == "fake":
+            from compass.fake_compass import FakeCompass
+
             self.compass = FakeCompass()
         else:
             raise ValueError("Unknown compass type")

src/compass/compass/quaternion_publisher.py

@@ -0,0 +1,40 @@
+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)
+
+    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,7 @@
         "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,9 @@ 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_publisher", parameters=[config]
+    )
     gps_node = Node(package="gps", executable="gps", parameters=[config])
     geo_to_cart_node = Node(package="gps", executable="geo_to_cart", parameters=[config])
 
@@ -24,4 +27,7 @@ def generate_launch_description():  # all launch files need a function with this
         ),
     )
 
-    return LaunchDescription([gps_node, geo_to_cart_node, compass_node, camera_launch])
+    return LaunchDescription(
+        [gps_node, geo_to_cart_node, quaternion_compass_node, 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;