00001 import rospy 00002 from .dbm_param import DBMParam 00003 from .dbm_rate import DBMRate 00004 from threading import Thread 00005 00006 class RateThread(Thread): 00007 def __init__(self, rate, pub, get_message_method, print_message = False): 00008 Thread.__init__(self) 00009 00010 self.rate = rate 00011 self.pub = pub 00012 self.get_message_method = get_message_method 00013 self.print_message = print_message 00014 00015 def run(self): 00016 while not rospy.is_shutdown(): 00017 message = self.get_message_method() 00018 00019 if self.print_message: 00020 rospy.loginfo(message) 00021 00022 self.pub.publish(message) 00023 self.rate.sleep() 00024 00025 class DBMPublisher(rospy.Publisher): 00026 def __init__(self, name, data_class, min_frequency, max_frequency, default_frequency = None, 00027 subscriber_listener=None, tcp_nodelay=False, latch=False, headers=None, queue_size=None): 00028 super(DBMPublisher, self).__init__(name, data_class, subscriber_listener, tcp_nodelay, latch, headers, queue_size) 00029 00030 self.min_frequency = min_frequency 00031 self.max_frequency = max_frequency 00032 self.default_frequency = default_frequency 00033 self.subscriber_listener = subscriber_listener 00034 self.tcp_nodelay = tcp_nodelay 00035 self.latch = latch 00036 self.headers = headers 00037 self.queue_size = queue_size 00038 00039 rospy.Subscriber(name, rospy.AnyMsg, self.callback) 00040 00041 def start(self, get_message_method, print_message = False): 00042 self.managed_topic_name = self.name + "/optimized" 00043 00044 full_rate_pub = rospy.Publisher(self.name, self.data_class, 00045 self.subscriber_listener, self.tcp_nodelay, self.latch, self.headers, self.queue_size) 00046 managed_rate_pub = rospy.Publisher(self.managed_topic_name, self.data_class, 00047 self.subscriber_listener, self.tcp_nodelay, self.latch, self.headers, self.queue_size) 00048 00049 full_rate = rospy.Rate(self.max_frequency) 00050 managed_rate = DBMRate(managed_rate_pub, self.min_frequency, self.max_frequency, self.default_frequency) 00051 00052 full_rate_thread = RateThread(full_rate, full_rate_pub, get_message_method) 00053 managed_rate_thread = RateThread(managed_rate, managed_rate_pub, get_message_method, print_message) 00054 00055 managed_rate_thread.start() 00056 full_rate_thread.start() 00057 00058 def callback(self, data): 00059 if not hasattr(self, 'last_message_size'): 00060 self.last_message_size = 0 00061 00062 current_message_size = len(data._buff) 00063 00064 if current_message_size != self.last_message_size: 00065 DBMParam.set_message_size_in_bytes(self.name, current_message_size) 00066 00067 self.last_message_size = current_message_size