From 4b9f1da897914cc0f8ea5c507d138ce05eacf849 Mon Sep 17 00:00:00 2001 From: Dani Martinez Date: Sat, 29 Jun 2024 14:31:55 +0000 Subject: [PATCH 1/5] Added support to Action Client --- AUTHORS.rst | 1 + src/roslibpy/__init__.py | 22 +++++++++ src/roslibpy/comm/comm.py | 68 +++++++++++++++++++++++++-- src/roslibpy/core.py | 96 +++++++++++++++++++++++++++++++++++++++ src/roslibpy/ros.py | 18 ++++++++ 5 files changed, 202 insertions(+), 3 deletions(-) diff --git a/AUTHORS.rst b/AUTHORS.rst index 43af378..b05a4bc 100644 --- a/AUTHORS.rst +++ b/AUTHORS.rst @@ -11,3 +11,4 @@ Authors * Pedro Pereira `@MisterOwlPT `_ * Domenic Rodriguez `@DomenicP `_ * Ilia Baranov `@iliabaranov `_ +* Dani Martinez `@danmartzla `_ \ No newline at end of file diff --git a/src/roslibpy/__init__.py b/src/roslibpy/__init__.py index 201890a..9cd62ff 100644 --- a/src/roslibpy/__init__.py +++ b/src/roslibpy/__init__.py @@ -82,6 +82,20 @@ class and are passed around via :class:`Topics ` using a **publish/subscr .. autoclass:: ServiceResponse :members: +Actions +-------- + +An Action client for ROS2 Actions can be used by managing goal/feedback/result +messages via :class:`ActionClient `. + +.. autoclass:: ActionClient + :members: +.. autoclass:: ActionGoal + :members: +.. autoclass:: ActionFeedback + :members: +.. autoclass:: ActionResult + :members: Parameter server ---------------- @@ -114,6 +128,10 @@ class and are passed around via :class:`Topics ` using a **publish/subscr __version__, ) from .core import ( + ActionClient, + ActionFeedback, + ActionGoal, + ActionResult, Header, Message, Param, @@ -140,6 +158,10 @@ class and are passed around via :class:`Topics ` using a **publish/subscr "Service", "ServiceRequest", "ServiceResponse", + "ActionClient", + "ActionGoal", + "ActionFeedback", + "ActionResult", "Time", "Topic", "set_rosapi_timeout", diff --git a/src/roslibpy/comm/comm.py b/src/roslibpy/comm/comm.py index 91092a2..72d3f83 100644 --- a/src/roslibpy/comm/comm.py +++ b/src/roslibpy/comm/comm.py @@ -3,7 +3,13 @@ import json import logging -from roslibpy.core import Message, MessageEncoder, ServiceResponse +from roslibpy.core import ( + ActionFeedback, + ActionResult, + Message, + MessageEncoder, + ServiceResponse, +) LOGGER = logging.getLogger("roslibpy") @@ -22,19 +28,23 @@ def __init__(self, *args, **kwargs): super(RosBridgeProtocol, self).__init__(*args, **kwargs) self.factory = None self._pending_service_requests = {} + self._pending_action_requests = {} self._message_handlers = { "publish": self._handle_publish, "service_response": self._handle_service_response, "call_service": self._handle_service_request, + "send_action_goal": self._handle_action_request, # TODO: action server + "cancel_action_goal": self._handle_action_cancel, # TODO: action server + "action_feedback": self._handle_action_feedback, + "action_result": self._handle_action_result, + "status": None, # TODO: add handlers for op: status } - # TODO: add handlers for op: status def on_message(self, payload): message = Message(json.loads(payload.decode("utf8"))) handler = self._message_handlers.get(message["op"], None) if not handler: raise RosBridgeException('No handler registered for operation "%s"' % message["op"]) - handler(message) def send_ros_message(self, message): @@ -106,3 +116,55 @@ def _handle_service_request(self, message): raise ValueError("Expected service name missing in service request") self.factory.emit(message["service"], message) + + def send_ros_action_goal(self, message, resultback, feedback, errback): + """Initiate a ROS action request by sending a goal through the ROS Bridge. + + Args: + message (:class:`.Message`): ROS Bridge Message containing the action request. + callback: Callback invoked on receiving result. + feedback: Callback invoked when receiving feedback from action server. + errback: Callback invoked on error. + """ + request_id = message["id"] + self._pending_action_requests[request_id] = (resultback, feedback, errback) + + json_message = json.dumps(dict(message), cls=MessageEncoder).encode("utf8") + LOGGER.debug("Sending ROS action goal request: %s", json_message) + + self.send_message(json_message) + + def _handle_action_request(self, message): + if "action" not in message: + raise ValueError("Expected action name missing in action request") + raise RosBridgeException('Action server capabilities not yet implemented') + + def _handle_action_cancel(self, message): + if "action" not in message: + raise ValueError("Expected action name missing in action request") + raise RosBridgeException('Action server capabilities not yet implemented') + + def _handle_action_feedback(self, message): + if "action" not in message: + raise ValueError("Expected action name missing in action feedback") + + request_id = message["id"] + _, feedback, _ = self._pending_action_requests.get(request_id, None) + feedback(ActionFeedback(message["values"])) + + def _handle_action_result(self, message): + request_id = message["id"] + action_handlers = self._pending_action_requests.get(request_id, None) + + if not action_handlers: + raise RosBridgeException('No handler registered for action request ID: "%s"' % request_id) + + resultback, _ , errback = action_handlers + del self._pending_action_requests[request_id] + + if "result" in message and message["result"] is False: + if errback: + errback(message["values"]) + else: + if resultback: + resultback(ActionResult(message["values"])) diff --git a/src/roslibpy/core.py b/src/roslibpy/core.py index 6424155..9321818 100644 --- a/src/roslibpy/core.py +++ b/src/roslibpy/core.py @@ -20,6 +20,9 @@ "Service", "ServiceRequest", "ServiceResponse", + "ActionGoal", + "ActionFeedback", + "ActionResult", "Time", "Topic", ] @@ -131,6 +134,33 @@ def __init__(self, values=None): self.update(values) +class ActionResult(UserDict): + """Result returned from a action call.""" + + def __init__(self, values=None): + self.data = {} + if values is not None: + self.update(values) + + +class ActionFeedback(UserDict): + """Feedback returned from a action call.""" + + def __init__(self, values=None): + self.data = {} + if values is not None: + self.update(values) + + +class ActionGoal(UserDict): + """Action Goal for an action call.""" + + def __init__(self, values=None): + self.data = {} + if values is not None: + self.update(values) + + class MessageEncoder(json.JSONEncoder): """Internal class to serialize some of the core data types into json.""" @@ -491,6 +521,72 @@ def _service_response_handler(self, request): self.ros.send_on_ready(call) +class ActionClient(object): + """Action Client of ROS2 services. + + Args: + ros (:class:`.Ros`): Instance of the ROS connection. + name (:obj:`str`): Service name, e.g. ``/add_two_ints``. + service_type (:obj:`str`): Service type, e.g. ``rospy_tutorials/AddTwoInts``. + """ + + def __init__(self, ros, name, action_type, reconnect_on_close=True): + self.ros = ros + self.name = name + self.action_type = action_type + + self._service_callback = None + self._is_advertised = False + self.reconnect_on_close = reconnect_on_close + + def send_goal(self, goal, result_back, feedback_back, failed_back): + """ Start a service call. + + Note: + The service can be used either as blocking or non-blocking. + If the ``callback`` parameter is ``None``, then the call will + block until receiving a response. Otherwise, the service response + will be returned in the callback. + + Args: + request (:class:`.ServiceRequest`): Service request. + callback: Callback invoked on successful execution. + errback: Callback invoked on error. + timeout: Timeout for the operation, in seconds. Only used if blocking. + + Returns: + object: Service response if used as a blocking call, otherwise ``None``. + """ + if self._is_advertised: + return + + action_goal_id = "send_action_goal:%s:%d" % (self.name, self.ros.id_counter) + + message = Message( + { + "op": "send_action_goal", + "id": action_goal_id, + "action": self.name, + "action_type": self.action_type, + "args": dict(goal), + "feedback": True, + } + ) + + self.ros.call_async_action(message, result_back, feedback_back, failed_back) + return action_goal_id + + def cancel_goal(self, goal_id): + message = Message( + { + "op": "cancel_action_goal", + "id": goal_id, + "action": self.name, + } + ) + self.ros.send_on_ready(message) + + class Param(object): """A ROS parameter. diff --git a/src/roslibpy/ros.py b/src/roslibpy/ros.py index 72ff7d6..c36bf67 100644 --- a/src/roslibpy/ros.py +++ b/src/roslibpy/ros.py @@ -272,6 +272,24 @@ def _send_internal(proto): self.factory.on_ready(_send_internal) + def call_async_action(self, message, resultback, feedback, errback): + """Send a action request to ROS once the connection is established. + + If a connection to ROS is already available, the request is sent immediately. + + Args: + message (:class:`.Message`): ROS Bridge Message containing the request. + resultback: Callback invoked on successful execution. + feedback: + errback: Callback invoked on error. + """ + + def _send_internal(proto): + proto.send_ros_action_goal(message, resultback, feedback, errback) + return proto + + self.factory.on_ready(_send_internal) + def set_status_level(self, level, identifier): level_message = Message({"op": "set_level", "level": level, "id": identifier}) From a86bdcffab7d412d52793a8b6fe9da1ab3ca838a Mon Sep 17 00:00:00 2001 From: Dani Martinez Date: Sat, 29 Jun 2024 15:25:01 +0000 Subject: [PATCH 2/5] Minor comments fix --- src/roslibpy/core.py | 26 ++++++++++++++------------ src/roslibpy/ros.py | 2 +- 2 files changed, 15 insertions(+), 13 deletions(-) diff --git a/src/roslibpy/core.py b/src/roslibpy/core.py index 9321818..a86ee2e 100644 --- a/src/roslibpy/core.py +++ b/src/roslibpy/core.py @@ -522,12 +522,12 @@ def _service_response_handler(self, request): class ActionClient(object): - """Action Client of ROS2 services. + """Action Client of ROS2 actions. Args: ros (:class:`.Ros`): Instance of the ROS connection. - name (:obj:`str`): Service name, e.g. ``/add_two_ints``. - service_type (:obj:`str`): Service type, e.g. ``rospy_tutorials/AddTwoInts``. + name (:obj:`str`): Service name, e.g. ``/fibonacci``. + action_type (:obj:`str`): Action type, e.g. ``rospy_tutorials/fibonacci``. """ def __init__(self, ros, name, action_type, reconnect_on_close=True): @@ -539,23 +539,20 @@ def __init__(self, ros, name, action_type, reconnect_on_close=True): self._is_advertised = False self.reconnect_on_close = reconnect_on_close - def send_goal(self, goal, result_back, feedback_back, failed_back): + def send_goal(self, goal, resultback, feedback, errback): """ Start a service call. Note: - The service can be used either as blocking or non-blocking. - If the ``callback`` parameter is ``None``, then the call will - block until receiving a response. Otherwise, the service response - will be returned in the callback. + The action client is non-blocking. Args: request (:class:`.ServiceRequest`): Service request. - callback: Callback invoked on successful execution. + resultback: Callback invoked on receiving action result. + feedback: Callback invoked on receiving action feedback. errback: Callback invoked on error. - timeout: Timeout for the operation, in seconds. Only used if blocking. Returns: - object: Service response if used as a blocking call, otherwise ``None``. + object: goal ID if successfull, otherwise ``None``. """ if self._is_advertised: return @@ -573,10 +570,15 @@ def send_goal(self, goal, result_back, feedback_back, failed_back): } ) - self.ros.call_async_action(message, result_back, feedback_back, failed_back) + self.ros.call_async_action(message, resultback, feedback, errback) return action_goal_id def cancel_goal(self, goal_id): + """ Cancel an ongoing action. + + Args: + goal_id: Goal ID returned from "send_goal()" + """ message = Message( { "op": "cancel_action_goal", diff --git a/src/roslibpy/ros.py b/src/roslibpy/ros.py index c36bf67..ceaf9d2 100644 --- a/src/roslibpy/ros.py +++ b/src/roslibpy/ros.py @@ -280,7 +280,7 @@ def call_async_action(self, message, resultback, feedback, errback): Args: message (:class:`.Message`): ROS Bridge Message containing the request. resultback: Callback invoked on successful execution. - feedback: + feedback: Callback invoked on receiving action feedback. errback: Callback invoked on error. """ From 33a58cf8e320a32aaa173d10ad9baf966ddc62fc Mon Sep 17 00:00:00 2001 From: Dani Martinez Date: Sun, 30 Jun 2024 11:28:57 +0000 Subject: [PATCH 3/5] action client example added --- docs/files/ros2-action-client.py | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) create mode 100644 docs/files/ros2-action-client.py diff --git a/docs/files/ros2-action-client.py b/docs/files/ros2-action-client.py new file mode 100644 index 0000000..1ed5c2f --- /dev/null +++ b/docs/files/ros2-action-client.py @@ -0,0 +1,30 @@ +from __future__ import print_function +import roslibpy + +client = roslibpy.Ros(host='localhost', port=9090) +client.run() + +action_client = roslibpy.ActionClient(client, + '/fibonacci', + 'custom_action_interfaces/action/Fibonacci') + +def result_callback(msg): + print('Action result:',msg['sequence']) + +def feedback_callback(msg): + print('Action feedback:',msg['partial_sequence']) + +def fail_callback(msg): + print('Action failed:',msg) + +goal_id = action_client.send_goal(roslibpy.ActionGoal({'order': 8}), + result_callback, + feedback_callback, + fail_callback) + +goal.on('feedback', lambda f: print(f['sequence'])) +goal.send() +result = goal.wait(10) +action_client.dispose() + +print('Result: {}'.format(result['sequence'])) From 7f3f0668c6f585b71290b47f47ceda4ca513e440 Mon Sep 17 00:00:00 2001 From: Dani Martinez Date: Sun, 30 Jun 2024 11:29:33 +0000 Subject: [PATCH 4/5] Minor docs fix --- CHANGELOG.rst | 1 + docs/examples.rst | 7 +++++++ docs/files/ros2-action-client.py | 18 +++++++++++------- src/roslibpy/core.py | 2 ++ 4 files changed, 21 insertions(+), 7 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 067e65a..abf6f1c 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -11,6 +11,7 @@ Unreleased ---------- **Added** +* Added ROS2 action client object with limited capabilities ``roslibpy.ActionClient``. **Changed** diff --git a/docs/examples.rst b/docs/examples.rst index 3ba7ce9..f565c36 100644 --- a/docs/examples.rst +++ b/docs/examples.rst @@ -246,6 +246,13 @@ This example is very simplified and uses the :meth:`roslibpy.actionlib.Goal.wait function to make the code easier to read as an example. A more robust way to handle results is to hook up to the ``result`` event with a callback. +For action clients to deal with ROS2 action servers, check the following example: + +.. literalinclude :: files/ros2-action-client.py + :language: python + +* :download:`ros2-action-client.py ` + Querying ROS API ---------------- diff --git a/docs/files/ros2-action-client.py b/docs/files/ros2-action-client.py index 1ed5c2f..cdcabdc 100644 --- a/docs/files/ros2-action-client.py +++ b/docs/files/ros2-action-client.py @@ -1,5 +1,9 @@ from __future__ import print_function import roslibpy +import time + + +global result client = roslibpy.Ros(host='localhost', port=9090) client.run() @@ -7,9 +11,11 @@ action_client = roslibpy.ActionClient(client, '/fibonacci', 'custom_action_interfaces/action/Fibonacci') +result = None def result_callback(msg): - print('Action result:',msg['sequence']) + global result + result = msg['result'] def feedback_callback(msg): print('Action feedback:',msg['partial_sequence']) @@ -22,9 +28,7 @@ def fail_callback(msg): feedback_callback, fail_callback) -goal.on('feedback', lambda f: print(f['sequence'])) -goal.send() -result = goal.wait(10) -action_client.dispose() - -print('Result: {}'.format(result['sequence'])) +while result == None: + time.sleep(1) + +print('Action result: {}'.format(result['sequence'])) diff --git a/src/roslibpy/core.py b/src/roslibpy/core.py index a86ee2e..d3a8a1a 100644 --- a/src/roslibpy/core.py +++ b/src/roslibpy/core.py @@ -575,6 +575,7 @@ def send_goal(self, goal, resultback, feedback, errback): def cancel_goal(self, goal_id): """ Cancel an ongoing action. + NOTE: Async cancelation is not yet supported on rosbridge (rosbridge_suite issue #909) Args: goal_id: Goal ID returned from "send_goal()" @@ -587,6 +588,7 @@ def cancel_goal(self, goal_id): } ) self.ros.send_on_ready(message) + # Remove message_id from RosBridgeProtocol._pending_action_requests in comms.py? class Param(object): From 7c7bb32b8e70e5dc4bef633837e8b39456ce1ed6 Mon Sep 17 00:00:00 2001 From: Dani Martinez Date: Mon, 19 Aug 2024 15:22:20 +0000 Subject: [PATCH 5/5] Minor fixes --- CHANGELOG.rst | 4 ++-- README.rst | 2 +- docs/examples.rst | 2 +- docs/files/ros2-action-client.py | 16 ++++++++-------- docs/index.rst | 2 +- src/roslibpy/__init__.py | 8 ++++---- src/roslibpy/core.py | 4 ++-- 7 files changed, 19 insertions(+), 19 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index abf6f1c..6672b8d 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -11,7 +11,7 @@ Unreleased ---------- **Added** -* Added ROS2 action client object with limited capabilities ``roslibpy.ActionClient``. +* Added ROS 2 action client object with limited capabilities ``roslibpy.ActionClient``. **Changed** @@ -26,7 +26,7 @@ Unreleased **Added** -* Added a ROS2-compatible header class in ``roslibpy.ros2.Header``. +* Added a ROS 2 compatible header class in ``roslibpy.ros2.Header``. **Changed** diff --git a/README.rst b/README.rst index bdeabfb..017656f 100644 --- a/README.rst +++ b/README.rst @@ -44,7 +44,7 @@ local ROS environment, allowing usage from platforms other than Linux. The API of **roslibpy** is modeled to closely match that of `roslibjs`_. -ROS1 is fully supported. ROS2 support is still in progress. +ROS 1 is fully supported. ROS 2 support is still in progress. Main features diff --git a/docs/examples.rst b/docs/examples.rst index f565c36..53b1024 100644 --- a/docs/examples.rst +++ b/docs/examples.rst @@ -246,7 +246,7 @@ This example is very simplified and uses the :meth:`roslibpy.actionlib.Goal.wait function to make the code easier to read as an example. A more robust way to handle results is to hook up to the ``result`` event with a callback. -For action clients to deal with ROS2 action servers, check the following example: +For action clients to deal with ROS 2 action servers, check the following example: .. literalinclude :: files/ros2-action-client.py :language: python diff --git a/docs/files/ros2-action-client.py b/docs/files/ros2-action-client.py index cdcabdc..df75f23 100644 --- a/docs/files/ros2-action-client.py +++ b/docs/files/ros2-action-client.py @@ -5,25 +5,25 @@ global result -client = roslibpy.Ros(host='localhost', port=9090) +client = roslibpy.Ros(host="localhost", port=9090) client.run() action_client = roslibpy.ActionClient(client, - '/fibonacci', - 'custom_action_interfaces/action/Fibonacci') + "/fibonacci", + "custom_action_interfaces/action/Fibonacci") result = None def result_callback(msg): global result - result = msg['result'] + result = msg["result"] def feedback_callback(msg): - print('Action feedback:',msg['partial_sequence']) + print(f"Action feedback: {msg['partial_sequence']}") def fail_callback(msg): - print('Action failed:',msg) + print(f"Action failed: {msg}") -goal_id = action_client.send_goal(roslibpy.ActionGoal({'order': 8}), +goal_id = action_client.send_goal(roslibpy.ActionGoal({"order": 8}), result_callback, feedback_callback, fail_callback) @@ -31,4 +31,4 @@ def fail_callback(msg): while result == None: time.sleep(1) -print('Action result: {}'.format(result['sequence'])) +print("Action result: {}".format(result["sequence"])) diff --git a/docs/index.rst b/docs/index.rst index 5736c82..e2e5652 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -44,7 +44,7 @@ local ROS environment, allowing usage from platforms other than Linux. The API of **roslibpy** is modeled to closely match that of `roslibjs `_. -ROS1 is fully supported. ROS2 support is still in progress. +ROS 1 is fully supported. ROS 2 support is still in progress. ======== Contents diff --git a/src/roslibpy/__init__.py b/src/roslibpy/__init__.py index 9cd62ff..39ae3f6 100644 --- a/src/roslibpy/__init__.py +++ b/src/roslibpy/__init__.py @@ -41,13 +41,13 @@ Main ROS concepts ================= -ROS1 vs ROS2 +ROS 1 vs ROS 2 ------------ -This library has been tested to work with ROS1. ROS2 should work, but it is still +This library has been tested to work with ROS 1. ROS 2 should work, but it is still in the works. -One area in which ROS1 and ROS2 differ is in the header interface. To use ROS2, use +One area in which ROS 1 and ROS 2 differ is in the header interface. To use ROS 2, use the header defined in the `roslibpy.ros2` module. .. autoclass:: roslibpy.ros2.Header @@ -85,7 +85,7 @@ class and are passed around via :class:`Topics ` using a **publish/subscr Actions -------- -An Action client for ROS2 Actions can be used by managing goal/feedback/result +An Action client for ROS 2 Actions can be used by managing goal/feedback/result messages via :class:`ActionClient `. .. autoclass:: ActionClient diff --git a/src/roslibpy/core.py b/src/roslibpy/core.py index d3a8a1a..10c2cc2 100644 --- a/src/roslibpy/core.py +++ b/src/roslibpy/core.py @@ -53,7 +53,7 @@ def __init__(self, values=None): class Header(UserDict): """Represents a message header of the ROS type std_msgs/Header. - This header is only compatible with ROS1. For ROS2 headers, use :class:`roslibpy.ros2.Header`. + This header is only compatible with ROS 1. For ROS 2 headers, use :class:`roslibpy.ros2.Header`. """ @@ -522,7 +522,7 @@ def _service_response_handler(self, request): class ActionClient(object): - """Action Client of ROS2 actions. + """Action Client of ROS 2 actions. Args: ros (:class:`.Ros`): Instance of the ROS connection.