diff --git a/rclpy/services/minimal_client/client.py b/rclpy/services/minimal_client/client.py index 17fbe330..7a25ccfa 100644 --- a/rclpy/services/minimal_client/client.py +++ b/rclpy/services/minimal_client/client.py @@ -28,16 +28,15 @@ def main(args=None): while not cli.wait_for_service(timeout_sec=1.0): node.get_logger().info('service not available, waiting again...') - cli.call(req) - # when calling wait for future - # spin should not be called in the main loop - cli.wait_for_future() - # TODO(mikaelarguedas) This is not the final API, and this does not scale - # for multiple pending requests. This will change once an executor model is implemented - # In the future the response will not be stored in cli.response - node.get_logger().info( - 'Result of add_two_ints: for %d + %d = %d' % - (req.a, req.b, cli.response.sum)) + future = cli.call_async(req) + rclpy.spin_until_future_complete(node, future) + + if future.result() is not None: + node.get_logger().info( + 'Result of add_two_ints: for %d + %d = %d' % + (req.a, req.b, future.result().sum)) + else: + node.get_logger().info('Service call failed %r' % (future.exception(),)) node.destroy_node() rclpy.shutdown() diff --git a/rclpy/services/minimal_client/client_async.py b/rclpy/services/minimal_client/client_async.py index f32b412e..ab3ae283 100644 --- a/rclpy/services/minimal_client/client_async.py +++ b/rclpy/services/minimal_client/client_async.py @@ -30,17 +30,17 @@ def main(args=None): while not cli.wait_for_service(timeout_sec=1.0): node.get_logger().info('service not available, waiting again...') - cli.call(req) + future = cli.call_async(req) while rclpy.ok(): - # TODO(mikaelarguedas) This is not the final API, and this does not scale - # for multiple pending requests. This will change once an executor model is implemented - # In the future the response will not be stored in cli.response - if cli.response is not None: - node.get_logger().info( - 'Result of add_two_ints: for %d + %d = %d' % - (req.a, req.b, cli.response.sum)) - break rclpy.spin_once(node) + if future.done(): + if future.result() is not None: + node.get_logger().info( + 'Result of add_two_ints: for %d + %d = %d' % + (req.a, req.b, future.result().sum)) + else: + node.get_logger().info('Service call failed %r' % (future.exception(),)) + break node.destroy_node() rclpy.shutdown() diff --git a/rclpy/services/minimal_client/client_async_callback.py b/rclpy/services/minimal_client/client_async_callback.py new file mode 100644 index 00000000..bb0a90ee --- /dev/null +++ b/rclpy/services/minimal_client/client_async_callback.py @@ -0,0 +1,70 @@ +# Copyright 2018 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from example_interfaces.srv import AddTwoInts + +import rclpy +from rclpy.callback_groups import ReentrantCallbackGroup + + +def main(args=None): + rclpy.init(args=args) + node = rclpy.create_node('minimal_client') + # Node's default callback group is mutually exclusive. This would prevent the client response + # from being processed until the timer callback finished, but the timer callback in this + # example is waiting for the client response + cb_group = ReentrantCallbackGroup() + cli = node.create_client(AddTwoInts, 'add_two_ints', callback_group=cb_group) + did_run = False + did_get_result = False + + async def call_service(): + nonlocal cli, node, did_run, did_get_result + did_run = True + try: + req = AddTwoInts.Request() + req.a = 41 + req.b = 1 + future = cli.call_async(req) + result = await future + if result is not None: + node.get_logger().info( + 'Result of add_two_ints: for %d + %d = %d' % + (req.a, req.b, result.sum)) + else: + node.get_logger().info('Service call failed %r' % (future.exception(),)) + finally: + did_get_result = True + + while not cli.wait_for_service(timeout_sec=1.0): + node.get_logger().info('service not available, waiting again...') + + timer = node.create_timer(0.5, call_service, callback_group=cb_group) + + while rclpy.ok() and not did_run: + rclpy.spin_once(node) + + if did_run: + # call timer callback only once + timer.cancel() + + while rclpy.ok() and not did_get_result: + rclpy.spin_once(node) + + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/rclpy/services/minimal_client/client_async_member_function.py b/rclpy/services/minimal_client/client_async_member_function.py index 60b0a683..f320b15e 100644 --- a/rclpy/services/minimal_client/client_async_member_function.py +++ b/rclpy/services/minimal_client/client_async_member_function.py @@ -30,7 +30,7 @@ def __init__(self): def send_request(self): self.req.a = 41 self.req.b = 1 - self.cli.call(self.req) + self.future = self.cli.call_async(self.req) def main(args=None): @@ -40,15 +40,17 @@ def main(args=None): minimal_client.send_request() while rclpy.ok(): - # TODO(mikaelarguedas) This is not the final API, and this does not scale - # for multiple pending requests. This will change once an executor model is implemented - # In the future the response will not be stored in cli.response - if minimal_client.cli.response is not None: - minimal_client.get_logger().info( - 'Result of add_two_ints: for %d + %d = %d' % - (minimal_client.req.a, minimal_client.req.b, minimal_client.cli.response.sum)) - break rclpy.spin_once(minimal_client) + if minimal_client.future.done(): + if minimal_client.future.result() is not None: + response = minimal_client.future.result() + minimal_client.get_logger().info( + 'Result of add_two_ints: for %d + %d = %d' % + (minimal_client.req.a, minimal_client.req.b, response.sum)) + else: + minimal_client.get_logger().info( + 'Service call failed %r' % (minimal_client.future.exception(),)) + break minimal_client.destroy_node() rclpy.shutdown() diff --git a/rclpy/services/minimal_client/setup.py b/rclpy/services/minimal_client/setup.py index 28cf684a..8f8c0b20 100644 --- a/rclpy/services/minimal_client/setup.py +++ b/rclpy/services/minimal_client/setup.py @@ -9,7 +9,8 @@ py_modules=[ 'client', 'client_async', - 'client_async_member_function'], + 'client_async_member_function', + 'client_async_callback'], data_files=[ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), @@ -36,6 +37,7 @@ 'client_async = client_async:main', 'client_async_member_function =' ' client_async_member_function:main', + 'client_async_callback = client_async_callback:main', ], }, )