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 mock data 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,131 @@
+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.__declare_ros_parameters()
+
+        self.desired_heading_pub = self.create_publisher(
+            msg_type=DesiredHeading,
+            topic=Constants.PHYSICS_ENGINE_SUBSCRIPTIONS.DESIRED_HEADING,
+            qos_profile=self.qos_depth,
+        )
+        self.sail_trim_tab_angle_pub = self.create_publisher(
+            msg_type=SailCmd,
+            topic=Constants.PHYSICS_ENGINE_SUBSCRIPTIONS.SAIL_TRIM_TAB_ANGLE,
+            qos_profile=self.qos_depth,
+        )
+
+        timer_period = self.pub_period  # seconds
+        self.timer = self.create_timer(timer_period, self.timer_callback)
+
+    def __declare_ros_parameters(self):
+        """Declares ROS parameters from the global configuration file that will be used in this
+        node. This node will monitor for any changes to these parameters during execution and will
+        update itself accordingly.
+        """
+        self.get_logger().debug("Declaring ROS parameters...")
+        self.declare_parameters(
+            namespace="",
+            parameters=[
+                ("pub_period_sec", rclpy.Parameter.Type.DOUBLE),
+                ("mock_sail_trim_tab", rclpy.Parameter.Type.BOOL),
+                ("mock_desired_heading", rclpy.Parameter.Type.BOOL),
+                ("qos_depth", rclpy.Parameter.Type.INTEGER),
+            ],
+        )
+
+        all_parameters = self._parameters
+        for name, parameter in all_parameters.items():
+            value_str = str(parameter.value)
+            self.get_logger().debug(f"Got parameter {name} with value {value_str}")
+
+    def timer_callback(self):
+        if self.mock_desired_heading:
+            self.publish_mock_desired_heading()
+        if self.mock_sail_trim_tab:
+            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"
+        )
+
+    @property
+    def pub_period(self) -> float:
+        return self.get_parameter("pub_period_sec").get_parameter_value().double_value
+
+    @property
+    def mock_desired_heading(self) -> bool:
+        return self.get_parameter("mock_desired_heading").get_parameter_value().bool_value
+
+    @property
+    def mock_sail_trim_tab(self) -> bool:
+        return self.get_parameter("mock_sail_trim_tab").get_parameter_value().bool_value
+
+    @property
+    def qos_depth(self) -> int:
+        return self.get_parameter("qos_depth").get_parameter_value().integer_value
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    if is_mock_data_enabled():
+        try:
+            node = MockDataNode()
+            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",
         ],
     },
 )

src/global_launch/config/globals.yaml

@@ -76,3 +76,10 @@ data_collection_node:
       bag: false
       json: true
       write_period_sec: 0.5
+
+mock_data_node:
+   ros__parameters:
+      qos_depth: 10
+      mock_desired_heading: True
+      mock_sail_trim_tab: True
+      pub_period_sec: 5.0