diff --git a/src/boat_simulator/boat_simulator/nodes/mock_data/__init__.py b/src/boat_simulator/boat_simulator/nodes/mock_data/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/boat_simulator/boat_simulator/nodes/mock_data/mock_data_node.py b/src/boat_simulator/boat_simulator/nodes/mock_data/mock_data_node.py new file mode 100644 index 000000000..3cf575430 --- /dev/null +++ b/src/boat_simulator/boat_simulator/nodes/mock_data/mock_data_node.py @@ -0,0 +1,77 @@ +import random + +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() + rclpy.spin(node) + + +if __name__ == "__main__": + main() diff --git a/src/boat_simulator/launch/main_launch.py b/src/boat_simulator/launch/main_launch.py index 928d5042f..e2fa6c6ca 100644 --- a/src/boat_simulator/launch/main_launch.py +++ b/src/boat_simulator/launch/main_launch.py @@ -83,6 +83,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 +181,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.MULTITHREADING_CLI_ARG_NAME, + [LaunchConfiguration("enable_sim_multithreading")], + ] + + node = Node( + package=PACKAGE_NAME, + executable=node_name, + name=node_name, + parameters=ros_parameters, + ros_arguments=ros_arguments, + arguments=local_arguments, + ) + + return node diff --git a/src/boat_simulator/setup.py b/src/boat_simulator/setup.py index b0c4585fb..4acd90c8d 100644 --- a/src/boat_simulator/setup.py +++ b/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", ], }, )