Skip to content

Commit f857c5d

Browse files
sea-basssea-bass
committed
More actions functionality and tests
1 parent 998be32 commit f857c5d

File tree

6 files changed

+386
-49
lines changed

6 files changed

+386
-49
lines changed

‎rosbridge_library/src/rosbridge_library/capabilities/action_result.py‎

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,35 @@
1+
# Software License Agreement (BSD License)
2+
#
3+
# Copyright (c) 2023, PickNik Inc.
4+
# All rights reserved.
5+
#
6+
# Redistribution and use in source and binary forms, with or without
7+
# modification, are permitted provided that the following conditions
8+
# are met:
9+
#
10+
# * Redistributions of source code must retain the above copyright
11+
# notice, this list of conditions and the following disclaimer.
12+
# * Redistributions in binary form must reproduce the above
13+
# copyright notice, this list of conditions and the following
14+
# disclaimer in the documentation and/or other materials provided
15+
# with the distribution.
16+
# * Neither the name of the copyright holder nor the names of its
17+
# contributors may be used to endorse or promote products derived
18+
# from this software without specific prior written permission.
19+
#
20+
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21+
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22+
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23+
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24+
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25+
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26+
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27+
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28+
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29+
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30+
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31+
# POSSIBILITY OF SUCH DAMAGE.
32+
133
from rosbridge_library.capability import Capability
234
from rosbridge_library.internal import message_conversion, ros_loader
335

‎rosbridge_library/src/rosbridge_library/capabilities/advertise_action.py‎

Lines changed: 47 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,37 @@
1+
# Software License Agreement (BSD License)
2+
#
3+
# Copyright (c) 2023, PickNik Inc.
4+
# All rights reserved.
5+
#
6+
# Redistribution and use in source and binary forms, with or without
7+
# modification, are permitted provided that the following conditions
8+
# are met:
9+
#
10+
# * Redistributions of source code must retain the above copyright
11+
# notice, this list of conditions and the following disclaimer.
12+
# * Redistributions in binary form must reproduce the above
13+
# copyright notice, this list of conditions and the following
14+
# disclaimer in the documentation and/or other materials provided
15+
# with the distribution.
16+
# * Neither the name of the copyright holder nor the names of its
17+
# contributors may be used to endorse or promote products derived
18+
# from this software without specific prior written permission.
19+
#
20+
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21+
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22+
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23+
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24+
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25+
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26+
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27+
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28+
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29+
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30+
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31+
# POSSIBILITY OF SUCH DAMAGE.
32+
133
import fnmatch
34+
import time
235

336
import rclpy
437
from rclpy.action import ActionServer
@@ -12,11 +45,12 @@ class AdvertisedActionHandler:
1245

1346
id_counter = 1
1447

