def send(self, header, message):
""" Sends a header/msg/footer out the socket. Takes care of computing
length field for header and checksum field for footer. """
msg_buff = StringIO()
message.translator().preserialize()
message.translator().serialize(msg_buff)
pad_count = -msg_buff.tell() % 4
msg_buff.write("\x00" * pad_count)
footer = msg.CommonFooter(end=msg.CommonFooter.END)
header.length = msg_buff.tell() + footer.translator().size
# Write header and message to main buffer.
buff = StringIO()
header.translator().serialize(buff)
buff.write(msg_buff.getvalue())
# Write footer.
footer_start = buff.tell()
footer.translator().serialize(buff)
# Compute checksum.
buff.seek(0)
footer.checksum = 65536 - self._checksum(buff)
# Rewrite footer with correct checksum.
buff.seek(footer_start)
footer.translator().serialize(buff)
self.sock.send(buff.getvalue())
def recv(self, d=False):
""" Receive a packet from the port's socket.
Returns (header, pkt_str)
Returns None, None when no data. """
header = msg.CommonHeader()
footer = msg.CommonFooter()
try:
bytes_before_sync = []
while True:
sync = self.sock.recv(1)
if sync == "\xAA":
bytes_before_sync = ''.join(bytes_before_sync)
if len(bytes_before_sync
) > 0 and not bytes_before_sync.startswith(
"\r\n<OK"):
rospy.logwarn((
"Discarded %d bytes between end of previous message "
+ "and next sync byte.") % len(bytes_before_sync))
rospy.logwarn("Discarded: %s" %
repr(bytes_before_sync))
break
bytes_before_sync.append(sync)
sync = self.sock.recv(1)
if sync != "\x44":
raise ValueError(
"Bad sync2 byte, expected 0x44, received 0x%x" %
ord(sync[0]))
sync = self.sock.recv(1)
if sync != "\x12":
raise ValueError(
"Bad sync3 byte, expected 0x12, received 0x%x" %
ord(sync[0]))
# Four byte offset to account for 3 sync bytes and one header length byte already consumed.
header_length = ord(self.sock.recv(1)[0]) - 4
if header_length != header.translator().size:
raise ValueError("Bad header length. Expected %d, got %d" %
(header.translator().size, header_length))
except (socket.timeout, serial.SerialTimeoutException) as e:
rospy.logwarn("Connection timeout... %s" % str(e))
return None, None
header_str = self.sock.recv(header_length)
header_data = StringIO(header_str)
header.translator().deserialize(header_data)
packet_str = self.sock.recv(header.length)
footer_data = StringIO(self.sock.recv(footer.translator().size))
rospy.logwarn("message id: %d" % header.id)
return header, packet_str
def __init__(self, sock, **opts):
super(Port, self).__init__()
self.sock = sock
self.opts = opts
self.daemon = False
self.finish = threading.Event()
self.bSerial = False
# These are only for receiving.
self.header = msg.CommonHeader()
self.footer = msg.CommonFooter()