port.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 
00004 # Software License Agreement (BSD)
00005 #
00006 #  file      @port.py
00007 #  authors   Mike Purvis <mpurvis@clearpathrobotics.com>
00008 #            NovAtel <novatel.com/support>
00009 #  copyright Copyright (c) 2012, Clearpath Robotics, Inc., All rights reserved.
00010 #            Copyright (c) 2014, NovAtel Inc., All rights reserved.
00011 #
00012 # Redistribution and use in source and binary forms, with or without modification, are permitted provided that
00013 # the following conditions are met:
00014 #  * Redistributions of source code must retain the above copyright notice, this list of conditions and the
00015 #    following disclaimer.
00016 #  * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
00017 #    following disclaimer in the documentation and/or other materials provided with the distribution.
00018 #  * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
00019 #    products derived from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
00022 # RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
00023 # PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
00024 # DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
00025 # OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00026 # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00028 
00029 import rospy
00030 import novatel_msgs.msg as msg
00031 
00032 # Node source
00033 from translator import Translator
00034 
00035 # Python
00036 import threading
00037 import socket
00038 import struct
00039 from cStringIO import StringIO
00040 
00041 
00042 class Port(threading.Thread):
00043     """ Common base class for DataPort and ControlPort. Provides functionality to
00044       recv/send novatel-formatted packets from the socket. Could in future
00045       support LoggingPort and DisplayPort."""
00046     checksum_struct = struct.Struct("<hh")
00047 
00048     def __init__(self, sock, **opts):
00049         super(Port, self).__init__()
00050         self.sock = sock
00051         self.opts = opts
00052         self.daemon = False
00053         self.finish = threading.Event()
00054         self.bSerial = False
00055 
00056     def recv(self, d=False):
00057         """ Receive a packet from the port's socket.
00058         Returns (header, pkt_str)
00059         Returns None, None when no data. """
00060 
00061         header = msg.CommonHeader()
00062         footer = msg.CommonFooter()
00063 
00064         try:
00065             bytes_before_sync = []
00066             while True:
00067                 sync = self.sock.recv(1)
00068                 if sync == "\xAA":
00069                     bytes_before_sync = ''.join(bytes_before_sync)
00070                     if len(bytes_before_sync) > 0 and not bytes_before_sync.startswith("\r\n<OK"):
00071                         rospy.logwarn(("Discarded %d bytes between end of previous message " +
00072                                       "and next sync byte.") % len(bytes_before_sync))
00073                         rospy.logwarn("Discarded: %s" % repr(bytes_before_sync))
00074                     break
00075                 bytes_before_sync.append(sync)
00076 
00077             sync = self.sock.recv(1)
00078             if sync != "\x44":
00079                 raise ValueError("Bad sync2 byte, expected 0x44, received 0x%x" % ord(sync[0]))
00080             sync = self.sock.recv(1)
00081             if sync != "\x12":
00082                 raise ValueError("Bad sync3 byte, expected 0x12, received 0x%x" % ord(sync[0]))
00083 
00084             # Four byte offset to account for 3 sync bytes and one header length byte already consumed.
00085             header_length = ord(self.sock.recv(1)[0]) - 4
00086             if header_length != header.translator().size:
00087                 raise ValueError("Bad header length. Expected %d, got %d" %
00088                                  (header.translator().size, header_length))
00089 
00090         except socket.timeout:
00091             return None, None
00092 
00093         header_str = self.sock.recv(header_length)
00094         header_data = StringIO(header_str)
00095         header.translator().deserialize(header_data)
00096 
00097         packet_str = self.sock.recv(header.length)
00098         footer_data = StringIO(self.sock.recv(footer.translator().size))
00099 
00100         return header, packet_str
00101 
00102     def send(self, header, message):
00103         """ Sends a header/msg/footer out the socket. Takes care of computing
00104         length field for header and checksum field for footer. """
00105 
00106         msg_buff = StringIO()
00107         message.translator().preserialize()
00108         message.translator().serialize(msg_buff)
00109         pad_count = -msg_buff.tell() % 4
00110         msg_buff.write("\x00" * pad_count)
00111 
00112         footer = msg.CommonFooter(end=msg.CommonFooter.END)
00113         header.length = msg_buff.tell() + footer.translator().size
00114 
00115         # Write header and message to main buffer.
00116         buff = StringIO()
00117         header.translator().serialize(buff)
00118         buff.write(msg_buff.getvalue())
00119 
00120         # Write footer.
00121         footer_start = buff.tell()
00122         footer.translator().serialize(buff)
00123 
00124         # Compute checksum.
00125         buff.seek(0)
00126         footer.checksum = 65536 - self._checksum(buff)
00127 
00128         # Rewrite footer with correct checksum.
00129         buff.seek(footer_start)
00130         footer.translator().serialize(buff)
00131 
00132         self.sock.send(buff.getvalue())
00133 
00134     @classmethod
00135     def _checksum(cls, buff):
00136         """ Compute novatel checksum. Expects a StringIO with a
00137       size that is a multiple of four bytes. """
00138         checksum = 0
00139 
00140         while True:
00141             data = buff.read(cls.checksum_struct.size)
00142 
00143             if len(data) == 0:
00144                 break
00145             if len(data) < 4:
00146                 pad_count = len(data) % 4
00147                 data = data + "\x00" * pad_count
00148                 raise ValueError("Checksum data length is not a multiple of 4. %d" % len(data))
00149             print(data)
00150             c1, c2 = cls.checksum_struct.unpack(data)
00151             checksum += c1 + c2
00152         print(checksum, checksum % 65536)  # novatel 32 bit crc
00153         return checksum % 65536


novatel_span_driver
Author(s): NovAtel Support
autogenerated on Mon Apr 1 2019 10:03:10