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 index f60fc062a..7fd4b31c6 100644 --- 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 @@ -7,15 +7,19 @@ 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): - + """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. + + based on following: + https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Py-Publisher-And-Subscriber.html + + Publishers: + desired_heading_pub (Publisher): Publishes GPS data in a `GPS` message. + wind_sensors_pub (Publisher): Publishes mock desired heading data in a `DesiredHeading` message. + sail_trim_tab_angle_pub (Publisher): Publishes mock sail trim tab angle data in a `SailCmd` message + """ def __init__(self): super().__init__("mock_data") self.__declare_ros_parameters() @@ -47,6 +51,10 @@ def __declare_ros_parameters(self): ("mock_sail_trim_tab", rclpy.Parameter.Type.BOOL), ("mock_desired_heading", rclpy.Parameter.Type.BOOL), ("qos_depth", rclpy.Parameter.Type.INTEGER), + ("mock_desired_heading_lower_bound", rclpy.Parameter.Type.DOUBLE), + ("mock_desired_heading_upper_bound", rclpy.Parameter.Type.DOUBLE), + ("mock_sail_trim_tab_lower_bound", rclpy.Parameter.Type.DOUBLE), + ("mock_sail_trim_tab_upper_bound", rclpy.Parameter.Type.DOUBLE) ], ) @@ -63,7 +71,7 @@ def timer_callback(self): def publish_mock_desired_heading(self): """Publishes mock wind sensor data.""" - heading = random.uniform(-179.99, 180.0) + heading = random.uniform(self.mock_desired_heading_lower_bound, self.mock_desired_heading_upper_bound) helper_heading = HelperHeading() helper_heading.heading = heading @@ -73,21 +81,21 @@ def publish_mock_desired_heading(self): 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" + 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) + trim_tab_angle_degrees = random.uniform(self.mock_sail_trim_tab_lower_bound, self.mock_sail_trim_tab_upper_bound) 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" + f'Publishing to {self.sail_trim_tab_angle_pub.topic} ' + f'a mock trim tab angle of {trim_tab_angle_degrees} degrees' ) @property diff --git a/src/global_launch/config/README.md b/src/global_launch/config/README.md index 24d05a417..2b58f30e8 100644 --- a/src/global_launch/config/README.md +++ b/src/global_launch/config/README.md @@ -333,20 +333,44 @@ corresponding correct type as the second string. - _Datatype_: `int` - _Range_: `[1, MAX_INT)` -**`pub_period_sec`** - -- _Description_: The period at which the publishers publish. -- _Datatype_: `double` -- _Range_: `(0.0, MAX_DOUBLE)` - **`mock_desired_heading`** - _Description_: Set to True if mock data for desired heading should be generated. False otherwise. - _Datatype_: `bool` - _Range_: `(True, False)` + +**`mock_desired_heading_lower_bound`** + +- _Description_: Gives a lower bound for randomizing desired heading values. This value should be less than `mock_desired_heading_upper_bound`. +- _Datatype_: `double` +- _Range_: `(-MAX_DOUBLE, MAX_DOUBLE)` + +**`mock_desired_heading_upper_bound`** + +- _Description_: Gives a upper bound for randomizing desired heading values. This value should be greater than `mock_desired_heading_upper_bound`. +- _Datatype_: `double` +- _Range_: `(-MAX_DOUBLE, MAX_DOUBLE)` **`mock_sail_trim_tab`** - _Description_: Set to True if mock data for sail trim tab should be generated. False otherwise. - _Datatype_: `bool` - _Range_: `(True, False)` + +**`mock_sail_trim_tab_lower_bound`** + +- _Description_: Gives a lower bound for randomizing sail trim tab values. This value should be less than `mock_sail_trim_tab_upper_bound`. +- _Datatype_: `double` +- _Range_: `(-MAX_DOUBLE, MAX_DOUBLE)` + +**`mock_sail_trim_tab_upper_bound`** + +- _Description_: Gives a upper bound for randomizing sail trim tab values. This value should be greater than `mock_sail_trim_tab_lower_bound`. +- _Datatype_: `double` +- _Range_: `(-MAX_DOUBLE, MAX_DOUBLE)` + +**`pub_period_sec`** + +- _Description_: The period at which the publishers publish. +- _Datatype_: `double` +- _Range_: `(0.0, MAX_DOUBLE)` \ No newline at end of file diff --git a/src/global_launch/config/globals.yaml b/src/global_launch/config/globals.yaml index 1dbd59b42..50973c6e5 100644 --- a/src/global_launch/config/globals.yaml +++ b/src/global_launch/config/globals.yaml @@ -76,10 +76,13 @@ 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_desired_heading_lower_bound: -179.99 + mock_desired_heading_upper_bound: 180.00 mock_sail_trim_tab: True + mock_sail_trim_tab_lower_bound: -40.0 + mock_sail_trim_tab_desired_heading_upper_bound: 40 pub_period_sec: 5.0