Mock global path implementation

launch/main_launch.py

@@ -65,8 +65,11 @@ def setup_launch(context: LaunchContext) -> List[Node]:
     Returns:
         List[Node]: Nodes to launch.
     """
-    launch_description_entities = list()
+    mode = LaunchConfiguration("mode").perform(context)
+    launch_description_entities = []
     launch_description_entities.append(get_navigate_node_description(context))
+    if mode == "development":
+        launch_description_entities.append(get_mock_global_path_node_description(context))
     return launch_description_entities
 
 
@@ -97,3 +100,32 @@ def get_navigate_node_description(context: LaunchContext) -> Node:
     )
 
     return node
+
+
+def get_mock_global_path_node_description(context: LaunchContext) -> Node:
+    """Gets the launch description for the mgp_main node.
+
+    Args:
+        context (LaunchContext): The current launch context.
+
+    Returns:
+        Node: The node object that launches the mgp_main node.
+    """
+    node_name = "mgp_main"
+    ros_parameters = [LaunchConfiguration("config").perform(context)]
+    ros_arguments: List[SomeSubstitutionsType] = [
+        "--log-level",
+        [f"{node_name}:=", LaunchConfiguration("log_level")],
+    ]
+
+    node = Node(
+        package=PACKAGE_NAME,
+        executable="mock_global_path",
+        name=node_name,
+        output="screen",
+        emulate_tty=True,
+        parameters=ros_parameters,
+        ros_arguments=ros_arguments,
+    )
+
+    return node

local_pathfinding/node_mock_global_path.py

@@ -0,0 +1,141 @@
+"""The main node of the local_pathfinding package, represented by the `Sailbot` class."""
+
+from typing import List
+
+import numpy as np
+import rclpy
+from custom_interfaces.msg import GPS, HelperLatLon, Path
+from rclpy.node import Node
+
+# Destination is hardcoded temporarily as a single element list
+DESTINATION = []
+DESTINATION.append(HelperLatLon(lat=49.263, lon=-123.138))
+
+NUM_INTERVALS = 100
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    global_path = GlobalPath()
+
+    rclpy.spin(node=global_path)
+
+    global_path.destroy_node()
+    rclpy.shutdown()
+
+
+class GlobalPath(Node):
+    """Stores, and publishes the global path.
+
+    Subscribers:
+        gps_sub (Subscription): Subscribe to a `GPS` msg.
+
+    Publishers and their timers:
+        global_path_pub (Subscription): Subscribe to a `Path` msg.
+        global_path_timer (Timer): Call the global path callback function.
+
+    Attributes from subscribers:
+        gps (GPS): Data from the GPS sensor.
+    """
+
+    def __init__(self):
+        super().__init__(node_name="mock_global_path")
+
+        self.declare_parameters(
+            namespace="",
+            parameters=[
+                ("pub_period_sec", rclpy.Parameter.Type.DOUBLE),
+                ("global_path_param", DESTINATION),
+            ],
+        )
+
+        # subscribers
+        self.gps_sub = self.create_subscription(
+            msg_type=GPS, topic="gps", callback=self.gps_callback, qos_profile=10
+        )
+
+        # publishers and their timers
+        self.global_path_pub = self.create_publisher(
+            msg_type=Path, topic="global_path", qos_profile=10
+        )
+        pub_period_sec = self.get_parameter("pub_period_sec").get_parameter_value().double_value
+        self.get_logger().info(f"Got parameter: {pub_period_sec=}")
+        self.global_path_timer = self.create_timer(
+            timer_period_sec=pub_period_sec, callback=self.global_path_callback
+        )
+
+        # attributes from subscribers
+        self.gps = None
+
+    # subscriber callbacks
+
+    def gps_callback(self, msg: GPS):
+        self.get_logger().info(f"Received data from {self.gps_sub.topic}: {msg}")
+        self.gps = msg
+
+    # publisher callbacks
+
+    def global_path_callback(self):
+        """Get and publish the global path."""
+        if self.gps is not None:
+            global_path = self.get_global_path()
+
+            if global_path is None or len(global_path) < NUM_INTERVALS + 1:
+                self.get_logger().warning("Global path is invalid")
+
+            msg = Path()
+            msg.waypoints = global_path
+
+            self.global_path_pub.publish(msg)
+            self.get_logger().info(f"Publishing to {self.global_path_pub.topic}: {msg}")
+
+    # get_global_path and its helper functions
+
+    def get_global_path(self) -> List[HelperLatLon]:
+        """Get the global path.
+
+        Returns:
+            List[HelperLatLon]: The global path.
+        """
+        if not self._all_subs_active():
+            self._log_inactive_subs_warning()
+            return []
+
+        current_location = self.gps.latlon
+
+        global_path = self.get_parameter("global_path_param").get_parameter_value
+
+        # Check if global path parameter is just a destination point
+        if len(global_path) < 2:
+            if len(global_path) < 1:
+                self.get_logger().warning("Global path has 0 elements. Must have at least 1.")
+                # TODO do something here to handle this
+
+            # Create a simple global path from the single destination point
+            # There will be some distortion here, so the intermediate points wont be evenly spaced
+            latitudes = np.linspace(current_location.lat, global_path[0].lat, NUM_INTERVALS + 1)
+            longitudes = np.linspace(current_location.lon, global_path[0].lon, NUM_INTERVALS + 1)
+
+            global_path = []
+
+            for i in range(NUM_INTERVALS):
+                global_path.append(
+                    HelperLatLon(
+                        lat=latitudes[i],
+                        lon=longitudes[i],
+                    )
+                )
+            return global_path
+
+        return global_path
+
+    def _all_subs_active(self) -> bool:
+        return True  # TODO: this line is a placeholder, delete when mocks can be run
+
+    def _log_inactive_subs_warning(self):
+        # TODO: log which subscribers are inactive
+        self.get_logger().warning("There are inactive subscribers")
+
+
+if __name__ == "__main__":
+    main()

setup.py

@@ -3,27 +3,28 @@
 
 from setuptools import setup
 
-package_name = 'local_pathfinding'
+package_name = "local_pathfinding"
 
 setup(
     name=package_name,
-    version='0.0.0',
+    version="0.0.0",
     packages=[package_name],
     data_files=[
-        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
-        (os.path.join('share', package_name), glob('launch/*_launch.py')),
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
+        (os.path.join("share", package_name), glob("launch/*_launch.py")),
     ],
-    install_requires=['setuptools'],
+    install_requires=["setuptools"],
     zip_safe=True,
-    maintainer='Patrick Creighton',
-    maintainer_email='software@ubcsailbot.org',
+    maintainer="Patrick Creighton",
+    maintainer_email="software@ubcsailbot.org",
     description="UBC Sailbot's local pathfinding ROS package",
-    license='MIT',
-    tests_require=['pytest'],
+    license="MIT",
+    tests_require=["pytest"],
     entry_points={
-        'console_scripts': [
-            'navigate = local_pathfinding.node_navigate:main',
+        "console_scripts": [
+            "navigate = local_pathfinding.node_navigate:main",
+            "mock_global_path = local_pathfinding.node_mock_global_path:main",
         ],
     },
 )