15-
def __init__(self, action_name, action_type, protocol):
48+
def __init__(self, action_name, action_type, protocol, sleep_time=0.001):
1649
self.goal_futures = {}
1750
self.action_name = action_name
1851
self.action_type = action_type
1952
self.protocol = protocol
53+
self.sleep_time = sleep_time
2054
# setup the action
2155
self.action_server = ActionServer(
2256
protocol.node_handle,
@@ -31,26 +65,30 @@ def next_id(self):
3165
self.id_counter += 1
3266
return id
3367

34-
async def execute_callback(self, goal):
68+
def execute_callback(self, goal):
3569
# generate a unique ID
36-
goal_id = f"action_goal:{self.action}:{self.next_id()}"
70+
goal_id = f"action_goal:{self.action_name}:{self.next_id()}"
3771

3872
future = rclpy.task.Future()
39-
self.request_futures[goal_id] = future
73+
self.goal_futures[goal_id] = future
4074

4175
# build a request to send to the external client
4276
goal_message = {
4377
"op": "send_action_goal",
4478
"id": goal_id,
4579
"action": self.action_name,
46-
"args": message_conversion.extract_values(goal),
80+
"action_type": self.action_type,
81+
"args": message_conversion.extract_values(goal.request),
4782
}
4883
self.protocol.send(goal_message)
4984

50-
try:
51-
return await future
52-
finally:
53-
del self.goal_futures[goal_id]
85+
while not future.done():
86+
time.sleep(self.sleep_time)
87+
88+
result = future.result()
89+
goal.succeed()
90+
del self.goal_futures[goal_id]
91+
return result
5492

5593
def handle_result(self, goal_id, res):
5694
"""

‎rosbridge_library/src/rosbridge_library/capabilities/send_action_goal.py‎

Lines changed: 173 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,173 @@
1+
# Software License Agreement (BSD License)
2+
#
3+
# Copyright (c) 2023, PickNik Inc.
4+
# All rights reserved.
5+
#
6+
# Redistribution and use in source and binary forms, with or without
7+
# modification, are permitted provided that the following conditions
8+
# are met:
9+
#
10+
# * Redistributions of source code must retain the above copyright
11+
# notice, this list of conditions and the following disclaimer.
12+
# * Redistributions in binary form must reproduce the above
13+
# copyright notice, this list of conditions and the following
14+
# disclaimer in the documentation and/or other materials provided
15+
# with the distribution.
16+
# * Neither the name of the copyright holder nor the names of its
17+
# contributors may be used to endorse or promote products derived
18+
# from this software without specific prior written permission.
19+
#
20+
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21+
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22+
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23+
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24+
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25+
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26+
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27+
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28+
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29+
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30+
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31+
# POSSIBILITY OF SUCH DAMAGE.
32+
33+
import fnmatch
34+
from functools import partial
35+
from threading import Thread
36+
37+
from rosbridge_library.capability import Capability
38+
from rosbridge_library.internal.actions import ActionClientHandler
39+
40+
41+
class SendActionGoal(Capability):
42+
43+
send_action_goal_msg_fields = [
44+
(True, "action", str),
45+
(True, "action_type", str),
46+
(False, "fragment_size", (int, type(None))),
47+
(False, "compression", str),
48+
]
49+
50+
actions_glob = None
51+
52+
def __init__(self, protocol):
53+
# Call superclass constructor
54+
Capability.__init__(self, protocol)
55+
56+
# Register the operations that this capability provides
57+
call_services_in_new_thread = (
58+
protocol.node_handle.get_parameter("call_services_in_new_thread")
59+
.get_parameter_value()
60+
.bool_value
61+
)
62+
if call_services_in_new_thread:
63+
# Sends the action goal in a separate thread so multiple actions can be processed simultaneously.
64+
protocol.node_handle.get_logger().info("Sending action goal in new thread")
65+
protocol.register_operation(
66+
"send_action_goal",
67+
lambda msg: Thread(target=self.send_action_goal, args=(msg,)).start(),
68+
)
69+
else:
70+
# Sends the actions goal in this thread, so actions block and must be processed sequentially.
71+
protocol.node_handle.get_logger().info("Sending action goal in existing thread")
72+
protocol.register_operation("send_action_goal", self.send_action_goal)
73+
74+
def send_action_goal(self, message):
75+
# Pull out the ID
76+
cid = message.get("id", None)
77+
78+
# Typecheck the args
79+
self.basic_type_check(message, self.send_action_goal_msg_fields)
80+
81+
# Extract the args
82+
action = message["action"]
83+
action_type = message["action_type"]
84+
fragment_size = message.get("fragment_size", None)
85+
compression = message.get("compression", "none")
86+
args = message.get("args", [])
87+
88+
if SendActionGoal.actions_glob is not None and SendActionGoal.actions_glob:
89+
self.protocol.log("debug", f"Action security glob enabled, checking action: {action}")
90+
match = False
91+
for glob in SendActionGoal.actions_glob:
92+
if fnmatch.fnmatch(action, glob):
93+
self.protocol.log(
94+
"debug",
95+
f"Found match with glob {glob}, continuing sending action goal...",
96+
)
97+
match = True
98+
break
99+
if not match:
100+
self.protocol.log(
101+
"warn",
102+
f"No match found for action, cancelling sending action goal for: {action}",
103+
)
104+
return
105+
else:
106+
self.protocol.log("debug", "No action security glob, not checking sending action goal.")
107+
108+
# Check for deprecated action ID, eg. /rosbridge/topics#33
109+
cid = extract_id(action, cid)
110+
111+
# Create the callbacks
112+
s_cb = partial(self._success, cid, action, fragment_size, compression)
113+
e_cb = partial(self._failure, cid, action)
114+
feedback = True # TODO: Implement
115+
if feedback:
116+
f_cb = partial(self._feedback, cid, action)
117+
else:
118+
f_cb = None
119+
120+
# Run action client handler in the same thread.
121+
ActionClientHandler(
122+
trim_action_name(action), action_type, args, s_cb, e_cb, f_cb, self.protocol.node_handle
123+
).run()
124+
125+
def _success(self, cid, action, fragment_size, compression, message):
126+
outgoing_message = {
127+
"op": "action_result",
128+
"action": action,
129+
"values": message,
130+
"result": True,
131+
}
132+
if cid is not None:
133+
outgoing_message["id"] = cid
134+
# TODO: fragmentation, compression
135+
self.protocol.send(outgoing_message)
136+
137+
def _failure(self, cid, action, exc):
138+
self.protocol.log("error", "send_action_goal %s: %s" % (type(exc).__name__, str(exc)), cid)
139+
# send response with result: false
140+
outgoing_message = {
141+
"op": "action_result",
142+
"service": action,
143+
"values": str(exc),
144+
"result": False,
145+
}
146+
if cid is not None:
147+
outgoing_message["id"] = cid
148+
self.protocol.send(outgoing_message)
149+
150+
def _feedback(self, cid, action, message):
151+
outgoing_message = {
152+
"op": "action_feedback",
153+
"action": action,
154+
"values": message,
155+
}
156+
if cid is not None:
157+
outgoing_message["id"] = cid
158+
# TODO: fragmentation, compression
159+
print(outgoing_message)
160+
self.protocol.send(outgoing_message)
161+
162+
163+
def trim_action_name(action):
164+
if "#" in action:
165+
return action[: action.find("#")]
166+
return action
167+
168+
169+
def extract_id(action, cid):
170+
if cid is not None:
171+
return cid
172+
elif "#" in action:
173+
return action[action.find("#") + 1 :]

0 commit comments

Comments
 (0)