From 9df09d9752e84e120e806f97f2309ac659cc07cf Mon Sep 17 00:00:00 2001 From: ChenDongfag Date: Fri, 16 May 2025 21:54:06 +0800 Subject: [PATCH 01/10] modified: rosbridge_library/src/rosbridge_library/capabilities/subscribe.py --- .../capabilities/subscribe.py | 71 ++++++++++--------- 1 file changed, 39 insertions(+), 32 deletions(-) diff --git a/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py b/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py index 0f523ab22..3c81ba592 100644 --- a/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py +++ b/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py @@ -110,6 +110,18 @@ def subscribe( compression is to be used (current valid values are 'png') """ + if sid is None: + self.protocol.log( + "WARNING: subscribe called with no subscription id, " + + "this is not supported by ROSBridge" + ) + raise ValueError("Subscription ID cannot be None") + if sid in self.clients: + self.protocol.log( + "WARNING: subscribe called with existing subscription id, " + + "this is not supported by ROSBridge" + ) + raise KeyError(f"Subscription ID {sid} already exists") client_details = { "throttle_rate": throttle_rate, @@ -296,38 +308,33 @@ def unsubscribe(self, msg): self.protocol.log("info", "Unsubscribed from %s" % topic) def publish(self, topic, message, fragment_size=None, compression="none"): - """Publish a message to the client - - Keyword arguments: - topic -- the topic to publish the message on - message -- a ROS message wrapped by OutgoingMessage - fragment_size -- (optional) fragment the serialized message into msgs - with payloads not greater than this value - compression -- (optional) compress the message. valid values are - 'png' and 'none' - - """ - # TODO: fragmentation, proper ids - - outgoing_msg = {"op": "publish", "topic": topic} - if compression == "png": - outgoing_msg["msg"] = message.get_json_values() - outgoing_msg_dumped = encode_json(outgoing_msg) - outgoing_msg = {"op": "png", "data": encode_png(outgoing_msg_dumped)} - elif compression == "cbor": - outgoing_msg = message.get_cbor(outgoing_msg) - elif compression == "cbor-raw": - (secs, nsecs) = self.protocol.node_handle.get_clock().now().seconds_nanoseconds() - outgoing_msg["msg"] = { - "secs": secs, - "nsecs": nsecs, - "bytes": message.message, - } - outgoing_msg = message.get_cbor_raw(outgoing_msg) - else: - outgoing_msg["msg"] = message.get_json_values() - - self.protocol.send(outgoing_msg, compression=compression) + # 遍历当前主题下的所有订阅客户端 + subscription = self._subscriptions.get(topic) + if subscription: + # 为每个客户端生成独立的消息 + for sid in subscription.clients: + outgoing_msg = {"op": "publish", "topic": topic, "id": sid} + if compression == "png": + outgoing_msg["msg"] = message.get_json_values() + outgoing_msg_dumped = encode_json(outgoing_msg) + outgoing_msg = {"op": "png", "data": encode_png(outgoing_msg_dumped), "id": sid} + elif compression == "cbor": + outgoing_msg = message.get_cbor(outgoing_msg) + outgoing_msg["id"] = sid + elif compression == "cbor-raw": + (secs, nsecs) = self.protocol.node_handle.get_clock().now().seconds_nanoseconds() + outgoing_msg["msg"] = { + "secs": secs, + "nsecs": nsecs, + "bytes": message.message, + } + outgoing_msg = message.get_cbor_raw(outgoing_msg) + outgoing_msg["id"] = sid + else: + outgoing_msg["msg"] = message.get_json_values() + outgoing_msg["id"] = sid + + self.protocol.send(outgoing_msg, compression=compression) def finish(self): for subscription in self._subscriptions.values(): From 04e133fb184a7114a531466594c824744985f327 Mon Sep 17 00:00:00 2001 From: ChenDongfag Date: Fri, 16 May 2025 21:59:28 +0800 Subject: [PATCH 02/10] modified: rosbridge_library/src/rosbridge_library/capabilities/subscribe.py --- .../rosbridge_library/capabilities/subscribe.py | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py b/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py index 3c81ba592..9d8c6a57b 100644 --- a/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py +++ b/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py @@ -308,10 +308,20 @@ def unsubscribe(self, msg): self.protocol.log("info", "Unsubscribed from %s" % topic) def publish(self, topic, message, fragment_size=None, compression="none"): - # 遍历当前主题下的所有订阅客户端 + """Publish a message to the client + + Keyword arguments: + topic -- the topic to publish the message on + message -- a ROS message wrapped by OutgoingMessage + fragment_size -- (optional) fragment the serialized message into msgs + with payloads not greater than this value + compression -- (optional) compress the message. valid values are + 'png' and 'none' + + """ + subscription = self._subscriptions.get(topic) if subscription: - # 为每个客户端生成独立的消息 for sid in subscription.clients: outgoing_msg = {"op": "publish", "topic": topic, "id": sid} if compression == "png": From cfbb75c8cd31e019c7464471e495c23116c03bf4 Mon Sep 17 00:00:00 2001 From: ChenDongfag Date: Fri, 16 May 2025 22:10:40 +0800 Subject: [PATCH 03/10] modified: rosbridge_library/src/rosbridge_library/capabilities/subscribe.py --- .../src/rosbridge_library/capabilities/subscribe.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py b/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py index 9d8c6a57b..41930eeb8 100644 --- a/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py +++ b/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py @@ -317,7 +317,7 @@ def publish(self, topic, message, fragment_size=None, compression="none"): with payloads not greater than this value compression -- (optional) compress the message. valid values are 'png' and 'none' - + """ subscription = self._subscriptions.get(topic) @@ -332,7 +332,9 @@ def publish(self, topic, message, fragment_size=None, compression="none"): outgoing_msg = message.get_cbor(outgoing_msg) outgoing_msg["id"] = sid elif compression == "cbor-raw": - (secs, nsecs) = self.protocol.node_handle.get_clock().now().seconds_nanoseconds() + (secs, nsecs) = ( + self.protocol.node_handle.get_clock().now().seconds_nanoseconds() + ) outgoing_msg["msg"] = { "secs": secs, "nsecs": nsecs, From 53b87aab90e8724f65993f0cd890dfbe8bfba3df Mon Sep 17 00:00:00 2001 From: dongfang Date: Fri, 13 Jun 2025 17:00:12 +0800 Subject: [PATCH 04/10] modified: rosapi/src/rosapi/params.py --- rosapi/src/rosapi/params.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosapi/src/rosapi/params.py b/rosapi/src/rosapi/params.py index fb1aaa7d9..adcfd582b 100644 --- a/rosapi/src/rosapi/params.py +++ b/rosapi/src/rosapi/params.py @@ -38,7 +38,7 @@ from rcl_interfaces.srv import GetParameters, ListParameters, SetParameters from rclpy.callback_groups import MutuallyExclusiveCallbackGroup from rclpy.node import Node -from rclpy.parameter import get_parameter_value +from rclpy.parameter import Parameter from rclpy.task import Future from ros2node.api import get_absolute_node_name from rosapi.async_helper import futures_wait_for From df9b84927298649474f8e2559f9278093b48b64a Mon Sep 17 00:00:00 2001 From: ChenDongfag Date: Tue, 24 Jun 2025 11:49:29 +0800 Subject: [PATCH 05/10] modified: rosbridge_library/src/rosbridge_library/capabilities/subscribe.py --- .../src/rosbridge_library/capabilities/subscribe.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py b/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py index 41930eeb8..09a998840 100644 --- a/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py +++ b/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py @@ -340,8 +340,8 @@ def publish(self, topic, message, fragment_size=None, compression="none"): "nsecs": nsecs, "bytes": message.message, } - outgoing_msg = message.get_cbor_raw(outgoing_msg) outgoing_msg["id"] = sid + outgoing_msg = message.get_cbor_raw(outgoing_msg) else: outgoing_msg["msg"] = message.get_json_values() outgoing_msg["id"] = sid From 1721833921364f53347e9a3ec72c4071b118dac6 Mon Sep 17 00:00:00 2001 From: ChenDongfag Date: Tue, 24 Jun 2025 11:53:10 +0800 Subject: [PATCH 06/10] modified: rosapi/src/rosapi/params.py --- rosapi/src/rosapi/params.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosapi/src/rosapi/params.py b/rosapi/src/rosapi/params.py index 3af991ebc..20c79c35d 100644 --- a/rosapi/src/rosapi/params.py +++ b/rosapi/src/rosapi/params.py @@ -39,7 +39,7 @@ from rcl_interfaces.srv import GetParameters, ListParameters, SetParameters from rclpy.callback_groups import MutuallyExclusiveCallbackGroup from rclpy.node import Node -from rclpy.parameter import Parameter +from rclpy.parameter import get_parameter_value from rclpy.task import Future from ros2node.api import get_absolute_node_name from rosapi.async_helper import futures_wait_for From 589d0e243a8b1d2c3054c40a9abed2ac275b7fb7 Mon Sep 17 00:00:00 2001 From: ChenDongfag Date: Tue, 24 Jun 2025 12:06:42 +0800 Subject: [PATCH 07/10] modified: rosbridge_library/src/rosbridge_library/capabilities/subscribe.py --- .../src/rosbridge_library/capabilities/subscribe.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py b/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py index 09a998840..590f91d04 100644 --- a/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py +++ b/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py @@ -38,6 +38,7 @@ from rosbridge_library.internal.pngcompression import encode as encode_png from rosbridge_library.internal.subscribers import manager from rosbridge_library.internal.subscription_modifiers import MessageHandler +import uuid try: from ujson import dumps as encode_json # type: ignore[import-untyped] @@ -115,13 +116,15 @@ def subscribe( "WARNING: subscribe called with no subscription id, " + "this is not supported by ROSBridge" ) - raise ValueError("Subscription ID cannot be None") + sid = uuid.uuid4() + #raise ValueError("Subscription ID cannot be None") if sid in self.clients: self.protocol.log( "WARNING: subscribe called with existing subscription id, " + "this is not supported by ROSBridge" ) - raise KeyError(f"Subscription ID {sid} already exists") + sid = uuid.uuid4() + #raise KeyError(f"Subscription ID {sid} already exists") client_details = { "throttle_rate": throttle_rate, From e1e0f5ed6b851940c47592f141524705d912df08 Mon Sep 17 00:00:00 2001 From: ChenDongfag Date: Tue, 24 Jun 2025 12:09:16 +0800 Subject: [PATCH 08/10] modified: rosbridge_library/src/rosbridge_library/capabilities/subscribe.py --- .../src/rosbridge_library/capabilities/subscribe.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py b/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py index 590f91d04..87d999128 100644 --- a/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py +++ b/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py @@ -31,6 +31,7 @@ # POSSIBILITY OF SUCH DAMAGE. import fnmatch +import uuid from functools import partial from threading import Lock @@ -38,7 +39,6 @@ from rosbridge_library.internal.pngcompression import encode as encode_png from rosbridge_library.internal.subscribers import manager from rosbridge_library.internal.subscription_modifiers import MessageHandler -import uuid try: from ujson import dumps as encode_json # type: ignore[import-untyped] @@ -117,14 +117,13 @@ def subscribe( + "this is not supported by ROSBridge" ) sid = uuid.uuid4() - #raise ValueError("Subscription ID cannot be None") + if sid in self.clients: self.protocol.log( "WARNING: subscribe called with existing subscription id, " + "this is not supported by ROSBridge" ) sid = uuid.uuid4() - #raise KeyError(f"Subscription ID {sid} already exists") client_details = { "throttle_rate": throttle_rate, From 53dc09a240323e7bc8862728b927599a18f9cf67 Mon Sep 17 00:00:00 2001 From: ChenDongfag Date: Tue, 24 Jun 2025 14:33:40 +0800 Subject: [PATCH 09/10] modified: rosbridge_library/src/rosbridge_library/capabilities/subscribe.py --- .../src/rosbridge_library/capabilities/subscribe.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py b/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py index 87d999128..1aab0ed5a 100644 --- a/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py +++ b/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py @@ -331,8 +331,8 @@ def publish(self, topic, message, fragment_size=None, compression="none"): outgoing_msg_dumped = encode_json(outgoing_msg) outgoing_msg = {"op": "png", "data": encode_png(outgoing_msg_dumped), "id": sid} elif compression == "cbor": - outgoing_msg = message.get_cbor(outgoing_msg) outgoing_msg["id"] = sid + outgoing_msg = message.get_cbor(outgoing_msg) elif compression == "cbor-raw": (secs, nsecs) = ( self.protocol.node_handle.get_clock().now().seconds_nanoseconds() From 363021ed2e90e17e208617b875539f25171250ff Mon Sep 17 00:00:00 2001 From: dongfang Date: Tue, 24 Jun 2025 15:29:50 +0800 Subject: [PATCH 10/10] modified: rosapi/src/rosapi/params.py modified: rosbridge_library/src/rosbridge_library/capabilities/subscribe.py --- rosapi/src/rosapi/params.py | 2 +- .../src/rosbridge_library/capabilities/subscribe.py | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/rosapi/src/rosapi/params.py b/rosapi/src/rosapi/params.py index 20c79c35d..3af991ebc 100644 --- a/rosapi/src/rosapi/params.py +++ b/rosapi/src/rosapi/params.py @@ -39,7 +39,7 @@ from rcl_interfaces.srv import GetParameters, ListParameters, SetParameters from rclpy.callback_groups import MutuallyExclusiveCallbackGroup from rclpy.node import Node -from rclpy.parameter import get_parameter_value +from rclpy.parameter import Parameter from rclpy.task import Future from ros2node.api import get_absolute_node_name from rosapi.async_helper import futures_wait_for diff --git a/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py b/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py index 87d999128..ef411ee7c 100644 --- a/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py +++ b/rosbridge_library/src/rosbridge_library/capabilities/subscribe.py @@ -331,8 +331,9 @@ def publish(self, topic, message, fragment_size=None, compression="none"): outgoing_msg_dumped = encode_json(outgoing_msg) outgoing_msg = {"op": "png", "data": encode_png(outgoing_msg_dumped), "id": sid} elif compression == "cbor": - outgoing_msg = message.get_cbor(outgoing_msg) outgoing_msg["id"] = sid + outgoing_msg = message.get_cbor(outgoing_msg) + elif compression == "cbor-raw": (secs, nsecs) = ( self.protocol.node_handle.get_clock().now().seconds_nanoseconds()