protocol.py
Go to the documentation of this file.
00001 #! /usr/bin/env python -m
00002 # -*- coding: utf-8 -*-
00003 #     _____
00004 #    /  _  \
00005 #   / _/ \  \
00006 #  / / \_/   \
00007 # /  \_/  _   \  ___  _    ___   ___   ____   ____   ___   _____  _   _
00008 # \  / \_/ \  / /  _\| |  | __| / _ \ | ┌┐ \ | ┌┐ \ / _ \ |_   _|| | | |
00009 #  \ \_/ \_/ /  | |  | |  | └─┐| |_| || └┘ / | └┘_/| |_| |  | |  | └─┘ |
00010 #   \  \_/  /   | |_ | |_ | ┌─┘|  _  || |\ \ | |   |  _  |  | |  | ┌─┐ |
00011 #    \_____/    \___/|___||___||_| |_||_| \_\|_|   |_| |_|  |_|  |_| |_|
00012 #            ROBOTICS™
00013 #
00014 #  File: protocol.py
00015 #  Desc: Horizon Protocol Handlers
00016 #  
00017 #  Copyright © 2010 Clearpath Robotics, Inc. 
00018 #  All Rights Reserved
00019 # 
00020 #  Redistribution and use in source and binary forms, with or without
00021 #  modification, are permitted provided that the following conditions are met:
00022 #      * Redistributions of source code must retain the above copyright
00023 #        notice, this list of conditions and the following disclaimer.
00024 #      * Redistributions in binary form must reproduce the above copyright
00025 #        notice, this list of conditions and the following disclaimer in the
00026 #        documentation and/or other materials provided with the distribution.
00027 #      * Neither the name of Clearpath Robotics, Inc. nor the
00028 #        names of its contributors may be used to endorse or promote products
00029 #        derived from this software without specific prior written permission.
00030 # 
00031 #  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
00032 #  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
00033 #  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
00034 #  ARE DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY
00035 #  DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00036 #  (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00037 #  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00038 #  ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00039 #  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00040 #  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00041 #
00042 #  Please send comments, questions, or patches to code@clearpathrobotics.com
00043 #
00044 
00045 
00046 
00047 
00048 ################################################################################
00049 # Script
00050 
00051 
00052 
00053 # Check if run as a script
00054 if __name__ == "__main__":
00055     
00056     # Warn of Module ONLY status
00057     print ("ERROR: clearpath.horizon.protocol is a module and can NOT be run"\
00058            " as a script!\nFor a command-line interface demo of Horizon, run:"\
00059            "\n  python -m clearpath.horizon.demo\n"\
00060            "For Horizon message forwarding, run:\n"\
00061            "  python -m clearpath.horizon.forward")
00062 
00063     # Exit Error
00064     import sys
00065     sys.exit(1)
00066 
00067 
00068 
00069 
00070 ################################################################################
00071 # Module
00072 
00073 
00074 
00075 ## @package clearpath.horizon.protocol 
00076 #  Horizon Protocol Python Module
00077 # 
00078 #  Horizon Protocol Message Handlers                                          \n
00079 #  Abstracted from knowing messages & underlying transport.                   \n
00080 #  Supported Horizon version(s): 0.1 - 1.0
00081 #
00082 #  @author     Ryan Gariepy
00083 #  @author     Malcolm Robert
00084 #  @date       18/01/10
00085 #  @todo       Implement the protocol server wrapper & document in use
00086 #  @req        clearpath.utils                                                \n
00087 #              clearpath.horizon.codes                                        \n
00088 #              clearpath.horizon.messages                                     \n
00089 #              clearpath.horizon.payloads                                     \n
00090 #              clearpath.horizon.transports                                   \n
00091 #  @version    1.0
00092 #
00093 #  @section HORIZON
00094 #  @copydoc overview1
00095 #
00096 #  @section USE
00097 #
00098 #  The intended purpose of this module is to provide a layer between the 
00099 #  Horizon interface (or emulator) and the low-level transports that will 
00100 #  automatically handle acknowledgments and message formatting. Only 
00101 #  Payload classes, message codes, and errors are exposed.
00102 #
00103 #
00104 #  @section HISTORY
00105 #  Version 0.1 - 0.3 {Ryan Gariepy}
00106 #  - Initial Creation as protocol.py
00107 #
00108 #  Version 0.4 {Malcolm Robert}
00109 #  - Move to horizon_protocol.py
00110 #  - Added protocol abstraction
00111 #  - Added TCP/IP
00112 #  - Added UDP
00113 #  - Added logging
00114 #  - Added version support
00115 #  - Added Doxygen documentation
00116 #  - Changed version scheme to match Horizon doc
00117 #  - Horizon support for v0.4
00118 #
00119 #  Version 0.5
00120 #  - Horizon support for v0.5
00121 #
00122 #  Version 0.6
00123 #  - Moved to protocol.py
00124 #  - Horizon support for v0.6
00125 #  - Improved search for header in read
00126 #  - Added TCP encryption
00127 #
00128 #  Version 0.7
00129 #  - Extracted transports to transports.py
00130 #  - Horizon support for v0.7
00131 #
00132 #  Version 0.8
00133 #  - Horizon support for v 0.1 - 0.8
00134 #
00135 #  Version 1.0
00136 #  - Horizon support for v 0.1 - 1.0
00137 #  - Python 2.6+ & 3.x compatible
00138 #
00139 #  @section License
00140 #  @copydoc public_license
00141 #
00142 #  @defgroup overview1 Overview Part I
00143 #  @ingroup overview
00144 #  This protocol is meant as a simple way for users of the various Clearpath 
00145 #  Robotics research offerings to interface with the Clearpath Robotics 
00146 #  hardware. It includes several features intended to increase communication 
00147 #  reliability, while keeping message overhead and protocol complexity low. 
00148 #  For the sake of rapid prototyping, it is not intended for multiple devices 
00149 #  to be simultaneously connected to each communication line, removing the need 
00150 #  for addressing or negotiation.
00151 #
00152 """Horizon Protocol Message Handlers
00153 
00154    Copyright © 2010 Clearpath Robotics, Inc.
00155    All rights reserved
00156    
00157    Created: 18/01/10
00158    Authors: Ryan Gariepy & Malcolm Robert
00159    Version: 1.0
00160    """
00161 
00162 
00163 # Required Clearpath Modules
00164 from .. import utils            # Clearpath Utilities
00165 from .  import codes            # Horizon Message Codes
00166 from .  import messages         # Horizon Protocol Message Definition
00167 from .  import transports       # Horizon Transport Definitions
00168 
00169 # Required Python Modules
00170 import datetime                 # Date & Time Manipulation
00171 import logging                  # Logging Utilities
00172 import sys                      # Python Interpreter Functionality
00173 import time                     # System Date & Time
00174 import math
00175 
00176 
00177 # Module Support
00178 __version__  = "1.0"
00179 __revision__ = "$Revision: 916 $"
00180 
00181 
00182 ## Message Log
00183 logger = logging.getLogger('clearpath.horizon.protocol')
00184 """Horizon Protocol Module Log"""
00185 logger.setLevel(logging.NOTSET)
00186 logger.addHandler(utils.NullLoggingHandler())
00187 logger.propagate = False
00188 logger.debug("Loading clearpath.horizon.protocol ...")         
00189 
00190 
00191 
00192 
00193 class Client(object):
00194     """Horizon Transport Protocol Controller - Client Device"""
00195     
00196     
00197         
00198     ## Create A Horizon Protocol Client
00199     #  
00200     #  Constructor for the Horizon message Transport protocol client.         \n
00201     #  Performs the initial creation and initialization of the underlying 
00202     #  transport.                                                             \n
00203     #  Does NOT support version auto-detection.                               \n
00204     #                                                                         \n
00205     #  Refer to the transport's __init__ method for argument specifications.
00206     #                                                                         \n
00207     #  Override this method for subclass initialization.                      \n
00208     #  Overriding methods should call this method. 
00209     #
00210     #  @param  retries        The number of times to retry sending a message
00211     #                         that received a timeout or checksum error
00212     #  @param  send_timeout   The time to wait for an acknowledgment
00213     #                         in milliseconds, 0 - wait indefinitely
00214     #  @param  store_timeout  The time to store an un-handled message for the 
00215     #                         method get_waiting in milliseconds,
00216     #                         0 - store indefinitely
00217     #  @param  sys_time       Use system time (True) instead of time since 
00218     #                         instantiation (False)?
00219     #  @param  transport      The Transport class to use.
00220     #  @param  transport_args Dictionary of arguments to pass to the transport's
00221     #                         __init__ method. Do NOT include version or
00222     #                         store_timeout as these will be populated.
00223     #
00224     #  @pydoc
00225     def __init__(self, transport, transport_args, retries, 
00226                  send_timeout, rec_timeout, store_timeout):
00227 
00228         # Class Variables
00229         ## Message Handlers
00230         self._handlers_lock = transports.threading.Lock()
00231         self._handlers = { 0:[] }     # Format: { code:[handler] }
00232 
00233         self.start_time = time.time()
00234         self._transport = None
00235         self._transport_func = transport
00236         self._transport_args = transport_args
00237         self._retries = retries
00238         self._send_timeout = send_timeout
00239         self._rec_timeout = rec_timeout
00240         
00241         self.acks = True;
00242  
00243         # Create Transport
00244         transport_args['receive_callback'] = self.do_handlers
00245         transport_args['store_timeout'] = store_timeout
00246 
00247 
00248     def __del__(self):
00249         """Destroy A Horizon Transport"""
00250         # Cleanup Transport
00251         self.close()
00252         
00253 
00254     def __str__(self):
00255         """Return the transport name."""
00256         return str(self._transport)
00257     
00258 
00259     def open(self):
00260         if not self._transport:
00261             self._transport = self._transport_func(**self._transport_args)
00262             if not isinstance(self._transport, transports.Transport):
00263                 raise ValueError ("Invalid transport!")
00264 
00265         if not self._transport.is_open():
00266             self._transport.open()
00267         
00268     
00269     def close(self):
00270         self.remove_handler()
00271         if self._transport != None:
00272             self._transport.close()
00273 
00274 
00275     # If no offset provided, use the program start time.
00276     def timestamp(self):
00277         return math.floor((time.time() - self.start_time) * 1000)
00278 
00279 
00280     def emergency_stop(self):
00281         code = codes.codes['safety_status']
00282         self.send_message(code.set, code.payload(code.payload.EMERGENCY_STOP))
00283 
00284 
00285     def command(self, name, args):
00286         if 'self' in args: del args['self']
00287         self.send_message(messages.Message.command(name, args, self.timestamp(), no_ack=(not self.acks)))
00288 
00289 
00290     def request(self, name, args):
00291         try:
00292            # Prepare 1-off handler for the first response.
00293             self._received = None
00294             self.add_handler(handler = self._receiver, request = name)
00295             
00296             # Send the Message - blocks on its ack.
00297             if 'self' in args: del args['self']  
00298             message = messages.Message.request(name, args, self.timestamp())
00299             self.send_message(message)
00300 
00301             # If this is explicitly a subscription-cancelation request,
00302             # then exit now...
00303             if 'subscription' in args and args['subscription'] == 0xFFFF:
00304                 return;
00305 
00306             # Otherwise... wait on a first reply to return.
00307             retries = self._retries
00308             start = self.timestamp()
00309             while True:
00310                 if self.timestamp() - start > self._rec_timeout:
00311                     if retries > 0:
00312                         message = message.copy(timestamp=self.timestamp())
00313                         self.send_message(message)
00314                         retries -= 1           
00315                         start = self.timestamp()
00316                     else:
00317                         raise utils.TimeoutError (
00318                             "Timeout Occurred waiting for response!")
00319 
00320                 if self._received != None:
00321                     return self._received[1]
00322 
00323                 time.sleep(0.001)
00324         finally:
00325             self.remove_handler(handler = self._receiver, request = name)
00326 
00327 
00328 
00329     def _receiver(self, name, payload, timestamp):
00330         self._received = (name, payload, timestamp)
00331 
00332 
00333     # Blocking message sender.
00334     def send_message(self, message): 
00335         """Horizon Protocol Send Message"""
00336         if not self._transport.is_open(): 
00337             raise IOError ("Transport has not been opened!")
00338  
00339         # Non-blocking message sender
00340         self._transport.send_message(message)
00341         
00342         # Unless instructed otherwise, block on receiving the acknowledgment
00343         # to this message, re-send as necessary.
00344         if not message.no_ack:
00345             tries = self._retries
00346             while True:
00347                 ack = self._transport.receiver.has_ack(message.timestamp)
00348                 if ack:
00349                     if ack.payload.bad_code:
00350                         raise utils.UnsupportedCodeError("Acknowledgment says Bad Code.")
00351                     elif ack.payload.bad_format:
00352                         raise utils.FormatError("Acknowledgment says Bad Format.")
00353                     elif ack.payload.bad_values:
00354                         raise ValueError("Acknowledgment says Bad Values.")
00355                     elif ack.payload.bad_frequency:
00356                         raise utils.SubscriptionError("Acknowledgment says Bad Frequency.")
00357                     elif ack.payload.bad_code_count:
00358                         raise utils.SubscriptionError("Acknowledgment says Too Many Subscriptions.")
00359                     elif ack.payload.bad_bandwidth:
00360                         raise utils.SubscriptionError("Acknowledgment says Not Enough Bandwidth.")
00361                     else:
00362                         # Message delivered and acknowledged successfully.
00363                         return True
00364 
00365                 if self.timestamp() - message.timestamp > self._send_timeout:
00366                     if tries > 0:
00367                         # Attempt a re-send
00368                         tries -= 1
00369                         message = message.copy(timestamp = self.timestamp())
00370                         self._transport.send_message(message)
00371                     else:
00372                         raise utils.TimeoutError("Message Timeout Occurred!")
00373 
00374                 time.sleep(0.001)
00375 
00376 
00377     def add_handler(self, handler, backtrack = False, request = None):
00378         """Horizon Protocol Add Data Message Handler"""
00379         code = 0
00380         if request != None:
00381             code = codes.codes[request].data()
00382 
00383         with self._handlers_lock:
00384             # Add Handler
00385             if code not in self._handlers:
00386                 self._handlers[code] = []
00387             self._handlers[code].append(handler)
00388         
00389             # Backtrack
00390             if backtrack:
00391                 for tup in self.get_waiting(request): 
00392                     handler(tup[0], tup[1], tup[2])
00393 
00394 
00395     def remove_handler(self, handler=None, request=None):
00396         """Horizon Protocol Remove Data Message Handler"""
00397         code = 0
00398         if request != None:
00399             code = codes.codes[request].data()
00400 
00401         with self._handlers_lock:
00402             # Remove Handler
00403             if code in self._handlers: 
00404                 if handler != None and handler in self._handlers[code]:
00405                     self._handlers[code].remove(handler)
00406                 elif handler == None:
00407                     self._handlers[code] = []
00408 
00409 
00410     # This function is called from the secondary thread
00411     # transports.Serial.Receiver. Do not manipulate global state!
00412     def do_handlers(self, message):
00413         with self._handlers_lock:
00414             handlers = self._handlers[0][:]
00415             if message.code in self._handlers:
00416                 handlers += self._handlers[message.code]
00417 
00418         for handler in handlers:
00419             handler(codes.names[message.code], message.payload, message.timestamp)
00420 
00421         return len(handlers) > 0
00422 
00423 
00424     def get_waiting(self, request = None):
00425         code = 0
00426         if request != None:
00427             code = codes.codes[request].data
00428 
00429         waiting = []
00430         for message in self._transport.receiver.get_waiting(code):
00431             waiting.append((codes.names[message.code], message.payload, message.timestamp))
00432 
00433         return waiting
00434 
00435 
00436     def is_open(self):
00437         return self._transport != None and self._transport.is_open()
00438    
00439 
00440 logger.debug("... clearpath.horizon.protocol loaded.")


clearpath_base
Author(s): Mike Purvis
autogenerated on Sat Dec 28 2013 16:50:48