Implemented mock data node

src/boat_simulator/boat_simulator/common/constants.py

@@ -91,6 +91,9 @@ class BoatProperties:
 # CLI argument name for data collection option
 DATA_COLLECTION_CLI_ARG_NAME = "--enable-data-collection"
 
+# CLI argument name for data collection option
+MOCK_DATA_CLI_ARG_NAME = "--enable-mock-data"
+
 # Enumerated orientation indices since indexing pitch, roll, and yaw could be arbitrary
 ORIENTATION_INDICES = Enum("ORIENTATION_INDICES", ["PITCH", "ROLL", "YAW"], start=0)  # x, y, x
 

src/boat_simulator/boat_simulator/nodes/mock_data/mock_data_node.py

@@ -0,0 +1,91 @@
+import random
+import sys
+
+import rclpy
+from custom_interfaces.msg import DesiredHeading, HelperHeading, SailCmd
+from rclpy.node import Node
+
+import boat_simulator.common.constants as Constants
+
+# based on following:
+# https://docs.ros.org
+# /en/humble/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Py-Publisher-And-Subscriber.html
+# the purpose of this node is to publish to all topics that there is
+# subscribers in physics engine node so send goal code can work.
+
+
+class MockDataNode(Node):
+
+    def __init__(self):
+        super().__init__("mock_data")
+
+        self.desired_heading_pub = self.create_publisher(
+            msg_type=DesiredHeading,
+            topic=Constants.PHYSICS_ENGINE_SUBSCRIPTIONS.DESIRED_HEADING,
+            qos_profile=10,
+        )
+        self.sail_trim_tab_angle_pub = self.create_publisher(
+            msg_type=SailCmd,
+            topic=Constants.PHYSICS_ENGINE_SUBSCRIPTIONS.SAIL_TRIM_TAB_ANGLE,
+            qos_profile=10,
+        )
+
+        timer_period = 5  # seconds
+        self.timer = self.create_timer(timer_period, self.timer_callback)
+
+    def timer_callback(self):
+        self.get_logger().warn("MOCK DATA NODE ACTIVE")
+        self.publish_mock_desired_heading()
+        self.publish_mock_sail_trim_tab_angle()
+
+    def publish_mock_desired_heading(self):
+        """Publishes mock wind sensor data."""
+        heading = random.uniform(-179.99, 180.0)
+
+        helper_heading = HelperHeading()
+        helper_heading.heading = heading
+
+        msg = DesiredHeading()
+        msg.heading = helper_heading
+
+        self.desired_heading_pub.publish(msg)
+        self.get_logger().info(
+            f"Publishing to {self.desired_heading_pub.topic} "
+            + f"a mock desired heading of {heading} degrees"
+        )
+
+    def publish_mock_sail_trim_tab_angle(self):
+        """Publishes mock wind sensor data."""
+        trim_tab_angle_degrees = random.uniform(-40, 40)
+
+        msg = SailCmd()
+        msg.trim_tab_angle_degrees = trim_tab_angle_degrees
+
+        self.sail_trim_tab_angle_pub.publish(msg)
+        self.get_logger().info(
+            f"Publishing to {self.sail_trim_tab_angle_pub.topic} "
+            + f"a mock trim tab angle of {trim_tab_angle_degrees} degrees"
+        )
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = MockDataNode()
+    if is_mock_data_enabled():
+        try:
+            rclpy.spin(node)
+        finally:
+            rclpy.shutdown()
+
+
+def is_mock_data_enabled() -> bool:
+    try:
+        is_mock_data_enabled_index = sys.argv.index(Constants.MOCK_DATA_CLI_ARG_NAME) + 1
+        is_mock_data_enabled = sys.argv[is_mock_data_enabled_index] == "true"
+    except ValueError:
+        is_mock_data_enabled = False
+    return is_mock_data_enabled
+
+
+if __name__ == "__main__":
+    main()

src/boat_simulator/launch/main_launch.py

@@ -30,6 +30,12 @@
         choices=["true", "false"],
         description="Enable data collection in the boat simulator",
     ),
+    DeclareLaunchArgument(
+        name="enable-mock-data",
+        default_value="false",
+        choices=["true", "false"],
+        description="Enable generation of mock data from pathfinding",
+    ),
 ]
 
 
@@ -83,6 +89,7 @@ def setup_launch(context: LaunchContext) -> List[Node]:
     launch_description_entities.append(get_physics_engine_description(context))
     launch_description_entities.append(get_low_level_control_description(context))
     launch_description_entities.append(get_data_collection_description(context))
+    launch_description_entities.append(get_mock_data_description(context))
     return launch_description_entities
 
 
@@ -180,3 +187,36 @@ def get_data_collection_description(context: LaunchContext) -> Node:
     )
 
     return node
+
+
+def get_mock_data_description(context: LaunchContext) -> Node:
+    """Gets the launch description for the data collection node.
+
+    Args:
+        context (LaunchContext): The current launch context.
+
+    Returns:
+        Node: The node object that launches the data collection node.
+    """
+    node_name = "mock_data_node"
+    ros_parameters = [LaunchConfiguration("config").perform(context)]
+    ros_arguments: List[SomeSubstitutionsType] = [
+        "--log-level",
+        [f"{node_name}:=", LaunchConfiguration("log_level")],
+    ]
+    # may not need local arguments.
+    local_arguments: List[SomeSubstitutionsType] = [
+        Constants.MOCK_DATA_CLI_ARG_NAME,
+        [LaunchConfiguration("enable-mock-data")],
+    ]
+
+    node = Node(
+        package=PACKAGE_NAME,
+        executable=node_name,
+        name=node_name,
+        parameters=ros_parameters,
+        ros_arguments=ros_arguments,
+        arguments=local_arguments,
+    )
+
+    return node

src/boat_simulator/setup.py

@@ -30,6 +30,7 @@
             + "boat_simulator.nodes.low_level_control.low_level_control_node:main",
             "data_collection_node = "
             + "boat_simulator.nodes.data_collection.data_collection_node:main",
+            "mock_data_node = " + "boat_simulator.nodes.mock_data.mock_data_node:main",
         ],
     },
 )