-
Notifications
You must be signed in to change notification settings - Fork 1
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Asynchronous client calls in a timer cause the timer to never execute…
… a second time ros2/rclpy#1018 Signed-off-by: Tomoya Fujita <[email protected]>
- Loading branch information
1 parent
60bc3d9
commit 321e2aa
Showing
3 changed files
with
102 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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() |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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() |