#!/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. = .... """ 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 ".Response()" else: return self.future.done(), None