From f74e997f773e1105240aa029cd7c7039239ec4b5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Fri, 8 Aug 2025 23:15:31 +0200 Subject: [PATCH 1/8] Retrieve all parameters from protocol parameters Don't retrieve ROS parameters in rosbridge_library, instead, use parameters dictionary passed to RosbridgeProtocol for everything --- .../capabilities/advertise.py | 18 ++++++----- .../capabilities/advertise_action.py | 18 +++++++---- .../capabilities/advertise_service.py | 13 +++++--- .../capabilities/call_service.py | 31 ++++++++++--------- .../rosbridge_library/capabilities/publish.py | 14 ++++++--- .../capabilities/send_action_goal.py | 9 ++++-- .../capabilities/subscribe.py | 10 ++++-- .../capabilities/unadvertise_action.py | 15 ++++++--- .../capabilities/unadvertise_service.py | 11 +++++-- 9 files changed, 90 insertions(+), 49 deletions(-) diff --git a/rosbridge_library/src/rosbridge_library/capabilities/advertise.py b/rosbridge_library/src/rosbridge_library/capabilities/advertise.py index 00faa1ebf..60b87b995 100644 --- a/rosbridge_library/src/rosbridge_library/capabilities/advertise.py +++ b/rosbridge_library/src/rosbridge_library/capabilities/advertise.py @@ -30,6 +30,7 @@ # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +from __future__ import annotations import fnmatch @@ -83,7 +84,7 @@ class Advertise(Capability): advertise_msg_fields = ((True, "topic", str), (True, "type", str)) unadvertise_msg_fields = ((True, "topic", str),) - topics_glob = None + topics_glob: list[str] | None = None def __init__(self, protocol): # Call superclass constructor @@ -96,8 +97,11 @@ def __init__(self, protocol): # Initialize class variables self._registrations = {} - if protocol.parameters and "unregister_timeout" in protocol.parameters: - manager.unregister_timeout = protocol.parameters.get("unregister_timeout") + if protocol.parameters: + if "unregister_timeout" in protocol.parameters: + manager.unregister_timeout = protocol.parameters["unregister_timeout"] + if "topics_glob" in protocol.parameters: + self.topics_glob = protocol.parameters["topics_glob"] def advertise(self, message): # Pull out the ID @@ -109,10 +113,10 @@ def advertise(self, message): latch = message.get("latch", False) queue_size = message.get("queue_size", 100) - if Advertise.topics_glob is not None and Advertise.topics_glob: + if self.topics_glob: self.protocol.log("debug", "Topic security glob enabled, checking topic: " + topic) match = False - for glob in Advertise.topics_glob: + for glob in self.topics_glob: if fnmatch.fnmatch(topic, glob): self.protocol.log( "debug", @@ -144,10 +148,10 @@ def unadvertise(self, message): self.basic_type_check(message, self.unadvertise_msg_fields) topic = message["topic"] - if Advertise.topics_glob is not None and Advertise.topics_glob: + if self.topics_glob: self.protocol.log("debug", "Topic security glob enabled, checking topic: " + topic) match = False - for glob in Advertise.topics_glob: + for glob in self.topics_glob: if fnmatch.fnmatch(topic, glob): self.protocol.log( "debug", diff --git a/rosbridge_library/src/rosbridge_library/capabilities/advertise_action.py b/rosbridge_library/src/rosbridge_library/capabilities/advertise_action.py index 7d6602a0c..a9c0f085f 100644 --- a/rosbridge_library/src/rosbridge_library/capabilities/advertise_action.py +++ b/rosbridge_library/src/rosbridge_library/capabilities/advertise_action.py @@ -29,9 +29,10 @@ # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +from __future__ import annotations import fnmatch -from typing import Any +from typing import TYPE_CHECKING, Any from action_msgs.msg import GoalStatus from rclpy.action import ActionServer @@ -42,7 +43,9 @@ from rosbridge_library.capability import Capability from rosbridge_library.internal import message_conversion from rosbridge_library.internal.ros_loader import get_action_class -from rosbridge_library.protocol import Protocol + +if TYPE_CHECKING: + from rosbridge_library.protocol import Protocol class AdvertisedActionHandler: @@ -196,14 +199,17 @@ def graceful_shutdown(self) -> None: class AdvertiseAction(Capability): - actions_glob = None - advertise_action_msg_fields = ((True, "action", str), (True, "type", str)) + actions_glob: list[str] | None = None + def __init__(self, protocol: Protocol) -> None: # Call superclass constructor Capability.__init__(self, protocol) + if protocol.parameters and "actions_glob" in protocol.parameters: + self.actions_glob = protocol.parameters["actions_glob"] + # Register the operations that this capability provides protocol.register_operation("advertise_action", self.advertise_action) @@ -214,13 +220,13 @@ def advertise_action(self, message: dict) -> None: # parse the incoming message action_name = message["action"] - if AdvertiseAction.actions_glob is not None and AdvertiseAction.actions_glob: + if self.actions_glob: self.protocol.log( "debug", "Action security glob enabled, checking action: " + action_name, ) match = False - for glob in AdvertiseAction.actions_glob: + for glob in self.actions_glob: if fnmatch.fnmatch(action_name, glob): self.protocol.log( "debug", diff --git a/rosbridge_library/src/rosbridge_library/capabilities/advertise_service.py b/rosbridge_library/src/rosbridge_library/capabilities/advertise_service.py index c7a6f26da..87a9dbdca 100644 --- a/rosbridge_library/src/rosbridge_library/capabilities/advertise_service.py +++ b/rosbridge_library/src/rosbridge_library/capabilities/advertise_service.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import fnmatch import rclpy @@ -85,14 +87,17 @@ def graceful_shutdown(self): class AdvertiseService(Capability): - services_glob = None - advertise_service_msg_fields = ((True, "service", str), (True, "type", str)) + services_glob: list[str] | None = None + def __init__(self, protocol): # Call superclass constructor Capability.__init__(self, protocol) + if protocol.parameters and "services_glob" in protocol.parameters: + self.services_glob = protocol.parameters["services_glob"] + # Register the operations that this capability provides protocol.register_operation("advertise_service", self.advertise_service) @@ -103,13 +108,13 @@ def advertise_service(self, message): # parse the incoming message service_name = message["service"] - if AdvertiseService.services_glob is not None and AdvertiseService.services_glob: + if self.services_glob: self.protocol.log( "debug", "Service security glob enabled, checking service: " + service_name, ) match = False - for glob in AdvertiseService.services_glob: + for glob in self.services_glob: if fnmatch.fnmatch(service_name, glob): self.protocol.log( "debug", diff --git a/rosbridge_library/src/rosbridge_library/capabilities/call_service.py b/rosbridge_library/src/rosbridge_library/capabilities/call_service.py index 2429c33ef..b675403a6 100644 --- a/rosbridge_library/src/rosbridge_library/capabilities/call_service.py +++ b/rosbridge_library/src/rosbridge_library/capabilities/call_service.py @@ -29,6 +29,7 @@ # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +from __future__ import annotations import fnmatch from functools import partial @@ -45,25 +46,27 @@ class CallService(Capability): (False, "compression", str), ) - services_glob = None + # parameters + services_glob: list[str] | None = None + default_timeout: float = 5.0 + call_services_in_new_thread: bool = True def __init__(self, protocol): # Call superclass constructor Capability.__init__(self, protocol) - self.default_timeout = ( - protocol.node_handle.get_parameter("default_call_service_timeout") - .get_parameter_value() - .double_value - ) + if self.protocol.parameters: + if "services_glob" in self.protocol.parameters: + self.services_glob = self.protocol.parameters["services_glob"] + if "default_call_service_timeout" in self.protocol.parameters: + self.default_timeout = self.protocol.parameters["default_call_service_timeout"] + if "call_services_in_new_thread" in self.protocol.parameters: + self.call_services_in_new_thread = self.protocol.parameters[ + "call_services_in_new_thread" + ] # Register the operations that this capability provides - call_services_in_new_thread = ( - protocol.node_handle.get_parameter("call_services_in_new_thread") - .get_parameter_value() - .bool_value - ) - if call_services_in_new_thread: + if self.call_services_in_new_thread: # Calls the service in a separate thread so multiple services can be processed simultaneously. protocol.node_handle.get_logger().info("Calling services in new thread") protocol.register_operation( @@ -88,12 +91,12 @@ def call_service(self, message): args = message.get("args", []) timeout = message.get("timeout", self.default_timeout) - if CallService.services_glob is not None and CallService.services_glob: + if self.services_glob: self.protocol.log( "debug", "Service security glob enabled, checking service: " + service ) match = False - for glob in CallService.services_glob: + for glob in self.services_glob: if fnmatch.fnmatch(service, glob): self.protocol.log( "debug", diff --git a/rosbridge_library/src/rosbridge_library/capabilities/publish.py b/rosbridge_library/src/rosbridge_library/capabilities/publish.py index e88998f4d..b42a903a0 100644 --- a/rosbridge_library/src/rosbridge_library/capabilities/publish.py +++ b/rosbridge_library/src/rosbridge_library/capabilities/publish.py @@ -30,6 +30,7 @@ # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +from __future__ import annotations import fnmatch @@ -40,7 +41,7 @@ class Publish(Capability): publish_msg_fields = ((True, "topic", str),) - topics_glob = None + topics_glob: list[str] | None = None def __init__(self, protocol): # Call superclass constructor @@ -52,8 +53,11 @@ def __init__(self, protocol): # Save the topics that are published on for the purposes of unregistering self._published = {} - if protocol.parameters and "unregister_timeout" in protocol.parameters: - manager.unregister_timeout = protocol.parameters.get("unregister_timeout") + if protocol.parameters: + if "unregister_timeout" in protocol.parameters: + manager.unregister_timeout = protocol.parameters["unregister_timeout"] + if "topics_glob" in protocol.parameters: + self.topics_glob = protocol.parameters["topics_glob"] def publish(self, message): # Do basic type checking @@ -62,10 +66,10 @@ def publish(self, message): latch = message.get("latch", False) queue_size = message.get("queue_size", 100) - if Publish.topics_glob is not None and Publish.topics_glob: + if self.topics_glob: self.protocol.log("debug", "Topic security glob enabled, checking topic: " + topic) match = False - for glob in Publish.topics_glob: + for glob in self.topics_glob: if fnmatch.fnmatch(topic, glob): self.protocol.log( "debug", diff --git a/rosbridge_library/src/rosbridge_library/capabilities/send_action_goal.py b/rosbridge_library/src/rosbridge_library/capabilities/send_action_goal.py index 1f12fdb89..c09c12e74 100644 --- a/rosbridge_library/src/rosbridge_library/capabilities/send_action_goal.py +++ b/rosbridge_library/src/rosbridge_library/capabilities/send_action_goal.py @@ -56,7 +56,7 @@ class SendActionGoal(Capability): ) cancel_action_goal_msg_fields = ((True, "action", str),) - actions_glob = None + actions_glob: list[str] | None = None client_handler_list: dict[str, ActionClientHandler] def __init__(self, protocol: Protocol) -> None: @@ -65,6 +65,9 @@ def __init__(self, protocol: Protocol) -> None: self.client_handler_list = {} + if protocol.parameters and "actions_glob" in protocol.parameters: + self.actions_glob = protocol.parameters["actions_glob"] + # Register the operations that this capability provides send_action_goals_in_new_thread = ( protocol.node_handle.get_parameter("send_action_goals_in_new_thread") @@ -103,10 +106,10 @@ def send_action_goal(self, message: dict) -> None: compression = message.get("compression", "none") args = message.get("args", []) - if SendActionGoal.actions_glob is not None and SendActionGoal.actions_glob: + if self.actions_glob: self.protocol.log("debug", f"Action security glob enabled, checking action: {action}") match = False - for glob in SendActionGoal.actions_glob: + for glob in self.actions_glob: if fnmatch.fnmatch(action, glob): self.protocol.log( "debug", diff --git a/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py b/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py index 41902a7c2..7656f4aaf 100644 --- a/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py +++ b/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py @@ -29,6 +29,7 @@ # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +from __future__ import annotations import fnmatch from functools import partial @@ -224,12 +225,15 @@ class Subscribe(Capability): ) unsubscribe_msg_fields = (True, "topic", str) - topics_glob = None + topics_glob: list[str] | None = None def __init__(self, protocol): # Call superclass constructor Capability.__init__(self, protocol) + if protocol.parameters and "topics_glob" in protocol.parameters: + self.topics_glob = protocol.parameters["topics_glob"] + # Register the operations that this capability provides protocol.register_operation("subscribe", self.subscribe) protocol.register_operation("unsubscribe", self.unsubscribe) @@ -246,10 +250,10 @@ def subscribe(self, msg): # Make the subscription topic = msg["topic"] - if Subscribe.topics_glob is not None and Subscribe.topics_glob: + if self.topics_glob is not None and self.topics_glob: self.protocol.log("debug", "Topic security glob enabled, checking topic: " + topic) match = False - for glob in Subscribe.topics_glob: + for glob in self.topics_glob: if fnmatch.fnmatch(topic, glob): self.protocol.log( "debug", diff --git a/rosbridge_library/src/rosbridge_library/capabilities/unadvertise_action.py b/rosbridge_library/src/rosbridge_library/capabilities/unadvertise_action.py index 2114738e5..a499d5fac 100644 --- a/rosbridge_library/src/rosbridge_library/capabilities/unadvertise_action.py +++ b/rosbridge_library/src/rosbridge_library/capabilities/unadvertise_action.py @@ -29,20 +29,27 @@ # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +from __future__ import annotations import fnmatch +from typing import TYPE_CHECKING from rosbridge_library.capability import Capability -from rosbridge_library.protocol import Protocol + +if TYPE_CHECKING: + from rosbridge_library.protocol import Protocol class UnadvertiseAction(Capability): - actions_glob = None + actions_glob: list[str] | None = None def __init__(self, protocol: Protocol) -> None: # Call superclass constructor Capability.__init__(self, protocol) + if protocol.parameters and "actions_glob" in protocol.parameters: + self.actions_glob = protocol.parameters["actions_glob"] + # Register the operations that this capability provides protocol.register_operation("unadvertise_action", self.unadvertise_action) @@ -50,13 +57,13 @@ def unadvertise_action(self, message: dict) -> None: # parse the message action_name = message["action"] - if UnadvertiseAction.actions_glob is not None and UnadvertiseAction.actions_glob: + if self.actions_glob: self.protocol.log( "debug", f"Action security glob enabled, checking action: {action_name}", ) match = False - for glob in UnadvertiseAction.actions_glob: + for glob in self.actions_glob: if fnmatch.fnmatch(action_name, glob): self.protocol.log( "debug", diff --git a/rosbridge_library/src/rosbridge_library/capabilities/unadvertise_service.py b/rosbridge_library/src/rosbridge_library/capabilities/unadvertise_service.py index 117800b13..b9a0e560d 100644 --- a/rosbridge_library/src/rosbridge_library/capabilities/unadvertise_service.py +++ b/rosbridge_library/src/rosbridge_library/capabilities/unadvertise_service.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import fnmatch from rosbridge_library.capability import Capability @@ -6,12 +8,15 @@ class UnadvertiseService(Capability): # unadvertise_service_msg_fields = [(True, "service", (str, unicode))] - services_glob = None + services_glob: list[str] | None = None def __init__(self, protocol): # Call superclass constructor Capability.__init__(self, protocol) + if protocol.parameters and "services_glob" in protocol.parameters: + self.services_glob = protocol.parameters["services_glob"] + # Register the operations that this capability provides protocol.register_operation("unadvertise_service", self.unadvertise_service) @@ -19,13 +24,13 @@ def unadvertise_service(self, message): # parse the message service_name = message["service"] - if UnadvertiseService.services_glob is not None and UnadvertiseService.services_glob: + if self.services_glob: self.protocol.log( "debug", "Service security glob enabled, checking service: " + service_name, ) match = False - for glob in UnadvertiseService.services_glob: + for glob in self.services_glob: if fnmatch.fnmatch(service_name, glob): self.protocol.log( "debug", From 4e4660b3e9ad38e2e0d9349977dd2c506e0e30f4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Fri, 8 Aug 2025 23:24:17 +0200 Subject: [PATCH 2/8] Rewrite parameter handling in Rosbridge Server --- .../scripts/rosbridge_websocket.py | 391 +++++++----------- .../src/rosbridge_server/websocket_handler.py | 44 +- 2 files changed, 166 insertions(+), 269 deletions(-) diff --git a/rosbridge_server/scripts/rosbridge_websocket.py b/rosbridge_server/scripts/rosbridge_websocket.py index fa9d6c481..2099400c3 100755 --- a/rosbridge_server/scripts/rosbridge_websocket.py +++ b/rosbridge_server/scripts/rosbridge_websocket.py @@ -32,24 +32,20 @@ # POSSIBILITY OF SUCH DAMAGE. +import argparse import sys import time import rclpy +from rcl_interfaces.msg import ParameterDescriptor from rclpy.node import Node -from rclpy.qos import DurabilityPolicy, QoSProfile -from std_msgs.msg import Int32 +from rclpy.parameter import Parameter +from rclpy.utilities import remove_ros_args from tornado.httpserver import HTTPServer from tornado.ioloop import IOLoop, PeriodicCallback from tornado.netutil import bind_sockets from tornado.web import Application -from rosbridge_library.capabilities.advertise import Advertise -from rosbridge_library.capabilities.advertise_service import AdvertiseService -from rosbridge_library.capabilities.call_service import CallService -from rosbridge_library.capabilities.publish import Publish -from rosbridge_library.capabilities.subscribe import Subscribe -from rosbridge_library.capabilities.unadvertise_service import UnadvertiseService from rosbridge_server import ClientManager, RosbridgeWebSocket @@ -61,104 +57,161 @@ def shutdown_hook(): IOLoop.instance().stop() +SERVER_PARAMETERS = ( + # Server parameters + ("port", int, 9090, "Port to listen on for WebSocket connections."), + ("address", str, "", "Address to bind the WebSocket server to."), + ("url_path", str, "/", "URL path for the WebSocket server."), + ("retry_startup_delay", float, 2.0, "Delay in seconds before retrying to start the server."), + ("certfile", str, "", "Path to the SSL certificate file."), + ("keyfile", str, "", "Path to the SSL key file."), + # Tornado settings + ("websocket_ping_interval", float, 0, "Interval in seconds for WebSocket ping messages."), + ("websocket_ping_timeout", float, 30, "Timeout in seconds for WebSocket ping responses."), + # Websocket handler parameters + ("use_compression", bool, False, "Enable compression for WebSocket messages."), +) + +PROTOCOL_PARAMETERS = ( + ("fragment_timeout", int, 600, "Timeout in seconds for receiving next fragment."), + ("delay_between_messages", float, 0.0, "Delay in seconds between sending messages."), + ("max_message_size", int, 1000000, "Maximum size of a message in bytes."), + ( + "unregister_timeout", + float, + 10.0, + "How long to wait before unregistering a client from publisher after unadvertising publisher.", + ), + ("bson_only_mode", bool, False, "Use BSON only mode for messages."), + ("topics_glob", str, "[*]", "Glob patterns for topics publish/subscribe."), + ("services_glob", str, "[*]", "Glob patterns for services call/advertise."), + ("actions_glob", str, "[*]", "Glob patterns for actions send/advertise."), + ("call_services_in_new_thread", bool, True, "Call services in a new threads."), + ("default_call_service_timeout", float, 5.0, "Default timeout for service calls."), + ("send_action_goals_in_new_thread", bool, True, "Send action goals in a new threads."), +) + + +def parse_args() -> argparse.Namespace: + """Parse command line arguments and return them as a Namespace.""" + args = remove_ros_args(sys.argv)[1:] + parser = argparse.ArgumentParser(description="ROS 2 Rosbridge WebSocket Server") + for name, type_, default_value, description in SERVER_PARAMETERS + PROTOCOL_PARAMETERS: + parser.add_argument( + f"--{name}", + type=type_, + default=default_value, + help=description, + ) + return parser.parse_args(args) + + +def parse_glob_string(glob_string: str) -> list[str]: + """ + Parse a glob string into a list of patterns. + + The glob string is expected to be in the format: "['pattern1', 'pattern2']" + """ + if not glob_string or glob_string == "[]": + return [] + # Remove the surrounding brackets and split by comma + return [s.strip().strip("'") for s in glob_string[1:-1].split(",") if s.strip()] + + class RosbridgeWebsocketNode(Node): def __init__(self): super().__init__("rosbridge_websocket") RosbridgeWebSocket.node_handle = self + RosbridgeWebSocket.client_manager = ClientManager(self) + + self._handle_parameters() + + # To be able to access the list of topics and services, + # you must be able to access the rosapi services. + self.protocol_parameters["services_glob"].append("/rosapi/*") - ################################################## - # Parameter handling # - ################################################## + RosbridgeWebSocket.protocol_parameters = self.protocol_parameters - self.protocol_parameter_handling() + RosbridgeWebSocket.use_compression = self.use_compression - # get tornado application parameters - tornado_settings = {} - tornado_settings["websocket_ping_interval"] = float( - self.declare_parameter("websocket_ping_interval", 0).value + self._start_server() + + def _handle_parameters(self): + # Declare ROS parameters + for name, _, default_value, description in SERVER_PARAMETERS + PROTOCOL_PARAMETERS: + self.declare_parameter( + name, default_value, ParameterDescriptor(description=description) + ) + + # Handle command line arguments + args = parse_args() + for name, _, _, _ in SERVER_PARAMETERS + PROTOCOL_PARAMETERS: + if hasattr(args, name): + # Override the parameter with the command line argument + # This allows command line arguments to take precedence over declared parameters + param = Parameter(name, value=getattr(args, name)) + self.set_parameters([param]) + + # Protocol parameters + self.protocol_parameters = {} + for name, _, _, _ in PROTOCOL_PARAMETERS: + self.protocol_parameters[name] = self.get_parameter(name).value + + self.protocol_parameters["topics_glob"] = parse_glob_string( + self.protocol_parameters["topics_glob"] + ) + self.protocol_parameters["services_glob"] = parse_glob_string( + self.protocol_parameters["services_glob"] ) - tornado_settings["websocket_ping_timeout"] = float( - self.declare_parameter("websocket_ping_timeout", 30).value + self.protocol_parameters["actions_glob"] = parse_glob_string( + self.protocol_parameters["actions_glob"] ) - if "--websocket_ping_interval" in sys.argv: - idx = sys.argv.index("--websocket_ping_interval") + 1 - if idx < len(sys.argv): - tornado_settings["websocket_ping_interval"] = float(sys.argv[idx]) - else: - print("--websocket_ping_interval argument provided without a value.") - sys.exit(-1) - - if "--websocket_ping_timeout" in sys.argv: - idx = sys.argv.index("--websocket_ping_timeout") + 1 - if idx < len(sys.argv): - tornado_settings["websocket_ping_timeout"] = float(sys.argv[idx]) - else: - print("--websocket_ping_timeout argument provided without a value.") - sys.exit(-1) - - # Server and SSL options - # SSL options, cannot set default to None - rclpy throws warning - certfile = self.declare_parameter("certfile", "").value - keyfile = self.declare_parameter("keyfile", "").value + + # Server and SSL parameters + self.port = self.get_parameter("port").get_parameter_value().integer_value + self.address = self.get_parameter("address").get_parameter_value().string_value + self.url_path = self.get_parameter("url_path").get_parameter_value().string_value + self.retry_startup_delay = ( + self.get_parameter("retry_startup_delay").get_parameter_value().double_value + ) + self.certfile = self.get_parameter("certfile").get_parameter_value().string_value + self.keyfile = self.get_parameter("keyfile").get_parameter_value().string_value + # if not set, set to None - if certfile == "": - certfile = None - if keyfile == "": - keyfile = None - - port = self.declare_parameter("port", 9090).value - if "--port" in sys.argv: - idx = sys.argv.index("--port") + 1 - if idx < len(sys.argv): - port = int(sys.argv[idx]) - else: - print("--port argument provided without a value.") - sys.exit(-1) - address = self.declare_parameter("address", "").value - if "--address" in sys.argv: - idx = sys.argv.index("--address") + 1 - if idx < len(sys.argv): - address = sys.argv[idx] - else: - print("--address argument provided without a value.") - sys.exit(-1) - - url_path = self.declare_parameter("url_path", "/").value - if "--url_path" in sys.argv: - idx = sys.argv.index("--url_path") + 1 - if idx < len(sys.argv): - url_path = str(sys.argv[idx]) - else: - print("--url_path argument provided without a value.") - sys.exit(-1) - - retry_startup_delay = self.declare_parameter("retry_startup_delay", 2.0).value # seconds. - if "--retry_startup_delay" in sys.argv: - idx = sys.argv.index("--retry_startup_delay") + 1 - if idx < len(sys.argv): - retry_startup_delay = int(sys.argv[idx]) - else: - print("--retry_startup_delay argument provided without a value.") - sys.exit(-1) - - ################################################## - # Done with parameter handling # - ################################################## + if self.certfile == "": + self.certfile = None + if self.keyfile == "": + self.keyfile = None + + # Tornado application parameters + self.tornado_settings = {} + self.tornado_settings["websocket_ping_interval"] = ( + self.get_parameter("websocket_ping_interval").get_parameter_value().double_value + ) + self.tornado_settings["websocket_ping_timeout"] = ( + self.get_parameter("websocket_ping_timeout").get_parameter_value().double_value + ) + + # WebSocket handler parameters + self.use_compression = ( + self.get_parameter("use_compression").get_parameter_value().bool_value + ) + def _start_server(self): handlers = [(r"/", RosbridgeWebSocket), (r"", RosbridgeWebSocket)] - if url_path != "/": - handlers = [(rf"{url_path}", RosbridgeWebSocket)] + if self.url_path != "/": + handlers = [(rf"{self.url_path}", RosbridgeWebSocket)] - application = Application(handlers, **tornado_settings) + application = Application(handlers, **self.tornado_settings) connected = False while not connected and self.context.ok(): try: ssl_options = None - if certfile is not None and keyfile is not None: - ssl_options = {"certfile": certfile, "keyfile": keyfile} - sockets = bind_sockets(port, address) + if self.certfile is not None and self.keyfile is not None: + ssl_options = {"certfile": self.certfile, "keyfile": self.keyfile} + sockets = bind_sockets(self.port, self.address) actual_port = sockets[0].getsockname()[1] server = HTTPServer(application, ssl_options=ssl_options) server.add_sockets(sockets) @@ -167,169 +220,9 @@ def __init__(self): connected = True except OSError as e: # noqa: PERF203 self.get_logger().warn( - f"Unable to start server: {e} Retrying in {retry_startup_delay}s." + f"Unable to start server: {e} Retrying in {self.retry_startup_delay}s." ) - time.sleep(retry_startup_delay) - - def protocol_parameter_handling(self): - RosbridgeWebSocket.use_compression = self.declare_parameter("use_compression", False).value - - RosbridgeWebSocket.call_services_in_new_thread = self.declare_parameter( - "call_services_in_new_thread", True - ).value - - RosbridgeWebSocket.default_call_service_timeout = self.declare_parameter( - "default_call_service_timeout", 5.0 - ).value - - RosbridgeWebSocket.send_action_goals_in_new_thread = self.declare_parameter( - "send_action_goals_in_new_thread", True - ).value - - # get RosbridgeProtocol parameters - RosbridgeWebSocket.fragment_timeout = self.declare_parameter( - "fragment_timeout", RosbridgeWebSocket.fragment_timeout - ).value - - RosbridgeWebSocket.delay_between_messages = self.declare_parameter( - "delay_between_messages", RosbridgeWebSocket.delay_between_messages - ).value - - RosbridgeWebSocket.max_message_size = self.declare_parameter( - "max_message_size", RosbridgeWebSocket.max_message_size - ).value - - RosbridgeWebSocket.unregister_timeout = self.declare_parameter( - "unregister_timeout", RosbridgeWebSocket.unregister_timeout - ).value - - bson_only_mode = self.declare_parameter("bson_only_mode", False).value - - RosbridgeWebSocket.client_manager = ClientManager(self) - - # Publisher for number of connected clients - # QoS profile with transient local durability (latched topic in ROS 1). - client_count_qos_profile = QoSProfile( - depth=10, - durability=DurabilityPolicy.TRANSIENT_LOCAL, - ) - - RosbridgeWebSocket.client_count_pub = self.create_publisher( - Int32, "client_count", qos_profile=client_count_qos_profile - ) - RosbridgeWebSocket.client_count_pub.publish(Int32(data=0)) - - # Get the glob strings and parse them as arrays. - topics_glob = self.declare_parameter("topics_glob", "").value - - services_glob = self.declare_parameter("services_glob", "").value - - params_glob = self.declare_parameter("params_glob", "").value - - RosbridgeWebSocket.topics_glob = [ - element.strip().strip("'") - for element in topics_glob[1:-1].split(",") - if len(element.strip().strip("'")) > 0 - ] - RosbridgeWebSocket.services_glob = [ - element.strip().strip("'") - for element in services_glob[1:-1].split(",") - if len(element.strip().strip("'")) > 0 - ] - RosbridgeWebSocket.params_glob = [ - element.strip().strip("'") - for element in params_glob[1:-1].split(",") - if len(element.strip().strip("'")) > 0 - ] - - if "--fragment_timeout" in sys.argv: - idx = sys.argv.index("--fragment_timeout") + 1 - if idx < len(sys.argv): - RosbridgeWebSocket.fragment_timeout = int(sys.argv[idx]) - else: - print("--fragment_timeout argument provided without a value.") - sys.exit(-1) - - if "--delay_between_messages" in sys.argv: - idx = sys.argv.index("--delay_between_messages") + 1 - if idx < len(sys.argv): - RosbridgeWebSocket.delay_between_messages = float(sys.argv[idx]) - else: - print("--delay_between_messages argument provided without a value.") - sys.exit(-1) - - if "--max_message_size" in sys.argv: - idx = sys.argv.index("--max_message_size") + 1 - if idx < len(sys.argv): - value = sys.argv[idx] - RosbridgeWebSocket.max_message_size = int(value) - else: - print("--max_message_size argument provided without a value. (can be )") - sys.exit(-1) - - if "--unregister_timeout" in sys.argv: - idx = sys.argv.index("--unregister_timeout") + 1 - if idx < len(sys.argv): - RosbridgeWebSocket.unregister_timeout = float(sys.argv[idx]) - else: - print("--unregister_timeout argument provided without a value.") - sys.exit(-1) - - if "--topics_glob" in sys.argv: - idx = sys.argv.index("--topics_glob") + 1 - if idx < len(sys.argv): - value = sys.argv[idx] - if value == "None": - RosbridgeWebSocket.topics_glob = [] - else: - RosbridgeWebSocket.topics_glob = [ - element.strip().strip("'") for element in value[1:-1].split(",") - ] - else: - print("--topics_glob argument provided without a value. (can be None or a list)") - sys.exit(-1) - - if "--services_glob" in sys.argv: - idx = sys.argv.index("--services_glob") + 1 - if idx < len(sys.argv): - value = sys.argv[idx] - if value == "None": - RosbridgeWebSocket.services_glob = [] - else: - RosbridgeWebSocket.services_glob = [ - element.strip().strip("'") for element in value[1:-1].split(",") - ] - else: - print("--services_glob argument provided without a value. (can be None or a list)") - sys.exit(-1) - - if "--params_glob" in sys.argv: - idx = sys.argv.index("--params_glob") + 1 - if idx < len(sys.argv): - value = sys.argv[idx] - if value == "None": - RosbridgeWebSocket.params_glob = [] - else: - RosbridgeWebSocket.params_glob = [ - element.strip().strip("'") for element in value[1:-1].split(",") - ] - else: - print("--params_glob argument provided without a value. (can be None or a list)") - sys.exit(-1) - - if ("--bson_only_mode" in sys.argv) or bson_only_mode: - RosbridgeWebSocket.bson_only_mode = bson_only_mode - - # To be able to access the list of topics and services, you must be able to access the rosapi services. - if RosbridgeWebSocket.services_glob: - RosbridgeWebSocket.services_glob.append("/rosapi/*") - - Subscribe.topics_glob = RosbridgeWebSocket.topics_glob - Advertise.topics_glob = RosbridgeWebSocket.topics_glob - Publish.topics_glob = RosbridgeWebSocket.topics_glob - AdvertiseService.services_glob = RosbridgeWebSocket.services_glob - UnadvertiseService.services_glob = RosbridgeWebSocket.services_glob - CallService.services_glob = RosbridgeWebSocket.services_glob + time.sleep(self.retry_startup_delay) def main(args=None): diff --git a/rosbridge_server/src/rosbridge_server/websocket_handler.py b/rosbridge_server/src/rosbridge_server/websocket_handler.py index 0e6fa85dc..96975937d 100644 --- a/rosbridge_server/src/rosbridge_server/websocket_handler.py +++ b/rosbridge_server/src/rosbridge_server/websocket_handler.py @@ -30,12 +30,15 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +from __future__ import annotations + import sys import threading import traceback import uuid from collections import deque from functools import partial, wraps +from typing import TYPE_CHECKING, ClassVar from tornado.ioloop import IOLoop from tornado.iostream import StreamClosedError @@ -44,6 +47,11 @@ from rosbridge_library.rosbridge_protocol import RosbridgeProtocol from rosbridge_library.util import bson +if TYPE_CHECKING: + from rclpy.node import Node + + from .client_manager import ClientManager + _io_loop = IOLoop.instance() @@ -114,33 +122,29 @@ def run(self): class RosbridgeWebSocket(WebSocketHandler): - clients_connected = 0 - use_compression = False - - # The following are passed on to RosbridgeProtocol - # defragmentation.py: - fragment_timeout = 600 # seconds - # protocol.py: - delay_between_messages = 0 # seconds - max_message_size = 10000000 # bytes - unregister_timeout = 10.0 # seconds - bson_only_mode = False - node_handle = None + # Class variable to track the number of connected client + clients_connected: ClassVar[int] = 0 + + # Class variable to manage connected clients + client_manager: ClassVar[ClientManager | None] = None + + # Node handle to pass to RosbridgeProtocol when opening a connection + node_handle: ClassVar[Node | None] = None + + # Parameters to pass to RosbridgeProtocol when opening a connection + protocol_parameters: ClassVar = {} + + # Parameters for the WebSocket handler + use_compression: ClassVar[bool] = False @log_exceptions def open(self): cls = self.__class__ - parameters = { - "fragment_timeout": cls.fragment_timeout, - "delay_between_messages": cls.delay_between_messages, - "max_message_size": cls.max_message_size, - "unregister_timeout": cls.unregister_timeout, - "bson_only_mode": cls.bson_only_mode, - } + assert cls.node_handle is not None, "Node handle must be set before opening a WebSocket" try: self.client_id = uuid.uuid4() self.protocol = RosbridgeProtocol( - self.client_id, cls.node_handle, parameters=parameters + self.client_id, cls.node_handle, parameters=cls.protocol_parameters ) self.incoming_queue = IncomingQueue(self.protocol) self.incoming_queue.start() From 1cf4cb4c3fca7a4a9604ce951174bc87414c28af Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Fri, 8 Aug 2025 21:37:57 +0000 Subject: [PATCH 3/8] Declare ROS parameters read-only --- rosbridge_server/scripts/rosbridge_websocket.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosbridge_server/scripts/rosbridge_websocket.py b/rosbridge_server/scripts/rosbridge_websocket.py index 2099400c3..9c7148524 100755 --- a/rosbridge_server/scripts/rosbridge_websocket.py +++ b/rosbridge_server/scripts/rosbridge_websocket.py @@ -141,7 +141,7 @@ def _handle_parameters(self): # Declare ROS parameters for name, _, default_value, description in SERVER_PARAMETERS + PROTOCOL_PARAMETERS: self.declare_parameter( - name, default_value, ParameterDescriptor(description=description) + name, default_value, ParameterDescriptor(description=description, read_only=True) ) # Handle command line arguments From e1875c660ba894c142ce9978be5becd047f18c8d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Fri, 8 Aug 2025 21:54:45 +0000 Subject: [PATCH 4/8] Fix setting parameters through non-ROS cli arguments --- .../scripts/rosbridge_websocket.py | 28 +++++++------------ 1 file changed, 10 insertions(+), 18 deletions(-) diff --git a/rosbridge_server/scripts/rosbridge_websocket.py b/rosbridge_server/scripts/rosbridge_websocket.py index 9c7148524..4e06877f7 100755 --- a/rosbridge_server/scripts/rosbridge_websocket.py +++ b/rosbridge_server/scripts/rosbridge_websocket.py @@ -39,7 +39,6 @@ import rclpy from rcl_interfaces.msg import ParameterDescriptor from rclpy.node import Node -from rclpy.parameter import Parameter from rclpy.utilities import remove_ros_args from tornado.httpserver import HTTPServer from tornado.ioloop import IOLoop, PeriodicCallback @@ -96,13 +95,8 @@ def parse_args() -> argparse.Namespace: """Parse command line arguments and return them as a Namespace.""" args = remove_ros_args(sys.argv)[1:] parser = argparse.ArgumentParser(description="ROS 2 Rosbridge WebSocket Server") - for name, type_, default_value, description in SERVER_PARAMETERS + PROTOCOL_PARAMETERS: - parser.add_argument( - f"--{name}", - type=type_, - default=default_value, - help=description, - ) + for name, type_, _, description in SERVER_PARAMETERS + PROTOCOL_PARAMETERS: + parser.add_argument(f"--{name}", type=type_, help=description) return parser.parse_args(args) @@ -138,21 +132,19 @@ def __init__(self): self._start_server() def _handle_parameters(self): + # Parse command line arguments + args = parse_args() + # Declare ROS parameters for name, _, default_value, description in SERVER_PARAMETERS + PROTOCOL_PARAMETERS: + value = default_value + if hasattr(args, name) and getattr(args, name) is not None: + # Override the parameter with the command line argument + value = getattr(args, name) self.declare_parameter( - name, default_value, ParameterDescriptor(description=description, read_only=True) + name, value, ParameterDescriptor(description=description, read_only=True) ) - # Handle command line arguments - args = parse_args() - for name, _, _, _ in SERVER_PARAMETERS + PROTOCOL_PARAMETERS: - if hasattr(args, name): - # Override the parameter with the command line argument - # This allows command line arguments to take precedence over declared parameters - param = Parameter(name, value=getattr(args, name)) - self.set_parameters([param]) - # Protocol parameters self.protocol_parameters = {} for name, _, _, _ in PROTOCOL_PARAMETERS: From db8cdb22b45dc205624613727909bb143d46cc1e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Sat, 9 Aug 2025 01:24:49 +0200 Subject: [PATCH 5/8] Fix parameter handling in send_action_goal --- .../capabilities/send_action_goal.py | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/rosbridge_library/src/rosbridge_library/capabilities/send_action_goal.py b/rosbridge_library/src/rosbridge_library/capabilities/send_action_goal.py index c09c12e74..af4b520f0 100644 --- a/rosbridge_library/src/rosbridge_library/capabilities/send_action_goal.py +++ b/rosbridge_library/src/rosbridge_library/capabilities/send_action_goal.py @@ -57,6 +57,8 @@ class SendActionGoal(Capability): cancel_action_goal_msg_fields = ((True, "action", str),) actions_glob: list[str] | None = None + send_action_goals_in_new_thread: bool = False + client_handler_list: dict[str, ActionClientHandler] def __init__(self, protocol: Protocol) -> None: @@ -65,16 +67,16 @@ def __init__(self, protocol: Protocol) -> None: self.client_handler_list = {} - if protocol.parameters and "actions_glob" in protocol.parameters: - self.actions_glob = protocol.parameters["actions_glob"] + if protocol.parameters: + if "actions_glob" in protocol.parameters: + self.actions_glob = protocol.parameters["actions_glob"] + if "send_action_goals_in_new_thread" in protocol.parameters: + self.send_action_goals_in_new_thread = protocol.parameters[ + "send_action_goals_in_new_thread" + ] # Register the operations that this capability provides - send_action_goals_in_new_thread = ( - protocol.node_handle.get_parameter("send_action_goals_in_new_thread") - .get_parameter_value() - .bool_value - ) - if send_action_goals_in_new_thread: + if self.send_action_goals_in_new_thread: # Sends the action goal in a separate thread so multiple actions can be processed simultaneously. protocol.node_handle.get_logger().info("Sending action goals in new thread") protocol.register_operation( From 10c3f4e21fec5544f32faa5ccdb518fdf20e77a8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Sat, 9 Aug 2025 13:46:32 +0200 Subject: [PATCH 6/8] Pass parameters to Protocol through constructor Instead of passing them using a class variable --- rosbridge_library/src/rosbridge_library/protocol.py | 8 +++++--- .../src/rosbridge_library/rosbridge_protocol.py | 3 +-- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/rosbridge_library/src/rosbridge_library/protocol.py b/rosbridge_library/src/rosbridge_library/protocol.py index 4cb14d1b1..b5f93974e 100644 --- a/rosbridge_library/src/rosbridge_library/protocol.py +++ b/rosbridge_library/src/rosbridge_library/protocol.py @@ -87,9 +87,9 @@ class Protocol: # Use only BSON for the whole communication if the server has been started with bson_only_mode:=True bson_only_mode = False - parameters = None + parameters: dict[str, Any] | None = None - def __init__(self, client_id, node_handle): + def __init__(self, client_id, node_handle, parameters=None): """ Initialize the protocol with a client ID and a ROS2 node handle. @@ -98,9 +98,11 @@ def __init__(self, client_id, node_handle): :param node_handle: A ROS2 node handle """ self.client_id = client_id + self.node_handle = node_handle + self.parameters = parameters + self.capabilities = [] self.operations = {} - self.node_handle = node_handle self.external_service_list = {} self.external_action_list = {} diff --git a/rosbridge_library/src/rosbridge_library/rosbridge_protocol.py b/rosbridge_library/src/rosbridge_library/rosbridge_protocol.py index 4904bec23..4c4a5974d 100644 --- a/rosbridge_library/src/rosbridge_library/rosbridge_protocol.py +++ b/rosbridge_library/src/rosbridge_library/rosbridge_protocol.py @@ -72,7 +72,6 @@ class RosbridgeProtocol(Protocol): parameters = None def __init__(self, client_id, node_handle, parameters=None): - self.parameters = parameters - Protocol.__init__(self, client_id, node_handle) + Protocol.__init__(self, client_id, node_handle, parameters) for capability_class in self.rosbridge_capabilities: self.add_capability(capability_class) From ff3b7b07e2d1174de88cdd6c60774fb20d52a131 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Sat, 9 Aug 2025 13:54:08 +0200 Subject: [PATCH 7/8] Improve parameter handling in protocol Respect max_message_size parameter when determining fragment size, even when client provides fragment_size parameter --- .../src/rosbridge_library/protocol.py | 27 ++++++++++++------- 1 file changed, 17 insertions(+), 10 deletions(-) diff --git a/rosbridge_library/src/rosbridge_library/protocol.py b/rosbridge_library/src/rosbridge_library/protocol.py index b5f93974e..5a6d673ad 100644 --- a/rosbridge_library/src/rosbridge_library/protocol.py +++ b/rosbridge_library/src/rosbridge_library/protocol.py @@ -29,6 +29,7 @@ # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +from __future__ import annotations import time from typing import Any @@ -76,16 +77,18 @@ class Protocol: buffer = "" old_buffer = "" busy = False - # if this is too low, ("simple")clients network stacks will get flooded (when sending fragments of a huge message..) - # .. depends on message_size/bandwidth/performance/client_limits/... - # !! this might be related to (or even be avoided by using) throttle_rate !! - delay_between_messages = 0 # global list of non-ros advertised services external_service_list: dict[str, Any] # global list of non-ros advertised actions external_action_list: dict[str, Any] + + max_message_size: int = 1000000 + # if this is too low, ("simple")clients network stacks will get flooded (when sending fragments of a huge message..) + # .. depends on message_size/bandwidth/performance/client_limits/... + # !! this might be related to (or even be avoided by using) throttle_rate !! + delay_between_messages: float = 0.0 # Use only BSON for the whole communication if the server has been started with bson_only_mode:=True - bson_only_mode = False + bson_only_mode: bool = False parameters: dict[str, Any] | None = None @@ -107,9 +110,14 @@ def __init__(self, client_id, node_handle, parameters=None): self.external_action_list = {} if self.parameters: - self.fragment_size = self.parameters["max_message_size"] - self.delay_between_messages = self.parameters["delay_between_messages"] - self.bson_only_mode = self.parameters.get("bson_only_mode", False) + if "max_message_size" in self.parameters: + self.max_message_size = self.parameters["max_message_size"] + if "delay_between_messages" in self.parameters: + self.delay_between_messages = self.parameters["delay_between_messages"] + if "bson_only_mode" in self.parameters: + self.bson_only_mode = self.parameters["bson_only_mode"] + + self.fragment_size = self.max_message_size # added default message_string="" to allow recalling incoming until buffer is empty without giving a parameter # --> allows to get rid of (..or minimize) delay between client-side sends @@ -213,8 +221,7 @@ def incoming(self, message_string=""): # This way, a client can change/overwrite its active values anytime by just including parameter field in any # message sent to rosbridge. Maybe need to be improved to bind parameter values to specific operation. if "fragment_size" in msg: - self.fragment_size = msg["fragment_size"] - # print "fragment size set to:", self.fragment_size + self.fragment_size = min(msg["fragment_size"], self.max_message_size) if "message_intervall" in msg and is_number(msg["message_intervall"]): self.delay_between_messages = msg["message_intervall"] if "png" in msg: From 5553b1345625af5cf5f576adc835a874f33866cc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Sat, 9 Aug 2025 13:56:19 +0200 Subject: [PATCH 8/8] Fix passing protocol parameters in tests --- .../capabilities/test_action_capabilities.py | 8 +++++--- .../test/capabilities/test_call_service.py | 20 ++++++++++--------- .../capabilities/test_service_capabilities.py | 10 ++++++---- 3 files changed, 22 insertions(+), 16 deletions(-) diff --git a/rosbridge_library/test/capabilities/test_action_capabilities.py b/rosbridge_library/test/capabilities/test_action_capabilities.py index 9b6a7dec1..56e45692f 100755 --- a/rosbridge_library/test/capabilities/test_action_capabilities.py +++ b/rosbridge_library/test/capabilities/test_action_capabilities.py @@ -30,10 +30,12 @@ def setUp(self): self.node = Node("test_action_capabilities") self.executor.add_node(self.node) - self.node.declare_parameter("call_services_in_new_thread", False) - self.node.declare_parameter("send_action_goals_in_new_thread", False) + protocol_parameters = { + "call_services_in_new_thread": False, + "send_action_goals_in_new_thread": False, + } - self.proto = Protocol(self._testMethodName, self.node) + self.proto = Protocol(self._testMethodName, self.node, protocol_parameters) # change the log function so we can verify errors are logged self.proto.log = self.mock_log # change the send callback so we can access the rosbridge messages diff --git a/rosbridge_library/test/capabilities/test_call_service.py b/rosbridge_library/test/capabilities/test_call_service.py index 2662096d1..ef085ca95 100755 --- a/rosbridge_library/test/capabilities/test_call_service.py +++ b/rosbridge_library/test/capabilities/test_call_service.py @@ -47,9 +47,11 @@ def setUp(self): self.node = Node("test_call_service") self.executor.add_node(self.node) - self.node.declare_parameter("call_services_in_new_thread", False) - self.node.declare_parameter("default_call_service_timeout", 5.0) - self.node.declare_parameter("send_action_goals_in_new_thread", False) + self.protocol_parameters = { + "call_services_in_new_thread": False, + "default_call_service_timeout": 5.0, + "send_action_goals_in_new_thread": False, + } # Create service servers with a separate callback group self.cb_group = ReentrantCallbackGroup() @@ -83,13 +85,13 @@ def tearDown(self): rclpy.shutdown() def test_missing_arguments(self): - proto = Protocol("test_missing_arguments", self.node) + proto = Protocol("test_missing_arguments", self.node, self.protocol_parameters) s = CallService(proto) msg = loads(dumps({"op": "call_service"})) self.assertRaises(MissingArgumentException, s.call_service, msg) def test_invalid_arguments(self): - proto = Protocol("test_invalid_arguments", self.node) + proto = Protocol("test_invalid_arguments", self.node, self.protocol_parameters) s = CallService(proto) msg = loads(dumps({"op": "call_service", "service": 3})) @@ -99,7 +101,7 @@ def test_call_service_works(self): client = self.node.create_client(Trigger, self.trigger_srv.srv_name) assert client.wait_for_service(1.0) - proto = Protocol("test_call_service_works", self.node) + proto = Protocol("test_call_service_works", self.node, self.protocol_parameters) s = CallService(proto) send_msg = loads(dumps({"op": "call_service", "service": self.trigger_srv.srv_name})) @@ -123,7 +125,7 @@ def test_call_service_args(self): client = self.node.create_client(SetBool, self.set_bool_srv.srv_name) assert client.wait_for_service(1.0) - proto = Protocol("test_call_service_args", self.node) + proto = Protocol("test_call_service_args", self.node, self.protocol_parameters) s = CallService(proto) send_msg = loads( dumps( @@ -154,7 +156,7 @@ def test_call_service_fails(self): client = self.node.create_client(Trigger, self.trigger_srv.srv_name) assert client.wait_for_service(1.0) - proto = Protocol("test_call_service_works", self.node) + proto = Protocol("test_call_service_works", self.node, self.protocol_parameters) s = CallService(proto) send_msg = loads( dumps( @@ -184,7 +186,7 @@ def test_call_service_timeout(self): client = self.node.create_client(Trigger, self.trigger_long_srv.srv_name) assert client.wait_for_service(1.0) - proto = Protocol("test_call_service_timeout", self.node) + proto = Protocol("test_call_service_timeout", self.node, self.protocol_parameters) s = CallService(proto) send_msg = loads( dumps({"op": "call_service", "service": self.trigger_long_srv.srv_name, "timeout": 2.0}) diff --git a/rosbridge_library/test/capabilities/test_service_capabilities.py b/rosbridge_library/test/capabilities/test_service_capabilities.py index 188b64559..d4f0ae3a1 100755 --- a/rosbridge_library/test/capabilities/test_service_capabilities.py +++ b/rosbridge_library/test/capabilities/test_service_capabilities.py @@ -23,11 +23,13 @@ def setUp(self): rclpy.init() self.node = Node("test_service_capabilities") - self.node.declare_parameter("call_services_in_new_thread", False) - self.node.declare_parameter("default_call_service_timeout", 5.0) - self.node.declare_parameter("send_action_goals_in_new_thread", False) + protocol_parameters = { + "call_services_in_new_thread": False, + "default_call_service_timeout": 5.0, + "send_action_goals_in_new_thread": False, + } - self.proto = Protocol(self._testMethodName, self.node) + self.proto = Protocol(self._testMethodName, self.node, protocol_parameters) # change the log function so we can verify errors are logged self.proto.log = self.mock_log # change the send callback so we can access the rosbridge messages