merge conflict resolved

controller/wingsail/wingsail_ctrl_node.py

@@ -4,7 +4,7 @@
 
 import rclpy
 import rclpy.utilities
-from custom_interfaces.msg import SailCmd
+from custom_interfaces.msg import GPS, SailCmd, WindSensor
 from rclpy.node import Node
 
 
@@ -23,7 +23,8 @@ class WingsailControllerNode(Node):
     while optimizing for speed by maximizing the lift-to-drag ratio of the wingsail.
 
     Subscriptions:
-        TO BE ADDED
+        __filtered_wind_sensors_sub (Subscription): Subscribes to the filtered_wind_sensor topic
+        __gps_sub (Subscription): Subscribes to the gps topic
 
     Publishers:
         TO BE ADDED
@@ -46,6 +47,8 @@ def __init_private_attributes(self):
         during the initialization process.
         """
         self.__trim_tab_angle = 0.0
+        self.__filtered_wind_sensor = None
+        self.__gps = None
 
     def __declare_ros_parameters(self):
         """Declares ROS parameters from the global configuration file that will be used in this
@@ -74,7 +77,21 @@ def __init_subscriptions(self):
         """
         # TODO Implement this function by subscribing to topics that give the desired input data
         # Callbacks for each subscriptions should be defined as private methods of this class
-        pass
+        self.get_logger().debug("Initializing subscriptions...")
+
+        self.__filtered_wind_sensor_sub = self.create_subscription(
+            msg_type=WindSensor,
+            topic="filtered_wind_sensor",
+            callback=self.__filtered_wind_sensor_sub_callback,
+            qos_profile=1,
+        )
+
+        self.__gps_sub = self.create_subscription(
+            msg_type=GPS,
+            topic="gps",
+            callback=self.__gps_sub_callback,
+            qos_profile=1,
+        )
 
     def __init_publishers(self):
         """Initializes the publishers of this node. Publishers update ROS topics so that other ROS
@@ -84,7 +101,6 @@ def __init_publishers(self):
         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",
@@ -121,6 +137,24 @@ def pub_period(self) -> float:
     def trim_tab_angle(self) -> float:
         return self.__trim_tab_angle
 
+    def __filtered_wind_sensor_sub_callback(self, msg: WindSensor) -> None:
+        """Stores the latest filtered wind sensor data
+
+        Args:
+            msg (WindSensor): Filtered wind sensor data from CanTrxRosIntf.
+        """
+        self.__filtered_wind_sensor = msg
+        self.get_logger().info(f"Received data from {self.__filtered_wind_sensor_sub.topic}")
+
+    def __gps_sub_callback(self, msg: GPS) -> None:
+        """Stores the latest gps data
+
+        Args:
+            msg (GPS): gps data from CanTrxRosIntf.
+        """
+        self.__gps = msg
+        self.get_logger().info(f"Received data from {self.__gps_sub.topic}")
+
 
 if __name__ == "__main__":
     main()