From bbf62e1cc4f528b850616ced4ac2a6c2a861efbc Mon Sep 17 00:00:00 2001
From: evannawfal <140864831+evannawfal@users.noreply.github.com>
Date: Thu, 30 Nov 2023 18:56:22 -0800
Subject: [PATCH] Write wingsail publishing logic (#15)

* Controller Publisher

* removed TODO

* PR changes
---
 controller/wingsail/wingsail_ctrl_node.py | 36 ++++++++++++++++++++---
 1 file changed, 32 insertions(+), 4 deletions(-)

diff --git a/controller/wingsail/wingsail_ctrl_node.py b/controller/wingsail/wingsail_ctrl_node.py
index aec92f4..e05dc1e 100644
--- a/controller/wingsail/wingsail_ctrl_node.py
+++ b/controller/wingsail/wingsail_ctrl_node.py
@@ -4,7 +4,7 @@
 
 import rclpy
 import rclpy.utilities
-from custom_interfaces.msg import GPS, WindSensor
+from custom_interfaces.msg import GPS, SailCmd, WindSensor
 from rclpy.node import Node
 
 
@@ -39,6 +39,7 @@ def __init__(self):
         self.__declare_ros_parameters()
         self.__init_subscriptions()
         self.__init_publishers()
+        self.__init_timer_callbacks()
         self.get_logger().debug("Node initialization complete. Starting execution...")
 
     def __init_private_attributes(self):
@@ -96,10 +97,37 @@ def __init_publishers(self):
         """Initializes the publishers of this node. Publishers update ROS topics so that other ROS
         nodes in the system can utilize the data produced by this node.
         """
-        # TODO Implement this function by initializing publishers for topics that give the desired
-        # output data
 
-        pass
+        self.get_logger().debug("Initializing publishers...")
+        self.__trim_tab_angle_pub = self.create_publisher(
+            msg_type=SailCmd,
+            # TODO change "topic" from a magic string to a constant similar to how its done in the
+            # boat simulator
+            topic="sail_cmd",
+            qos_profile=1,
+        )
+
+    def __init_timer_callbacks(self):
+        """Initializes the timer callbacks of this node. Timer callbacks are registered to be
+        called at the specified frequency."""
+
+        self.get_logger().debug("Initializing timer callbacks...")
+
+        # Publishing data to ROS topics
+        self.create_timer(
+            timer_period_sec=self.pub_period,
+            callback=self.__publish,
+        )
+
+    # PUBLISHER CALLBACKS
+    def __publish(self):
+        """Publishes a SailCmd message with the trim tab angle using the designated publisher.
+        It also logs information about the publication to the logger."""
+
+        msg = SailCmd()
+        msg.trim_tab_angle_degrees = 0
+        self.__trim_tab_angle_pub.publish(msg)
+        self.get_logger().info(f"Published to {self.__trim_tab_angle_pub.topic}")
 
     @property
     def pub_period(self) -> float: