Skip to content

Commit

Permalink
Asynchronous client calls in a timer cause the timer to never execute…
Browse files Browse the repository at this point in the history
… a second time

  ros2/rclpy#1018

Signed-off-by: Tomoya Fujita <[email protected]>
  • Loading branch information
fujitatomoya committed Oct 14, 2022
1 parent 60bc3d9 commit 321e2aa
Show file tree
Hide file tree
Showing 3 changed files with 102 additions and 0 deletions.
2 changes: 2 additions & 0 deletions prover_rclpy/setup.py
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,8 @@
'rclpy_1000_sub = src.rclpy_1000_sub:main',
'rclpy_1016_service = src.rclpy_1016_service:main',
'rclpy_1016_client = src.rclpy_1016_client:main',
'rclpy_1018_server = src.rclpy_1018_server:main',
'rclpy_1018_client = src.rclpy_1018_client:main',
'rclpy_doctor_940 = src.rclpy_doctor_940:main',
'rclpy_foo_940 = src.rclpy_foo_940:main',
'rclpy_bar_940 = src.rclpy_bar_940:main',
Expand Down
69 changes: 69 additions & 0 deletions prover_rclpy/src/rclpy_1018_client.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,69 @@
from example_interfaces.srv import AddTwoInts
import rclpy
from rclpy.node import Node

from rclpy.executors import MultiThreadedExecutor
from rclpy.callback_groups import MutuallyExclusiveCallbackGroup


class MinimalClient(Node):
def __init__(self):
super().__init__("minimal_client_async")
self.cb = None
self.timer_cb = MutuallyExclusiveCallbackGroup()
self.cli = self.create_client(
AddTwoInts, "add_two_ints", callback_group=self.cb
)
while not self.cli.wait_for_service(timeout_sec=1.0):
self.get_logger().info("service not available, waiting again...")
self.req = AddTwoInts.Request()

# Create a timer for the main loop execution
self.timer = self.create_timer(
1.0, self.main_loop, callback_group=self.timer_cb
)

async def send_request(self, a, b):
self.get_logger().info("send_request: enter")
self.req.a = a
self.req.b = b

# Only works once, then never executes the timer again
#self.future = self.cli.call_async(self.req)
#rclpy.spin_until_future_complete(self, self.future)
#self.get_logger().info("send_request: exit")
#return self.future.result()

# Suggested example by @llapx
#self.future = self.cli.call_async(self.req)
#while rclpy.ok():
# if self.future.done() and self.future.result():
# self.get_logger().info("send_request: exit")
# return self.future.result()

return await self.cli.call_async(self.req)

# Used to work, but blocks now
# return self.cli.call(self.req)

async def main_loop(self) -> None:
response = await self.send_request(4, 2)
self.get_logger().info(
"Result of add_two_ints: for %d + %d = %d" % (4, 2, response.sum)
)


def main(args=None):
rclpy.init(args=args)

minimal_client = MinimalClient()

executor = MultiThreadedExecutor()
executor.add_node(minimal_client)
executor.spin()

rclpy.shutdown()


if __name__ == "__main__":
main()
31 changes: 31 additions & 0 deletions prover_rclpy/src/rclpy_1018_server.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,31 @@
from example_interfaces.srv import AddTwoInts

import rclpy
from rclpy.node import Node


class MinimalService(Node):

def __init__(self):
super().__init__('minimal_service')
self.srv = self.create_service(AddTwoInts, 'add_two_ints', self.add_two_ints_callback)

def add_two_ints_callback(self, request, response):
response.sum = request.a + request.b
self.get_logger().info('Incoming request\na: %d b: %d' % (request.a, request.b))

return response


def main(args=None):
rclpy.init(args=args)

minimal_service = MinimalService()

rclpy.spin(minimal_service)

rclpy.shutdown()


if __name__ == '__main__':
main()

0 comments on commit 321e2aa

Please sign in to comment.