From 183e08e85377a855e9e26af15432bfde593683f8 Mon Sep 17 00:00:00 2001 From: winterhalderp Date: Tue, 20 Apr 2021 21:39:39 +0200 Subject: [PATCH] srv host and client implemented successfully --- src/custom_interfaces/srv/CustomSrv1.srv | 9 +---- src/pubsub/pubsub/pubsub_library_v3.py | 51 +++++++++++++++--------- src/pubsub/pubsub/service_client.py | 39 ++++++++++++++++++ src/pubsub/pubsub/service_host.py | 27 +++++++++++++ src/pubsub/setup.py | 4 +- 5 files changed, 103 insertions(+), 27 deletions(-) create mode 100644 src/pubsub/pubsub/service_client.py create mode 100644 src/pubsub/pubsub/service_host.py diff --git a/src/custom_interfaces/srv/CustomSrv1.srv b/src/custom_interfaces/srv/CustomSrv1.srv index 0d93ae4..2a0fa0e 100644 --- a/src/custom_interfaces/srv/CustomSrv1.srv +++ b/src/custom_interfaces/srv/CustomSrv1.srv @@ -1,10 +1,3 @@ -# Header data, eg timestamp -# Problem: "header__struct.hpp: No such file or directory" -#Header header - -# Sensor Data coming back from an array of atmospheric sensors -float32[] temperature -float32[] pressure -float32[] humidity +string command --- bool success diff --git a/src/pubsub/pubsub/pubsub_library_v3.py b/src/pubsub/pubsub/pubsub_library_v3.py index f2fabc6..2513b15 100644 --- a/src/pubsub/pubsub/pubsub_library_v3.py +++ b/src/pubsub/pubsub/pubsub_library_v3.py @@ -28,8 +28,8 @@ from rclpy.node import Node # Definition of Parent Classes #******************************************************************************# - - +# TOPICS +#******************************************************************************# class MinimalPublisher(Node): @@ -77,7 +77,6 @@ class MinimalPublisher(Node): #******************************************************************************# - class MinimalSubscriber(Node): def __init__(self, NODE_NAME, TOPIC_NAME, MSG_TYPE, NUM_MSGS): @@ -105,9 +104,12 @@ class MinimalSubscriber(Node): return self.msg -#******************************************************************************# +#******************************************************************************# +# SERVICE +#******************************************************************************# + class MinimalServiceProvider(Node): def __init__(self, NODE_NAME, SRV_NAME, SRV_TYPE): self.NODE_NAME = NODE_NAME @@ -126,20 +128,25 @@ class MinimalServiceProvider(Node): * request. * response. """ + self.get_logger().info("Service Call received") - # Add callback code here: - # What does Service do? - # .... - - - #self.get_logger().info('Incoming request\na: %d b: %d c: %d' % (request.a, request.b, request.c)) + # Abstraction layer: + response = self.user_defined(request, response) # Function defined below + + return response + + + # Insert your callback code here + def user_defined(self, request, response): + # Write user defined code here + # Adapt to fit your service type + response.success = True return response #******************************************************************************# - -class MinimalServiceClient(Node): +class MinimalServiceClientAsync(Node): def __init__(self, NODE_NAME, SRV_NAME, SRV_TYPE): self.NODE_NAME = NODE_NAME self.SRV_NAME = SRV_NAME @@ -154,11 +161,20 @@ class MinimalServiceClient(Node): def send_request(self, request): """ Feed request of type "SRV_TYPE.Request()" + Access variable using + * request = SRV_TYPE.Request() + * request. = .... """ self.request = request self.future = self.srv_client.call_async(self.request) - def check_if_service_completed(self): + def check_if_service_complete(self): + """ Checks if service call was answered by host. + + Returns tuple [done, response]: + * done: Bool (service call complete) + * response: msg class from srv type + """ if self.future.done(): try: response = self.future.result() @@ -166,10 +182,9 @@ class MinimalServiceClient(Node): self.get_logger().info( 'Service call failed %r' % (e,)) else: + pass # Adapt to fit service type - #self.get_logger().info( - 'Result of add_three_ints: for %d + %d + %d = %d' % - (minimal_client.req.a, minimal_client.req.b, minimal_client.req.c, response.sum)) - return response + # self.get_logger().info('Result of add_three_ints: for %d + %d + %d = %d' %(minimal_client.req.a, minimal_client.req.b, minimal_client.req.c, response.sum)) + return self.future.done(), response else: - return None + return self.future.done(), None diff --git a/src/pubsub/pubsub/service_client.py b/src/pubsub/pubsub/service_client.py new file mode 100644 index 0000000..2be5065 --- /dev/null +++ b/src/pubsub/pubsub/service_client.py @@ -0,0 +1,39 @@ +import rclpy +from rclpy.node import Node + +# Service Type Files +from custom_interfaces.srv import CustomSrv1 + +# Service Minimal Host and Client +import pubsub.pubsub_library_v3 as lib + + +def main(args=None): + rclpy.init(args=args) + + # Create Service Client + minimal_client = lib.MinimalServiceClientAsync(NODE_NAME="srv_client_node", SRV_NAME="test_srv", SRV_TYPE=CustomSrv1) + + # Create Service Request + srv_request = CustomSrv1.Request() + srv_request.command = "Hallo" + minimal_client.send_request(srv_request) # send request to service provider + + while rclpy.ok(): + try: + rclpy.spin_once(minimal_client, timeout_sec=0.1) + + done, response = minimal_client.check_if_service_complete() + if done: + print(response.success) + else: + print("Service not complete") + + except (KeyboardInterrupt, SystemExit): + print("\n\nShutting down...") + minimal_client.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/pubsub/pubsub/service_host.py b/src/pubsub/pubsub/service_host.py new file mode 100644 index 0000000..ca37dad --- /dev/null +++ b/src/pubsub/pubsub/service_host.py @@ -0,0 +1,27 @@ +import rclpy +from rclpy.node import Node + +# Service Type Files +from custom_interfaces.srv import CustomSrv1 + +# Service Minimal Host and Client +import pubsub.pubsub_library_v3 as lib + + +def main(args=None): + rclpy.init(args=args) + + minimal_service = lib.MinimalServiceProvider(NODE_NAME="Service_Host_Test", SRV_NAME="test_srv", SRV_TYPE=CustomSrv1) + + while rclpy.ok(): + try: + rclpy.spin_once(minimal_service, timeout_sec=0.1) + + except (KeyboardInterrupt, SystemExit): + print("\n\nShutting down...") + minimal_service.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/pubsub/setup.py b/src/pubsub/setup.py index 281eeb0..895d498 100644 --- a/src/pubsub/setup.py +++ b/src/pubsub/setup.py @@ -21,7 +21,9 @@ setup( entry_points={ 'console_scripts': [ 'talker = pubsub.talker:main', - 'listener = pubsub.listener:main' + 'listener = pubsub.listener:main', + 'srvhost = pubsub.service_host:main', + 'srvclient = pubsub.service_client:main' ], }, )