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/CHANGELOG.rst b/CHANGELOG.rst index 3981032..cffcdb5 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -11,6 +11,7 @@ Unreleased ---------- **Added** +* Added ROS 2 action client support. **Changed** @@ -40,7 +41,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 3ba7ce9..53b1024 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 ROS 2 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 new file mode 100644 index 0000000..64d17d8 --- /dev/null +++ b/docs/files/ros2-action-client.py @@ -0,0 +1,74 @@ +from __future__ import print_function +import roslibpy +import time + + +global result + +def result_callback(msg): + global result + result = msg + print("Action result:", msg) + +def feedback_callback(msg): + print(f"Action feedback: {msg['partial_sequence']}") + +def fail_callback(msg): + print(f"Action failed: {msg}") + + +def test_action_success(action_client): + """ This test function sends a action goal to an Action server. + """ + global result + result = None + + action_client.send_goal(roslibpy.ActionGoal({"order": 8}), + result_callback, + feedback_callback, + fail_callback) + + while result == None: + time.sleep(1) + + print("-----------------------------------------------") + print("Action status:", result["status"]) + print("Action result: {}".format(result["values"]["sequence"])) + + +def test_action_cancel(action_client): + """ This test function sends a cancel request to an Action server. + NOTE: Make sure to start the "rosbridge_server" node with the parameter + "send_action_goals_in_new_thread" set to "true". + """ + global result + result = None + + goal_id = action_client.send_goal(roslibpy.ActionGoal({"order": 8}), + result_callback, + feedback_callback, + fail_callback) + time.sleep(3) + print("Sending action goal cancel request...") + action_client.cancel_goal(goal_id) + + while result == None: + time.sleep(1) + + print("-----------------------------------------------") + print("Action status:", result["status"]) + print("Action result: {}".format(result["values"]["sequence"])) + + +if __name__ == "__main__": + client = roslibpy.Ros(host="localhost", port=9090) + client.run() + + action_client = roslibpy.ActionClient(client, + "/fibonacci", + "custom_action_interfaces/action/Fibonacci") + print("\n** Starting action client test **") + test_action_success(action_client) + + print("\n** Starting action goal cancelation test **") + test_action_cancel(action_client) 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 201890a..f498199 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 @@ -82,6 +82,20 @@ class and are passed around via :class:`Topics ` using a **publish/subscr .. autoclass:: ServiceResponse :members: +Actions +-------- + +An Action client for ROS 2 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,11 @@ class and are passed around via :class:`Topics ` using a **publish/subscr __version__, ) from .core import ( + ActionClient, + ActionFeedback, + ActionGoal, + ActionGoalStatus, + ActionResult, Header, Message, Param, @@ -140,6 +159,11 @@ class and are passed around via :class:`Topics ` using a **publish/subscr "Service", "ServiceRequest", "ServiceResponse", + "ActionClient", + "ActionGoal", + "ActionGoalStatus", + "ActionFeedback", + "ActionResult", "Time", "Topic", "set_rosapi_timeout", diff --git a/src/roslibpy/comm/comm.py b/src/roslibpy/comm/comm.py index 91092a2..8a33453 100644 --- a/src/roslibpy/comm/comm.py +++ b/src/roslibpy/comm/comm.py @@ -3,7 +3,14 @@ import json import logging -from roslibpy.core import Message, MessageEncoder, ServiceResponse +from roslibpy.core import ( + ActionFeedback, + ActionGoalStatus, + ActionResult, + Message, + MessageEncoder, + ServiceResponse, +) LOGGER = logging.getLogger("roslibpy") @@ -22,19 +29,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 +117,59 @@ 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] + + LOGGER.debug("Received Action result with status: %s", message["status"]) + + results = {"status": ActionGoalStatus(message["status"]).name, "values": message["values"]} + + if "result" in message and message["result"] is False: + if errback: + errback(results) + else: + if resultback: + resultback(ActionResult(results)) diff --git a/src/roslibpy/core.py b/src/roslibpy/core.py index 6424155..a3de567 100644 --- a/src/roslibpy/core.py +++ b/src/roslibpy/core.py @@ -3,6 +3,7 @@ import json import logging import time +from enum import Enum # Python 2/3 compatibility import list try: @@ -20,6 +21,9 @@ "Service", "ServiceRequest", "ServiceResponse", + "ActionGoal", + "ActionFeedback", + "ActionResult", "Time", "Topic", ] @@ -50,7 +54,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`. """ @@ -131,6 +135,47 @@ 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 ActionGoalStatus(Enum): + """ ROS2 Action Goal statuses. + Reference: https://docs.ros2.org/latest/api/action_msgs/msg/GoalStatus.html + """ + + UNKNOWN = 0 + ACCEPTED = 1 + EXECUTING = 2 + CANCELING = 3 + SUCCEEDED = 4 + CANCELED = 5 + ABORTED = 6 + + +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 +536,77 @@ def _service_response_handler(self, request): self.ros.send_on_ready(call) +class ActionClient(object): + """Action Client of ROS 2 actions. + + Args: + ros (:class:`.Ros`): Instance of the ROS connection. + 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): + 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, resultback, feedback, errback): + """ Start a service call. + + Note: + The action client is non-blocking. + + Args: + request (:class:`.ServiceRequest`): Service request. + resultback: Callback invoked on receiving action result. + feedback: Callback invoked on receiving action feedback. + errback: Callback invoked on error. + + Returns: + object: goal ID if successfull, 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, resultback, feedback, errback) + return action_goal_id + + 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()" + """ + message = Message( + { + "op": "cancel_action_goal", + "id": goal_id, + "action": self.name, + } + ) + self.ros.send_on_ready(message) + # Remove message_id from RosBridgeProtocol._pending_action_requests in comms.py? + # Not needed since an action result is returned upon cancelation. + + class Param(object): """A ROS parameter. diff --git a/src/roslibpy/ros.py b/src/roslibpy/ros.py index c69822e..78a29c8 100644 --- a/src/roslibpy/ros.py +++ b/src/roslibpy/ros.py @@ -273,6 +273,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: Callback invoked on receiving action 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})