Skip to content

Commit 75a63b8

Browse files
danmartzlaDani Martinez
andauthored
Added ROS 2 ActionClient support (#125)
Co-authored-by: Dani Martinez <[email protected]>
1 parent 0c732f5 commit 75a63b8

File tree

10 files changed

+318
-10
lines changed

10 files changed

+318
-10
lines changed

AUTHORS.rst

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -11,3 +11,4 @@ Authors
1111
* Pedro Pereira `@MisterOwlPT <https://github.com/MisterOwlPT>`_
1212
* Domenic Rodriguez `@DomenicP <https://github.com/DomenicP>`_
1313
* Ilia Baranov `@iliabaranov <https://github.com/iliabaranov>`_
14+
* Dani Martinez `@danmartzla <https://github.com/danmartzla>`_

CHANGELOG.rst

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@ Unreleased
1111
----------
1212

1313
**Added**
14+
* Added ROS 2 action client support.
1415

1516
**Changed**
1617

@@ -55,7 +56,7 @@ Unreleased
5556

5657
**Added**
5758

58-
* Added a ROS2-compatible header class in ``roslibpy.ros2.Header``.
59+
* Added a ROS 2 compatible header class in ``roslibpy.ros2.Header``.
5960

6061
**Changed**
6162

README.rst

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -44,7 +44,7 @@ local ROS environment, allowing usage from platforms other than Linux.
4444

4545
The API of **roslibpy** is modeled to closely match that of `roslibjs`_.
4646

47-
ROS1 is fully supported. ROS2 support is still in progress.
47+
ROS 1 is fully supported. ROS 2 support is still in progress.
4848

4949

5050
Main features

docs/examples.rst

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -246,6 +246,13 @@ This example is very simplified and uses the :meth:`roslibpy.actionlib.Goal.wait
246246
function to make the code easier to read as an example. A more robust way to handle
247247
results is to hook up to the ``result`` event with a callback.
248248

249+
For action clients to deal with ROS 2 action servers, check the following example:
250+
251+
.. literalinclude :: files/ros2-action-client.py
252+
:language: python
253+
254+
* :download:`ros2-action-client.py <files/ros2-action-client.py>`
255+
249256
Querying ROS API
250257
----------------
251258

docs/files/ros2-action-client.py

Lines changed: 74 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,74 @@
1+
from __future__ import print_function
2+
import roslibpy
3+
import time
4+
5+
6+
global result
7+
8+
def result_callback(msg):
9+
global result
10+
result = msg
11+
print("Action result:", msg)
12+
13+
def feedback_callback(msg):
14+
print(f"Action feedback: {msg['partial_sequence']}")
15+
16+
def fail_callback(msg):
17+
print(f"Action failed: {msg}")
18+
19+
20+
def test_action_success(action_client):
21+
""" This test function sends a action goal to an Action server.
22+
"""
23+
global result
24+
result = None
25+
26+
action_client.send_goal(roslibpy.ActionGoal({"order": 8}),
27+
result_callback,
28+
feedback_callback,
29+
fail_callback)
30+
31+
while result == None:
32+
time.sleep(1)
33+
34+
print("-----------------------------------------------")
35+
print("Action status:", result["status"])
36+
print("Action result: {}".format(result["values"]["sequence"]))
37+
38+
39+
def test_action_cancel(action_client):
40+
""" This test function sends a cancel request to an Action server.
41+
NOTE: Make sure to start the "rosbridge_server" node with the parameter
42+
"send_action_goals_in_new_thread" set to "true".
43+
"""
44+
global result
45+
result = None
46+
47+
goal_id = action_client.send_goal(roslibpy.ActionGoal({"order": 8}),
48+
result_callback,
49+
feedback_callback,
50+
fail_callback)
51+
time.sleep(3)
52+
print("Sending action goal cancel request...")
53+
action_client.cancel_goal(goal_id)
54+
55+
while result == None:
56+
time.sleep(1)
57+
58+
print("-----------------------------------------------")
59+
print("Action status:", result["status"])
60+
print("Action result: {}".format(result["values"]["sequence"]))
61+
62+
63+
if __name__ == "__main__":
64+
client = roslibpy.Ros(host="localhost", port=9090)
65+
client.run()
66+
67+
action_client = roslibpy.ActionClient(client,
68+
"/fibonacci",
69+
"custom_action_interfaces/action/Fibonacci")
70+
print("\n** Starting action client test **")
71+
test_action_success(action_client)
72+
73+
print("\n** Starting action goal cancelation test **")
74+
test_action_cancel(action_client)

docs/index.rst

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -44,7 +44,7 @@ local ROS environment, allowing usage from platforms other than Linux.
4444

4545
The API of **roslibpy** is modeled to closely match that of `roslibjs <http://wiki.ros.org/roslibjs>`_.
4646

47-
ROS1 is fully supported. ROS2 support is still in progress.
47+
ROS 1 is fully supported. ROS 2 support is still in progress.
4848

4949
========
5050
Contents

src/roslibpy/__init__.py

Lines changed: 27 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -41,13 +41,13 @@
4141
Main ROS concepts
4242
=================
4343
44-
ROS1 vs ROS2
44+
ROS 1 vs ROS 2
4545
------------
4646
47-
This library has been tested to work with ROS1. ROS2 should work, but it is still
47+
This library has been tested to work with ROS 1. ROS 2 should work, but it is still
4848
in the works.
4949
50-
One area in which ROS1 and ROS2 differ is in the header interface. To use ROS2, use
50+
One area in which ROS 1 and ROS 2 differ is in the header interface. To use ROS 2, use
5151
the header defined in the `roslibpy.ros2` module.
5252
5353
.. autoclass:: roslibpy.ros2.Header
@@ -82,6 +82,20 @@ class and are passed around via :class:`Topics <Topic>` using a **publish/subscr
8282
.. autoclass:: ServiceResponse
8383
:members:
8484
85+
Actions
86+
--------
87+
88+
An Action client for ROS 2 Actions can be used by managing goal/feedback/result
89+
messages via :class:`ActionClient <ActionClient>`.
90+
91+
.. autoclass:: ActionClient
92+
:members:
93+
.. autoclass:: ActionGoal
94+
:members:
95+
.. autoclass:: ActionFeedback
96+
:members:
97+
.. autoclass:: ActionResult
98+
:members:
8599
86100
Parameter server
87101
----------------
@@ -114,6 +128,11 @@ class and are passed around via :class:`Topics <Topic>` using a **publish/subscr
114128
__version__,
115129
)
116130
from .core import (
131+
ActionClient,
132+
ActionFeedback,
133+
ActionGoal,
134+
ActionGoalStatus,
135+
ActionResult,
117136
Header,
118137
Message,
119138
Param,
@@ -140,6 +159,11 @@ class and are passed around via :class:`Topics <Topic>` using a **publish/subscr
140159
"Service",
141160
"ServiceRequest",
142161
"ServiceResponse",
162+
"ActionClient",
163+
"ActionGoal",
164+
"ActionGoalStatus",
165+
"ActionFeedback",
166+
"ActionResult",
143167
"Time",
144168
"Topic",
145169
"set_rosapi_timeout",

src/roslibpy/comm/comm.py

Lines changed: 70 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,14 @@
33
import json
44
import logging
55

6-
from roslibpy.core import Message, MessageEncoder, ServiceResponse
6+
from roslibpy.core import (
7+
ActionFeedback,
8+
ActionGoalStatus,
9+
ActionResult,
10+
Message,
11+
MessageEncoder,
12+
ServiceResponse,
13+
)
714

815
LOGGER = logging.getLogger("roslibpy")
916

@@ -22,19 +29,23 @@ def __init__(self, *args, **kwargs):
2229
super(RosBridgeProtocol, self).__init__(*args, **kwargs)
2330
self.factory = None
2431
self._pending_service_requests = {}
32+
self._pending_action_requests = {}
2533
self._message_handlers = {
2634
"publish": self._handle_publish,
2735
"service_response": self._handle_service_response,
2836
"call_service": self._handle_service_request,
37+
"send_action_goal": self._handle_action_request, # TODO: action server
38+
"cancel_action_goal": self._handle_action_cancel, # TODO: action server
39+
"action_feedback": self._handle_action_feedback,
40+
"action_result": self._handle_action_result,
41+
"status": None, # TODO: add handlers for op: status
2942
}
30-
# TODO: add handlers for op: status
3143

3244
def on_message(self, payload):
3345
message = Message(json.loads(payload.decode("utf8")))
3446
handler = self._message_handlers.get(message["op"], None)
3547
if not handler:
3648
raise RosBridgeException('No handler registered for operation "%s"' % message["op"])
37-
3849
handler(message)
3950

4051
def send_ros_message(self, message):
@@ -106,3 +117,59 @@ def _handle_service_request(self, message):
106117
raise ValueError("Expected service name missing in service request")
107118

108119
self.factory.emit(message["service"], message)
120+
121+
def send_ros_action_goal(self, message, resultback, feedback, errback):
122+
"""Initiate a ROS action request by sending a goal through the ROS Bridge.
123+
124+
Args:
125+
message (:class:`.Message`): ROS Bridge Message containing the action request.
126+
callback: Callback invoked on receiving result.
127+
feedback: Callback invoked when receiving feedback from action server.
128+
errback: Callback invoked on error.
129+
"""
130+
request_id = message["id"]
131+
self._pending_action_requests[request_id] = (resultback, feedback, errback)
132+
133+
json_message = json.dumps(dict(message), cls=MessageEncoder).encode("utf8")
134+
LOGGER.debug("Sending ROS action goal request: %s", json_message)
135+
136+
self.send_message(json_message)
137+
138+
def _handle_action_request(self, message):
139+
if "action" not in message:
140+
raise ValueError("Expected action name missing in action request")
141+
raise RosBridgeException('Action server capabilities not yet implemented')
142+
143+
def _handle_action_cancel(self, message):
144+
if "action" not in message:
145+
raise ValueError("Expected action name missing in action request")
146+
raise RosBridgeException('Action server capabilities not yet implemented')
147+
148+
def _handle_action_feedback(self, message):
149+
if "action" not in message:
150+
raise ValueError("Expected action name missing in action feedback")
151+
152+
request_id = message["id"]
153+
_, feedback, _ = self._pending_action_requests.get(request_id, None)
154+
feedback(ActionFeedback(message["values"]))
155+
156+
def _handle_action_result(self, message):
157+
request_id = message["id"]
158+
action_handlers = self._pending_action_requests.get(request_id, None)
159+
160+
if not action_handlers:
161+
raise RosBridgeException('No handler registered for action request ID: "%s"' % request_id)
162+
163+
resultback, _ , errback = action_handlers
164+
del self._pending_action_requests[request_id]
165+
166+
LOGGER.debug("Received Action result with status: %s", message["status"])
167+
168+
results = {"status": ActionGoalStatus(message["status"]).name, "values": message["values"]}
169+
170+
if "result" in message and message["result"] is False:
171+
if errback:
172+
errback(results)
173+
else:
174+
if resultback:
175+
resultback(ActionResult(results))

0 commit comments

Comments
 (0)