silverhammer_lowspeed_receiver.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
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 import roslib
00013 from roslib.message import get_message_class
00014 
00015 class SilverHammerUDPListener():
00016     def __init__(self, server, buffer_size, format, message, pub):
00017         self.server = server
00018         self.format = format
00019         self.pub = pub
00020         self.message = message
00021         self.buffer_size = buffer_size
00022     def run(self):
00023         recv_data, addr = self.server.recvfrom(self.buffer_size)
00024         msg = unpackMessage(recv_data, self.format, self.message)
00025         self.pub.publish(msg)
00026         print "received:", msg
00027 
00028 class SilverHammerLowspeedReceiver():
00029     def __init__(self):
00030         message_class_str = rospy.get_param("~message", 
00031                                             "jsk_network_tools/FC2OCS")
00032         try:
00033             self.receive_message = get_message_class(message_class_str)
00034         except:
00035             raise Exception("invalid topic type: %s"%message_class_str)
00036         self.receive_port = rospy.get_param("~receive_port", 1024)
00037         self.receive_ip = rospy.get_param("~receive_ip", "127.0.0.1")
00038         self.receive_buffer = rospy.get_param("~receive_buffer_size", 250)
00039         self.socket_server = socket(AF_INET, SOCK_DGRAM)
00040         self.socket_server.bind((self.receive_ip, self.receive_port))
00041         self.receive_format = msgToStructFormat(self.receive_message())
00042         self.pub = rospy.Publisher("~output", self.receive_message)
00043     def messageCallback(self, msg):
00044         with self.lock:
00045             self.latest_message = msg
00046     def sendTimerCallback(self, event):
00047         with self.lock:
00048             if self.latest_message:
00049                 rospy.loginfo("sending message")
00050                 packed_data = packMessage(self.latest_message, self.send_format)
00051                 self.socket_client.sendto(packed_data, (self.to_ip, self.to_port))
00052                 self.latest_message = None
00053             else:
00054                 rospy.loginfo("no message is available")
00055     def run(self):
00056         while not rospy.is_shutdown():
00057             recv_data, addr = self.socket_server.recvfrom(self.receive_buffer)
00058             msg = unpackMessage(recv_data, self.receive_format, 
00059                                 self.receive_message)
00060             self.pub.publish(msg)
00061             print "received:", msg
00062 
00063 if __name__ == "__main__":
00064     rospy.init_node("silverhammer_lowspeed_receiver")
00065     rc = SilverHammerLowspeedReceiver()
00066     rc.run()


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