ROS2_pubsub/src/pubsub/pubsub/pubsub_library_v3.py

#!/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
#******************************************************************************#
# TOPICS
#******************************************************************************#

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
        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

    def return_msg(self):
        return self.msg




#******************************************************************************#
# SERVICE
#******************************************************************************#

class MinimalServiceProvider(Node):
    """ Minimal Service Provider Class
    Inputs:
    * NODE_NAME: string
    * SRV_NAME: string
    * SRV_TYPE: service type class
    * srv_callback_fct: service callback function

    srv_callback_fct - Inputs:
    * request: class of type srv request
    * response: class of type srv response

    Return:
    * response: class of type srv response
    """


    def __init__(self, NODE_NAME, SRV_NAME, SRV_TYPE, srv_callback_fct=None):
        self.NODE_NAME = NODE_NAME
        self.SRV_NAME = SRV_NAME
        self.SRV_TYPE = SRV_TYPE
        if srv_callback_fct == None:
            self.srv_callback = self.std_srv_callback
        else:
            self.srv_callback = srv_callback_fct
        # Init above laying class "Node" 
        super().__init__(self.NODE_NAME)
        print("\tStarting Service as Host:\t%s"%(self.SRV_NAME))
        #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):
        """
        * request: first half of srv-file
        * response: second half of srv-file
        
        access values using class access:
        * request.var_name
        * response.var_name
        """
        self.get_logger().info("[srv: %s] Service Call Received"%(self.SRV_NAME))
        # 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 your user defined code here which will be run at every service call

        Input:
        * request: srv request class
        Return:
        * response: srv response class

        Access these variables using class access, eg. request.var_name
        """
        # Write user defined code here
        # Adapt to fit your service type
        # You could overwrite this fdunctions with a new function when initializing this class 
        response.success = True # Change this to fit your service type
        return response


#******************************************************************************#

class MinimalServiceClientAsync(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 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...')
        self.request = self.SRV_TYPE.Request()
        self.done_was_true = False

    def send_request(self, request):
        """
        Feed request of type "SRV_TYPE.Request()"
        Access variable using 
        * request = SRV_TYPE.Request()
        * request.<var_name> = ....
        """
        self.request = request
        self.done_was_true = False
        self.future = self.srv_client.call_async(self.request)

    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()
                self.done_was_true = True
            except Exception as e:
                self.get_logger().info('Service call failed %r' % (e,))
            else:
                pass
                # Adapt to fit your custom 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 self.future.done(), response # response is of type "<service_type_name>.Response()"
        else:
            return self.future.done(), None