silverhammer_util.py
Go to the documentation of this file.
00001 from StringIO import StringIO
00002 from struct import Struct, pack, unpack
00003 import re
00004 import rospy
00005 import roslib
00006 import roslib.message
00007 
00008 # utility function
00009 def parseMessageType(field_string):
00010     match = re.match("(.*)\[([\d]*)\]", field_string)
00011     if match:
00012         num_string = match.group(2)
00013         return (match.group(1), int(num_string))
00014     else:
00015         return (field_string, 1)
00016 def fieldToTopic(field):
00017     field.replace("__", "/")
00018     
00019 def msgToStructFormat(msg):
00020     slots = list(msg.__slots__)           #copy
00021     slot_types = list(msg._slot_types)
00022     fmt_stream = StringIO()
00023     fmt_stream.write("!")
00024     for slot, slot_type in zip(slots, slot_types):
00025         parsed_type = parseMessageType(slot_type)
00026         field_type = parsed_type[0]
00027         field_length = parsed_type[1]
00028         if field_type == "char":
00029             fmt_stream.write("c" * field_length)
00030         elif field_type == "bool":
00031             fmt_stream.write("?" * field_length)
00032         elif field_type == "int8":
00033             fmt_stream.write("b" * field_length)
00034         elif field_type == "uint8":
00035             fmt_stream.write("B" * field_length)
00036         elif field_type == "int16":
00037             raise Exception("int16 is not supported")
00038         elif field_type == "uint16":
00039             fmt_stream.write("H" * field_length)
00040         elif field_type == "int32":
00041             fmt_stream.write("i" * field_length)
00042         elif field_type == "uint32":
00043             fmt_stream.write("I" * field_length)
00044         elif field_type == "int64":
00045             fmt_stream.write("q" * field_length)
00046         elif field_type == "uint64":
00047             fmt_stream.write("Q" * field_length)
00048         elif field_type == "float32":
00049             fmt_stream.write("f" * field_length)
00050         elif field_type == "float64":
00051             fmt_stream.write("d" * field_length)
00052         elif field_type == "string":
00053             raise Excception("string is not supported!, please use char[static_length]")
00054         elif field_type == "duration" or field_type == "time":
00055             raise Excception("duration and time are not supported")
00056     return fmt_stream.getvalue()
00057 
00058 def packableValue(value, value_type):
00059     if value_type == "bool":
00060         return value
00061     else:
00062         return ord(value)
00063     
00064 def packMessage(msg, fmt):
00065     data = []
00066     for slot, slot_type in zip(msg.__slots__, msg._slot_types):
00067         slot_value = getattr(msg, slot)
00068         parsed_type = parseMessageType(slot_type)
00069         field_type = parsed_type[0]
00070         if hasattr(slot_value, "__len__"):   #array
00071             for i in range(len(slot_value)):
00072                 data.append(packableValue(slot_value[i], field_type))
00073         else:
00074             data.append(packableValue(slot_value, field_type))
00075     packed = pack(fmt, *data)
00076     return packed
00077 
00078 def unpackArrayValue(array, field_type):
00079     if field_type == "bool":
00080         return [ord(v) for v in value]
00081     else:
00082         return array
00083 
00084 def unpackValue(val, field_type):
00085     if field_type == "bool":
00086         return ord(val) == 1
00087     else:
00088         return val
00089     
00090 def unpackMessage(data, fmt, message_class):
00091     unpacked_data = unpack(fmt, data)
00092     msg = message_class()
00093     counter = 0
00094     for slot, slot_type in zip(msg.__slots__, msg._slot_types):
00095         slot_value = getattr(msg, slot)
00096         parsed_type = parseMessageType(slot_type)
00097         field_type = parsed_type[0]
00098         field_length = parsed_type[1]
00099         target_data = data[counter:counter + field_length]
00100         if hasattr(slot_value, "__len__"):   #array
00101             setattr(msg, slot, unpackArrayValue(target_data, field_type))
00102         else:
00103             setattr(msg, slot, unpackValue(target_data[0], field_type))
00104         counter = counter + field_length
00105     return msg
00106 
00107 def publishersFromMessage(msg, prefix=""):
00108     ret = []
00109     for slot, slot_type in zip(msg.__slots__, msg._slot_types):
00110         topic_name = prefix + "/" + slot.replace("__", "/")
00111         try:
00112             msg_class = roslib.message.get_message_class(slot_type)
00113         except:
00114             raise Exception("invalid topic type: %s"%slot_type)
00115         ret.append(rospy.Publisher(topic_name, msg_class))
00116     return ret
00117 
00118 def decomposeLargeMessage(msg, prefix=""):
00119     ret = dict()
00120     for slot, slot_type in zip(msg.__slots__, msg._slot_types):
00121         topic_name = prefix + "/" + slot.replace("__", "/")
00122         print topic_name
00123         ret[topic_name] = getattr(msg, slot)
00124     return ret
00125         
00126     
00127 def subscribersFromMessage(msg):
00128     ret = []
00129     for slot, slot_type in zip(msg.__slots__, msg._slot_types):
00130         topic_name = "/" + slot.replace("__", "/")
00131         try:
00132             msg_class = roslib.message.get_message_class(slot_type)
00133         except:
00134             raise Exception("invalid topic type: %s"%slot_type)
00135         ret.append((topic_name, msg_class))
00136     return ret
00137 
00138 #################################################w
00139 # Packet definition for Large Data
00140 # |SeqID(4byte)|ID(4byte)|NUM_PACKET(4byte)|data.... |
00141 #################################################w
00142 class LargeDataUDPPacket():
00143     def __init__(self, seq_id, id, num, data, packet_size):
00144         self.id = id
00145         self.seq_id = seq_id
00146         self.num = num
00147         self.data = data
00148         self.packet_size = packet_size
00149     def pack(self):
00150         return pack("!III%ds" % (len(self.data)),
00151                     self.seq_id, self.id, self.num, self.data)
00152     #def pack(self):
00153     @classmethod
00154     def headerSize(cls):
00155         return 4 + 4 + 4
00156     @classmethod
00157     def fromData(cls, data, packet_size):
00158         unpacked = unpack("!III%ds" % (len(data) - cls.headerSize()), data)
00159         ret = cls(unpacked[0], unpacked[1], unpacked[2], unpacked[3], 
00160                   packet_size)
00161         #print "buffer length", len(unpacked[3])
00162         return ret
00163         
00164 def separateBufferIntoPackets(seq_id, buffer, packet_size):
00165     buffer_packet_size = packet_size - LargeDataUDPPacket.headerSize()
00166     num_packet = len(buffer) / buffer_packet_size
00167     if len(buffer) % buffer_packet_size != 0:
00168         num_packet = num_packet + 1
00169     packets = []
00170     for i in range(num_packet):
00171         packets.append(LargeDataUDPPacket(seq_id, i, num_packet, 
00172                                           buffer[i*buffer_packet_size:(i+1)*buffer_packet_size],
00173                                           packet_size))
00174         #print "buffer length", len(buffer[i*num_packet:(i+1)*num_packet])
00175     return packets


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