port.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 #     _____
00004 #    /  _  \
00005 #   / _/ \  \
00006 #  / / \_/   \
00007 # /  \_/  _   \  ___  _    ___   ___   ____   ____   ___   _____  _   _
00008 # \  / \_/ \  / /  _\| |  | __| / _ \ | ┌┐ \ | ┌┐ \ / _ \ |_   _|| | | |
00009 #  \ \_/ \_/ /  | |  | |  | └─┐| |_| || └┘ / | └┘_/| |_| |  | |  | └─┘ |
00010 #   \  \_/  /   | |_ | |_ | ┌─┘|  _  || |\ \ | |   |  _  |  | |  | ┌─┐ |
00011 #    \_____/    \___/|___||___||_| |_||_| \_\|_|   |_| |_|  |_|  |_| |_|
00012 #            ROBOTICS™
00013 #
00014 #
00015 #  Copyright © 2012 Clearpath Robotics, Inc. 
00016 #  All Rights Reserved
00017 #  
00018 # Redistribution and use in source and binary forms, with or without
00019 # modification, are permitted provided that the following conditions are met:
00020 #     * Redistributions of source code must retain the above copyright
00021 #       notice, this list of conditions and the following disclaimer.
00022 #     * Redistributions in binary form must reproduce the above copyright
00023 #       notice, this list of conditions and the following disclaimer in the
00024 #       documentation and/or other materials provided with the distribution.
00025 #     * Neither the name of Clearpath Robotics, Inc. nor the
00026 #       names of its contributors may be used to endorse or promote products
00027 #       derived from this software without specific prior written permission.
00028 # 
00029 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00030 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00031 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00032 # DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY
00033 # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00034 # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00035 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00036 # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00037 # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00038 # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00039 #
00040 # Please send comments, questions, or patches to skynet@clearpathrobotics.com
00041 #
00042 
00043 import applanix_msgs.msg as msg
00044 
00045 # Node source 
00046 from translator import Translator
00047 
00048 # Python
00049 import threading
00050 import socket
00051 import struct
00052 from cStringIO import StringIO
00053 
00054 
00055 class Port(threading.Thread):
00056   """ Common base class for DataPort and ControlPort. Provides functionality to
00057       recv/send Applanix-formatted packets from the socket. Could in future 
00058       support LoggingPort and DisplayPort."""
00059   checksum_struct = struct.Struct("<hh")
00060 
00061   def __init__(self, sock, **opts):
00062     super(Port, self).__init__()
00063     self.sock = sock
00064     self.opts = opts
00065     self.daemon = False
00066     self.finish = threading.Event()
00067 
00068     # These are only for receiving. 
00069     self.header = msg.CommonHeader()
00070     self.footer = msg.CommonFooter()
00071 
00072   def recv(self, d=False):
00073     """ Receive a packet from the port's socket.
00074         Returns (pkt_id, pkt_str), where pkt_id is ("$GRP"|"$MSG", num)
00075         Returns None, None when no data. """
00076     try:
00077       header_str = self.sock.recv(self.header.translator().size)
00078     except socket.timeout:
00079       return None, None
00080 
00081     header_data = StringIO(header_str)
00082     self.header.translator().deserialize(header_data)
00083     pkt_id = (str(self.header.start).encode('string_escape'), self.header.id)
00084 
00085     # Initial sanity check.
00086     if pkt_id[0] not in (msg.CommonHeader.START_GROUP, msg.CommonHeader.START_MESSAGE):
00087       raise ValueError("Bad header %s.%d" % pkt_id)
00088 
00089     # Special case for a troublesome undocumented packet.
00090     if pkt_id == ("$GRP", 20015):
00091       self.sock.recv(135)
00092       return None, None
00093 
00094     # Receive remainder of packet from data socket. 
00095     pkt_str = self.sock.recv(self.header.length)
00096 
00097     # Check package footer.
00098     footer_data = StringIO(pkt_str[-self.footer.translator().size:])
00099     self.footer.translator().deserialize(footer_data)
00100     if str(self.footer.end) != msg.CommonFooter.END:
00101       raise("Bad footer from pkt %s.%d" % pkt_id)
00102 
00103     # Check package checksum.
00104     if self._checksum(StringIO(header_str + pkt_str)) != 0:
00105       raise("Bad checksum from pkt %s.%d: %%d" % pkt_id % checksum)
00106 
00107     return pkt_id, pkt_str 
00108 
00109   def send(self, header, message):
00110     """ Sends a header/msg/footer out the socket. Takes care of computing
00111         length field for header and checksum field for footer. """
00112     msg_buff = StringIO()
00113     message.translator().preserialize()
00114     message.translator().serialize(msg_buff)
00115     pad_count = -msg_buff.tell() % 4
00116     msg_buff.write("\x00" * pad_count)
00117 
00118     footer = msg.CommonFooter(end=msg.CommonFooter.END)
00119     header.length = msg_buff.tell() + footer.translator().size
00120 
00121     # Write header and message to main buffer.
00122     buff = StringIO()
00123     header.translator().serialize(buff)
00124     buff.write(msg_buff.getvalue())
00125      
00126     # Write footer.
00127     footer_start = buff.tell()
00128     footer.translator().serialize(buff) 
00129 
00130     # Compute checksum.
00131     buff.seek(0)
00132     footer.checksum = 65536 - self._checksum(buff)
00133 
00134     # Rewrite footer with correct checksum.
00135     buff.seek(footer_start)
00136     footer.translator().serialize(buff) 
00137 
00138     #print buff.getvalue().encode("string_escape")
00139     self.sock.send(buff.getvalue())
00140 
00141   @classmethod
00142   def _checksum(cls, buff):
00143     """ Compute Applanix checksum. Expects a StringIO with a 
00144       size that is a multiple of four bytes. """
00145     checksum = 0
00146     while True:
00147       data = buff.read(cls.checksum_struct.size)
00148       if len(data) == 0:
00149         break
00150       if len(data) < 4:
00151         raise ValueError("Checksum data length is not a multiple of 4.")
00152       c1, c2 = cls.checksum_struct.unpack(data)
00153       checksum += c1 + c2
00154     return checksum % 65536
00155 


applanix_bridge
Author(s): Mike Purvis
autogenerated on Thu Jan 2 2014 11:05:14