From 2d7675fc931aaeb92ef91c46184883e92ee7c3d3 Mon Sep 17 00:00:00 2001 From: winterhalderp Date: Tue, 20 Apr 2021 18:58:20 +0200 Subject: [PATCH] srv host and client programmed - v1 --- src/pubsub/pubsub/pubsub_library_v3.py | 175 +++++++++++++++++++++++++ 1 file changed, 175 insertions(+) create mode 100644 src/pubsub/pubsub/pubsub_library_v3.py diff --git a/src/pubsub/pubsub/pubsub_library_v3.py b/src/pubsub/pubsub/pubsub_library_v3.py new file mode 100644 index 0000000..f2fabc6 --- /dev/null +++ b/src/pubsub/pubsub/pubsub_library_v3.py @@ -0,0 +1,175 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +#********************************************# +# Publisher/Subscriber Class Library + +# Creted for: +# ROS2 Workshop 2020 +# Roverentwicklung für Explorationsaufgaben +# Institute for Space Systems +# University of Stuttgart + +# Created by Patrick Winterhalder +# IRS, University of Stuttgart +#********************************************# + +import rclpy +from rclpy.node import Node + +# How to use: +# from pubsub_library import MinimalPublisher +# from pubsub_library import MinimalSubscriber +# minimal_publisher = MinimalPublisher(NODE_NAME='minimal_pub', TOPIC_NAME='user_controller', MSG_TYPE=Usercontroller, MSG_PERIOD=0.5) +# minimal_subscriber = MinimalSubscriber(NODE_NAME='minimal_sub', TOPIC_NAME='epos_feedback', MSG_TYPE=Eposreturn) +# See --> talker.py, listener.py + + + +# Definition of Parent Classes +#******************************************************************************# + + + +class MinimalPublisher(Node): + + def __init__(self, NODE_NAME, TOPIC_NAME, MSG_TYPE, MSG_PERIOD): + self.NODE_NAME= NODE_NAME + self.TOPIC_NAME = TOPIC_NAME + self.CUSTOM_MSG = MSG_TYPE + self.timer_period = MSG_PERIOD # [seconds] + # Init above laying class Node + super().__init__(self.NODE_NAME) + print("\t- " + str(TOPIC_NAME) + "\n") + self.publisher_ = self.create_publisher( + self.CUSTOM_MSG, + self.TOPIC_NAME, + 10) + self.new_msg = False + # Define Node Frequency, equivalent to ~rospy.rate() + self.timer = self.create_timer(self.timer_period, self.publisher_timer) + return + + def publisher_timer(self): + if self.new_msg is True: + try: + #self.get_logger().info('Pub:') + self.publisher_.publish(self.msg) + self.new_msg = False + except TypeError: + print("[ERROR] Msg-Data-Types do not match") + return + + # Publish using Timer + def timer_publish(self, msg): + self.msg=msg + self.new_msg = True + return + + # Publish directly without Timer + def direct_publish(self, msg): + try: + #self.get_logger().info('Pub:') + self.publisher_.publish(msg) + except TypeError: + print("[ERROR] Msg-Data-Types do not match") + return + +#******************************************************************************# + + +class MinimalSubscriber(Node): + + def __init__(self, NODE_NAME, TOPIC_NAME, MSG_TYPE, NUM_MSGS): + self.NODE_NAME= NODE_NAME + self.TOPIC_NAME = TOPIC_NAME + self.CUSTOM_MSG = MSG_TYPE + self.NUM_MSGS = NUM_MSGS + self.topic_received = False + # Init above laying class "Node" + super().__init__(self.NODE_NAME) + self.subscription = self.create_subscription( + self.CUSTOM_MSG, # Message Type + self.TOPIC_NAME, # Topic Name + self.listener_callback, # Callback Function + self.NUM_MSGS) # List of saved messages + self.subscription # prevent unused variable warning + return + + def listener_callback(self, msg): + #self.get_logger().info('I heard: "%s"' % msg.data) + self.msg = msg + return + + def return_msg(self): + return self.msg + + +#******************************************************************************# + + +class MinimalServiceProvider(Node): + def __init__(self, NODE_NAME, SRV_NAME, SRV_TYPE): + self.NODE_NAME = NODE_NAME + self.SRV_NAME = SRV_NAME + self.SRV_TYPE = SRV_TYPE + # Init above laying class "Node" + super().__init__(self.NODE_NAME) + self.srv_host = self.create_service(self.SRV_TYPE, self.SRV_NAME, self.service_callback) + + def service_callback(self, request, response): + """ + * request: first half of srv-file + * response: second half of srv-file + + access values using: + * request. + * response. + """ + + # 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)) + return response + + +#******************************************************************************# + + +class MinimalServiceClient(Node): + def __init__(self, NODE_NAME, SRV_NAME, SRV_TYPE): + self.NODE_NAME = NODE_NAME + self.SRV_NAME = SRV_NAME + self.SRV_TYPE = SRV_TYPE + # Init above laying class "Node" + super().__init__(self.NODE_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.request = self.SRV_TYPE.Request() + + def send_request(self, request): + """ + Feed request of type "SRV_TYPE.Request()" + """ + self.request = request + self.future = self.srv_client.call_async(self.request) + + def check_if_service_completed(self): + if self.future.done(): + try: + response = self.future.result() + except Exception as e: + self.get_logger().info( + 'Service call failed %r' % (e,)) + else: + # 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 + else: + return None