From e2aa178cac3b570c1ebf7911abe36c5df44ce47e Mon Sep 17 00:00:00 2001 From: winterhalderp Date: Thu, 29 Apr 2021 16:04:31 +0200 Subject: [PATCH] Correction topic & services --- src/pubsub/pubsub/pubsub_library_v3.py | 11 +++---- src/pubsub/pubsub/service_client.py | 28 ++++++++++++------ src/pubsub/pubsub/topic_listener.py | 40 +++++++++++++------------- src/pubsub/pubsub/topic_talker.py | 4 +-- 4 files changed, 47 insertions(+), 36 deletions(-) diff --git a/src/pubsub/pubsub/pubsub_library_v3.py b/src/pubsub/pubsub/pubsub_library_v3.py index c235bcc..9c1d07a 100644 --- a/src/pubsub/pubsub/pubsub_library_v3.py +++ b/src/pubsub/pubsub/pubsub_library_v3.py @@ -93,10 +93,12 @@ class MinimalSubscriber(Node): self.listener_callback, # Callback Function self.NUM_MSGS) # List of saved messages self.subscription # prevent unused variable warning + print("MinimalSubscriber `%s` created"%(self.NODE_NAME)) return def listener_callback(self, msg): #self.get_logger().info('I heard: "%s"' % msg.data) + self.topic_received = True self.msg = msg return @@ -141,9 +143,6 @@ class MinimalServiceProvider(Node): #self.srv_host = self.create_service(self.SRV_TYPE, self.SRV_NAME, self.service_callback) self.srv_host = self.create_service(self.SRV_TYPE, self.SRV_NAME, self.srv_callback) - - - #def service_callback(self, request, response): def std_srv_callback(self, request, response): """ @@ -184,12 +183,13 @@ class MinimalServiceClientAsync(Node): self.NODE_NAME = NODE_NAME self.SRV_NAME = SRV_NAME self.SRV_TYPE = SRV_TYPE - # Init above laying class "Node" + # Init above laying class "Node" (super class) super().__init__(self.NODE_NAME) + print("Starting Service Client:\t%s"%(self.SRV_NAME)) self.srv_client = self.create_client(self.SRV_TYPE, self.SRV_NAME) while not self.srv_client.wait_for_service(timeout_sec=1.0): - self.get_logger().info('service not available, waiting again...') + self.get_logger().info('service not available, waiting...') self.request = self.SRV_TYPE.Request() self.done_was_true = False @@ -214,6 +214,7 @@ class MinimalServiceClientAsync(Node): if self.future.done(): try: response = self.future.result() + self.done_was_true = True except Exception as e: self.get_logger().info('Service call failed %r' % (e,)) else: diff --git a/src/pubsub/pubsub/service_client.py b/src/pubsub/pubsub/service_client.py index 2be5065..ea7007a 100644 --- a/src/pubsub/pubsub/service_client.py +++ b/src/pubsub/pubsub/service_client.py @@ -5,29 +5,39 @@ from rclpy.node import Node from custom_interfaces.srv import CustomSrv1 # Service Minimal Host and Client -import pubsub.pubsub_library_v3 as lib +from pubsub.pubsub_library_v3 import MinimalServiceClientAsync 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) + #minimal_client = lib.MinimalServiceClientAsync(NODE_NAME="srv_client_node", SRV_NAME="test_srv", SRV_TYPE=CustomSrv1) + minimal_client = 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 + + # Send Service Request to host + print("Service requested...") + minimal_client.done_was_true = False # set service as not done + minimal_client.send_request(srv_request) while rclpy.ok(): try: + # Loop checks if service is complete 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") + + # check if service was already done before this loop + if not minimal_client.done_was_true: + done, response = minimal_client.check_if_service_complete() # returns tuple + if done: + print("Service done !") + print("Service Response:\t%s"%(response.success)) + minimal_client.done_was_true = True # set service as done + else: + print("Service not complete") except (KeyboardInterrupt, SystemExit): print("\n\nShutting down...") diff --git a/src/pubsub/pubsub/topic_listener.py b/src/pubsub/pubsub/topic_listener.py index 61c5d34..03f8e70 100644 --- a/src/pubsub/pubsub/topic_listener.py +++ b/src/pubsub/pubsub/topic_listener.py @@ -18,55 +18,55 @@ import rclpy from rclpy.node import Node # Import Subscriber Library -from .pubsub_library import CustomMsg1_sub -from .pubsub_library import CustomMsg2_sub +from .pubsub_library_v3 import MinimalSubscriber # Import Message Types -from pubsub_msg.msg import CustomMsg1 -from pubsub_msg.msg import CustomMsg2 +from custom_interfaces.msg import CustomMsg1 +from custom_interfaces.msg import CustomMsg2 + def main(args=None): rclpy.init(args=args) # Start nodes here, should create object for every node - listener_1 = CustomMsg1_sub(NODE_NAME="pubsub_listener_1", TOPIC_NAME="/chatter1",MSG_TYPE=CustomMsg1, NUM_MSGS=0) - listener_2 = CustomMsg2_sub(NODE_NAME="pubsub_listener_2", TOPIC_NAME="/chatter2",MSG_TYPE=CustomMsg2, NUM_MSGS=0) + listener_1 = MinimalSubscriber(NODE_NAME="pubsub_listener_1", TOPIC_NAME="/chatter1",MSG_TYPE=CustomMsg1, NUM_MSGS=0) + listener_2 = MinimalSubscriber(NODE_NAME="pubsub_listener_2", TOPIC_NAME="/chatter2",MSG_TYPE=CustomMsg2, NUM_MSGS=0) + # create empty variables of msg type (optional) + msg_1 = CustomMsg1() + msg_2 = CustomMsg2() while rclpy.ok(): try: # Insert main looping script here... - # Receive Topics by running nodes + + # Receive msgs by running nodes rclpy.spin_once(listener_1, timeout_sec=0.01) rclpy.spin_once(listener_2, timeout_sec=0.01) - # Do sth if message was received + + # Check if msg received if listener_1.topic_received is True: - listener_1.topic_received = False # zurücksetzen + listener_1.topic_received = False # gleich zurücksetzen msg_1 = listener_1.return_msg() #********************************* # Do sth based on received message + print("/chatter1 received") + print(msg_1) #********************************* if listener_2.topic_received is True: - listener_2.topic_received = False # zurücksetzen + listener_2.topic_received = False # gleich zurücksetzen msg_2 = listener_2.return_msg() #********************************* # Do sth based on received message + print("/chatter2 received") + print(msg_2) #********************************* - - # Check if "msg_1" or "msg_2" is available as a local variable - if 'msg_1' in locals(): - # Print msg_1 - listener_1.print_msg() - if 'msg_2' in locals(): - # Print msg_2 - listener_2.print_msg() - # Here, you can now also publish/pass on the results of this script by - # using the "MinimalPublisher" class. For this please refer to "talker.py". + # using the "MinimalPublisher" class. For this please refer to "talker.py" except (KeyboardInterrupt, SystemExit): print("\n\nShutting down...") diff --git a/src/pubsub/pubsub/topic_talker.py b/src/pubsub/pubsub/topic_talker.py index b2edd6c..d3d80f5 100644 --- a/src/pubsub/pubsub/topic_talker.py +++ b/src/pubsub/pubsub/topic_talker.py @@ -21,8 +21,8 @@ from rclpy.node import Node from .pubsub_library import MinimalPublisher # Import Message Types (Message Files) -from pubsub_msg.msg import CustomMsg1 -from pubsub_msg.msg import CustomMsg2 +from custom_interfaces.msg import CustomMsg1 +from custom_interfaces.msg import CustomMsg2 def main(args=None):