284 lines
10 KiB
Python
284 lines
10 KiB
Python
#!/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.PUBLISHER_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.PUBLISHER_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)
|
|
return
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#***************************************************************************#
|
|
# Child Classes inherite through super().__init__(...) from parent class
|
|
# Child's purpose is to simplify subscribing to custom messages
|
|
# For this to work you must customize "listener_callback", "return_msg" (and "print_msg")
|
|
|
|
|
|
|
|
# Exemplary Subscriber Class: Inheriting from MinimalSubscriber Class
|
|
# This class is custom made for receiving the specific msg type but uses MinimalSubscriber as foundation
|
|
# Create a new class of this type for every custom msg you want to receive, customizing the listener_callback,
|
|
# return_msg (and print_msg) functions to correspond to your message
|
|
|
|
class example_subscriber(MinimalSubscriber):
|
|
|
|
def __init__(self, NODE_NAME, TOPIC_NAME, MSG_TYPE, NUM_MSGS):
|
|
# Init MinimalSubscriber:
|
|
super().__init__(NODE_NAME, TOPIC_NAME, MSG_TYPE, NUM_MSGS)
|
|
print("\t- " + str(TOPIC_NAME))
|
|
# !!! CUSTOMIZE THESE VARIABLES TO WORK WITH YOUR MESSAGE FILE !!!
|
|
self.pos_actual = None
|
|
self.vel_actual = None
|
|
self.baffle_switch = None
|
|
self.current = None
|
|
self.temp = None
|
|
|
|
|
|
# !!! CUSTOMIZE THIS FUNCTION TO WORK WITH YOUR MESSAGE FILE !!!
|
|
def listener_callback(self, msg): # Overwrites callback from inherited class
|
|
self.topic_received = True
|
|
self.pos_actual = msg.pos_actual
|
|
self.vel_actual = msg.vel_actual
|
|
self.baffle_switch = msg.baffle_switch
|
|
self.current = msg.epos_current
|
|
self.temp = msg.epos_temp
|
|
#print_msg(self) # activate to print the received data --> customize print_msg(self) !
|
|
return
|
|
|
|
# !!! CUSTOMIZE THIS FUNCTION TO WORK WITH YOUR MESSAGE FILE !!!
|
|
def return_msg(self): # Extract only msg variables from "self" object
|
|
msg = self.CUSTOM_MSG()
|
|
msg.pos_actual = self.pos_actual
|
|
msg.vel_actual = self.vel_actual
|
|
msg.baffle_switch = self.baffle_switch
|
|
msg.epos_current = self.current
|
|
msg.epos_temp = self.temp
|
|
return msg
|
|
|
|
# !!! CUSTOMIZE THIS FUNCTION TO WORK WITH YOUR MESSAGE FILE !!! (optional)
|
|
def print_msg(self):
|
|
print("[SUBSCRIBER] %s received topic:\t/%s" %(self.NODE_NAME, self.TOPIC_NAME))
|
|
string = "Received:\t"
|
|
|
|
# Check length of all transmitted value lists to be equal (equal number of parameters)
|
|
length = len(self.pos_actual)
|
|
if any(len(lst) != length for lst in [self.vel_actual, self.current, self.temp]):
|
|
print("[TRANSMISSION ERROR] Length of lists inside topic %s do not match" %(self.TOPIC_NAME))
|
|
else:
|
|
complete_list = []
|
|
complete_list.append(self.pos_actual)
|
|
complete_list.append(self.vel_actual)
|
|
complete_list.append(self.current)
|
|
complete_list.append(self.temp)
|
|
#print(complete_list)
|
|
for i in range(len(complete_list)):
|
|
for y in range(len(complete_list[0])):
|
|
string += "%.2f,\t" %(complete_list[i][y])
|
|
string += str(self.baffle_switch)
|
|
print(string)
|
|
return
|
|
|
|
|
|
#************************************************************************************#
|
|
# CustomMsg1 Subscriber Class
|
|
class CustomMsg1_sub(MinimalSubscriber):
|
|
|
|
def __init__(self, NODE_NAME, TOPIC_NAME, MSG_TYPE, NUM_MSGS):
|
|
# Init MinimalSubscriber:
|
|
super().__init__(NODE_NAME, TOPIC_NAME, MSG_TYPE, NUM_MSGS)
|
|
print("\t- " + str(TOPIC_NAME))
|
|
# CustomMsg1 Variables
|
|
self.temperature = None
|
|
self.pressure = None
|
|
self.humidity = None
|
|
return
|
|
|
|
|
|
# !!! CUSTOMIZE THIS FUNCTION TO WORK WITH YOUR MESSAGE FILE !!!
|
|
def listener_callback(self, msg): # Overwrites callback from inherited class
|
|
self.topic_received = True
|
|
self.temperature = msg.temperature
|
|
self.pressure = msg.pressure
|
|
self.humidity = msg.humidity
|
|
#print_msg(self) # activate to print the received data --> customize print_msg(self) !
|
|
return
|
|
|
|
# !!! CUSTOMIZE THIS FUNCTION TO WORK WITH YOUR MESSAGE FILE !!!
|
|
def return_msg(self): # Extract only msg variables from "self" object
|
|
msg = self.CUSTOM_MSG()
|
|
msg.temperature = self.temperature
|
|
msg.pressure = self.pressure
|
|
msg.humidity = self.humidity
|
|
return msg
|
|
|
|
# !!! CUSTOMIZE THIS FUNCTION TO WORK WITH YOUR MESSAGE FILE !!! (optional)
|
|
def print_msg(self):
|
|
print("[SUBSCRIBER] %s received topic:\t/%s" %(self.NODE_NAME, self.TOPIC_NAME))
|
|
string = "Received:\t"
|
|
|
|
# Check length of all transmitted value lists to be equal (equal number of parameters)
|
|
length = len(self.temperature)
|
|
if any(len(lst) != length for lst in [self.temperature, self.pressure, self.humidity]):
|
|
print("[TRANSMISSION ERROR] Length of lists inside topic %s do not match" %(self.TOPIC_NAME))
|
|
else:
|
|
complete_list = []
|
|
complete_list.append(self.temperature)
|
|
complete_list.append(self.pressure)
|
|
complete_list.append(self.humidity)
|
|
#print(complete_list)
|
|
for i in range(len(complete_list)):
|
|
for y in range(len(complete_list[0])):
|
|
string += "%.2f,\t" %(complete_list[i][y])
|
|
print(string)
|
|
return
|
|
|
|
|
|
|
|
#************************************************************************************#
|
|
# CustomMsg2 Subscriber Class
|
|
class CustomMsg2_sub(MinimalSubscriber):
|
|
|
|
def __init__(self, NODE_NAME, TOPIC_NAME, MSG_TYPE, NUM_MSGS):
|
|
# Init MinimalSubscriber:
|
|
super().__init__(NODE_NAME, TOPIC_NAME, MSG_TYPE, NUM_MSGS)
|
|
print("\t- " + str(TOPIC_NAME))
|
|
# CustomMsg1 Variables
|
|
self.pitch_ctrl = None
|
|
self.yaw_ctrl = None
|
|
return
|
|
|
|
|
|
# !!! CUSTOMIZE THIS FUNCTION TO WORK WITH YOUR MESSAGE FILE !!!
|
|
def listener_callback(self, msg): # Overwrites callback from inherited class
|
|
self.topic_received = True
|
|
self.pitch_ctrl = msg.pitch_ctrl
|
|
self.yaw_ctrl = msg.yaw_ctrl
|
|
#print_msg(self) # activate to print the received data --> customize print_msg(self) !
|
|
return
|
|
|
|
# !!! CUSTOMIZE THIS FUNCTION TO WORK WITH YOUR MESSAGE FILE !!!
|
|
def return_msg(self): # Extract only msg variables from "self" object
|
|
msg = self.CUSTOM_MSG()
|
|
msg.pitch_ctrl = self.pitch_ctrl
|
|
msg.yaw_ctrl = self.yaw_ctrl
|
|
return msg
|
|
|
|
# !!! CUSTOMIZE THIS FUNCTION TO WORK WITH YOUR MESSAGE FILE !!! (optional)
|
|
def print_msg(self):
|
|
print("[SUBSCRIBER] %s received topic:\t/%s" %(self.NODE_NAME, self.TOPIC_NAME))
|
|
string = "Received:\t"
|
|
|
|
# Check length of all transmitted value lists to be equal (equal number of parameters)
|
|
complete_list = []
|
|
complete_list.append([self.pitch_ctrl])
|
|
complete_list.append([self.yaw_ctrl])
|
|
#print(complete_list)
|
|
for i in range(len(complete_list)):
|
|
for y in range(len(complete_list[0])):
|
|
string += "%.2f,\t" %(complete_list[i][y])
|
|
print(string)
|
|
return |