Go to the documentation of this file.00001
00002
00003 from jsk_network_tools.msg import FC2OCS, OCS2FC
00004 from jsk_network_tools.silverhammer_util import *
00005 from threading import Lock, Thread
00006 from socket import *
00007 from struct import Struct
00008 import os
00009 import rospy
00010 import signal
00011 import sys
00012
00013 import roslib
00014 from roslib.message import get_message_class
00015
00016
00017 class SilverHammerLowspeedStreamer():
00018 def __init__(self):
00019 message_class_str = rospy.get_param("~message",
00020 "jsk_network_tools/FC2OCS")
00021 try:
00022 self.send_message = get_message_class(message_class_str)
00023 except:
00024 raise Exception("invalid topic type: %s"%message_class_str)
00025 self.lock = Lock()
00026 self.to_port = rospy.get_param("~to_port", 1024)
00027 self.to_ip = rospy.get_param("~to_ip", "127.0.0.1")
00028 self.send_rate = rospy.get_param("~send_rate", 1)
00029 self.latest_message = None
00030 self.socket_client = socket(AF_INET, SOCK_DGRAM)
00031 self.send_format = msgToStructFormat(self.send_message())
00032 self.sub = rospy.Subscriber("~input",
00033 self.send_message, self.messageCallback)
00034 self.send_timer = rospy.Timer(rospy.Duration(1 / self.send_rate),
00035 self.sendTimerCallback)
00036 def messageCallback(self, msg):
00037 with self.lock:
00038 self.latest_message = msg
00039 def sendTimerCallback(self, event):
00040 with self.lock:
00041 if self.latest_message:
00042 rospy.loginfo("sending message")
00043 packed_data = packMessage(self.latest_message, self.send_format)
00044 self.socket_client.sendto(packed_data, (self.to_ip, self.to_port))
00045 self.latest_message = None
00046 else:
00047 rospy.loginfo("no message is available")
00048
00049
00050 if __name__ == "__main__":
00051 rospy.init_node("silverhammer_lowspeed_streamer")
00052 st = SilverHammerLowspeedStreamer()
00053 rospy.spin()
00054
00055
00056