Navigate Observer Node (#546)

* initial file created

* node runs

---------

Co-authored-by: Akshanjay Kompelli <63443017+FireBoyAJ24@users.noreply.github.com>

Author: SPDonaghy

src/local_pathfinding/launch/main_launch.py

@@ -73,6 +73,7 @@ def setup_launch(context: LaunchContext) -> List[Node]:
         launch_description_entities.append(get_mock_wind_sensor_node_description(context))
         launch_description_entities.append(get_mock_ais_node_description(context))
         launch_description_entities.append(get_mock_gps_node_description(context))
+        launch_description_entities.append(get_navigate_observer_node_description(context))
     return launch_description_entities
 
 
@@ -199,3 +200,33 @@ def get_mock_gps_node_description(context: LaunchContext) -> Node:
     )
 
     return node
+
+
+def get_navigate_observer_node_description(context: LaunchContext) -> Node:
+    """Gets the launch description for the navigate_observer node.
+
+    Args:
+        context (LaunchContext): The current launch context.
+
+    Returns:
+        Node: The node object that launches the navigate_observer node.
+    """
+    node_name = "navigate_observer"
+    mode = LaunchConfiguration("mode").perform(context)
+    ros_parameters = [{"mode": mode}, LaunchConfiguration("config").perform(context)]
+    ros_arguments: List[SomeSubstitutionsType] = [
+        "--log-level",
+        [f"{node_name}:=", LaunchConfiguration("log_level")],
+    ]
+
+    node = Node(
+        package=PACKAGE_NAME,
+        executable="navigate_observer",
+        name=node_name,
+        output="screen",
+        emulate_tty=True,
+        parameters=ros_parameters,
+        ros_arguments=ros_arguments,
+    )
+
+    return node

src/local_pathfinding/local_pathfinding/node_navigate_observer.py

@@ -0,0 +1,47 @@
+"""
+This node observes the navigate node by subscribing to the local_path ROS
+topic.
+"""
+
+import custom_interfaces.msg as ci
+import rclpy
+from rclpy.node import Node
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    sailbot = SailbotObserver()
+
+    rclpy.spin(node=sailbot)
+
+    sailbot.destroy_node()
+    rclpy.shutdown()
+
+
+class SailbotObserver(Node):
+    """Observes the Sailbot node, through the local_path topic, as it navigates.
+
+    Subscribers:
+        local_path_sub (Subscription): Subscribe to a `LPathData` msg.
+
+    Attributes From Subscribers:
+        local_path: (ci.LPathData
+    """
+
+    def __init__(self):
+        super().__init__(node_name="navigate_observer")
+
+        self.local_path_sub = self.create_subscription(
+            msg_type=ci.LPathData,
+            topic="local_path",
+            callback=self.local_path_callback,
+            qos_profile=10,
+        )
+
+    def local_path_callback(self, msg: ci.GPS):
+        self.get_logger().debug(f"Received data from {self.local_path_sub.topic}: {msg}")
+        self.local_path_data = msg
+
+
+if __name__ == "__main__":
+    main()

src/local_pathfinding/setup.py

@@ -28,6 +28,7 @@
             "mock_wind_sensor = local_pathfinding.node_mock_wind_sensor:main",
             "mock_ais = local_pathfinding.node_mock_ais:main",
             "mock_gps = local_pathfinding.node_mock_gps:main",
+            "navigate_observer = local_pathfinding.node_navigate_observer:main",
         ],
     },
 )