silverhammer_highspeed_streamer.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 from jsk_network_tools.msg import FC2OCSLargeData
00005 from jsk_network_tools.silverhammer_util import *
00006 from threading import Lock
00007 from StringIO import StringIO
00008 from io import BytesIO
00009 from socket import *
00010 
00011 import roslib
00012 from roslib.message import get_message_class
00013 
00014 class SilverHammerStreamer:
00015     def __init__(self):
00016         message_class_str = rospy.get_param("~message", 
00017                                             "jsk_network_tools/FC2OCSLargeData")
00018         try:
00019             self.message_class = get_message_class(message_class_str)
00020         except:
00021             raise Exception("invalid topic type: %s"%message_class_str)
00022         self.lock = Lock()
00023         self.send_port = rospy.get_param("~to_port", 16484)
00024         self.send_ip = rospy.get_param("~to_ip", "localhost")
00025         self.rate = rospy.get_param("~send_rate", 2)   #2Hz
00026         self.socket_client = socket(AF_INET, SOCK_DGRAM)
00027         self.packet_size = rospy.get_param("~packet_size", 1000)   #2Hz
00028         subscriber_info = subscribersFromMessage(self.message_class())
00029         self.messages = {}
00030         self.subscribe(subscriber_info)
00031         self.counter = 0
00032         self.send_timer = rospy.Timer(rospy.Duration(1.0 / self.rate),
00033                                       self.sendTimerCallback)
00034         
00035     def sendTimerCallback(self, event):
00036         buffer = StringIO()
00037         with self.lock:
00038             msg = self.message_class()
00039             subscriber_info = subscribersFromMessage(msg)
00040             for topic, message_class in subscriber_info:
00041                 field_name = topic[1:].replace("/", "__")
00042                 if topic in self.messages:
00043                     setattr(msg, field_name, self.messages[topic])
00044             rospy.msg.serialize_message(buffer, 0, msg)
00045         # send buffer as UDP
00046         self.sendBuffer(buffer.getvalue())
00047     def sendBuffer(self, buffer):
00048         packets = separateBufferIntoPackets(self.counter, buffer, self.packet_size)
00049         rospy.loginfo("sending %d packets", len(packets))
00050         for p in packets:
00051             self.socket_client.sendto(p.pack(), (self.send_ip, self.send_port))
00052         self.counter = self.counter + 1
00053         if self.counter > 65535:
00054             self.counter = 0
00055     def subscribe(self, subscriber_infos):
00056         self.subscribers = []
00057         for topic, message_class in subscriber_infos:
00058             sub = rospy.Subscriber(topic, message_class, self.genMessageCallback(topic))
00059             self.subscribers.append(sub)
00060     def messageCallback(self, msg, topic):
00061         with self.lock:
00062             self.messages[topic] = msg
00063     def genMessageCallback(self, topic):
00064         return lambda msg: self.messageCallback(msg, topic)
00065 
00066 if __name__ == "__main__":
00067     rospy.init_node("silverhammer_highspeed_streamer")
00068     streamer = SilverHammerStreamer()
00069     rospy.spin()
00070     


jsk_network_tools
Author(s): Yusuke Furuta
autogenerated on Sun Jan 25 2015 00:30:33