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 d809fad..aa21df0 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]) 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] + ) + 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