From faea333e11699c46cb91934dada6635fc93803ad Mon Sep 17 00:00:00 2001
From: japjikbatra <168212037+japjikbatra@users.noreply.github.com>
Date: Sat, 23 Nov 2024 13:27:28 -0800
Subject: [PATCH] =?UTF-8?q?Made=20a=20new=20node=20that=20converts=20singl?=
 =?UTF-8?q?e-float=20representing=20compass=20angle=E2=80=A6=20(#27)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

* Made a new node that converts single-float representing compass angle into a quaternion.

* reformat files

* Fixed executable value in sensor_launch

* Deleted timer

* only import compasses when needed

---------

Co-authored-by: chrehall68 <chrehall68@gmail.com>
---
 src/compass/compass/main.py                   |  6 ++-
 src/compass/compass/quaternion_publisher.py   | 40 +++++++++++++++++++
 src/compass/setup.py                          |  1 +
 src/urc_intelsys_2024/launch/sensor_launch.py |  7 +++-
 .../msg/COMPASS_QUA.msg                       |  4 ++
 5 files changed, 55 insertions(+), 3 deletions(-)
 create mode 100644 src/compass/compass/quaternion_publisher.py
 create mode 100644 src/urc_intelsys_2024_msgs/msg/COMPASS_QUA.msg

diff --git a/src/compass/compass/main.py b/src/compass/compass/main.py
index fbb8f7f..2da3621 100644
--- a/src/compass/compass/main.py
+++ b/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")
diff --git a/src/compass/compass/quaternion_publisher.py b/src/compass/compass/quaternion_publisher.py
new file mode 100644
index 0000000..773a54a
--- /dev/null
+++ b/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")
diff --git a/src/compass/setup.py b/src/compass/setup.py
index 8bbee29..6f47516 100644
--- a/src/compass/setup.py
+++ b/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",
         ],
     },
 )
diff --git a/src/urc_intelsys_2024/launch/sensor_launch.py b/src/urc_intelsys_2024/launch/sensor_launch.py
index acfca6a..3b5e858 100644
--- a/src/urc_intelsys_2024/launch/sensor_launch.py
+++ b/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])
 
     # create launch description for the luxonis depthai ros driver
@@ -23,4 +26,6 @@ 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]
+    )
diff --git a/src/urc_intelsys_2024_msgs/msg/COMPASS_QUA.msg b/src/urc_intelsys_2024_msgs/msg/COMPASS_QUA.msg
new file mode 100644
index 0000000..26d98de
--- /dev/null
+++ b/src/urc_intelsys_2024_msgs/msg/COMPASS_QUA.msg
@@ -0,0 +1,4 @@
+Float64 x;
+Float64 y;
+Float64 z;
+Float64 a;
\ No newline at end of file