$search
00001 #! /usr/bin/env python -m 00002 # -*- coding: utf-8 -*- 00003 # _____ 00004 # / _ \ 00005 # / _/ \ \ 00006 # / / \_/ \ 00007 # / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ 00008 # \ / \_/ \ / / _\| | | __| / _ \ | ┌┐ \ | ┌┐ \ / _ \ |_ _|| | | | 00009 # \ \_/ \_/ / | | | | | └─┐| |_| || └┘ / | └┘_/| |_| | | | | └─┘ | 00010 # \ \_/ / | |_ | |_ | ┌─┘| _ || |\ \ | | | _ | | | | ┌─┐ | 00011 # \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| 00012 # ROBOTICS™ 00013 # 00014 # File: transports.py 00015 # Desc: Horizon Message Transport Controllers 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.transports 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.transports 00076 # Horizon Transports Python Module 00077 # 00078 # Horizon Message Transport Controllers \n 00079 # Supported Horizon version(s): 0.1 - 1.0 \n 00080 # \n 00081 # Supported Transports: 00082 # - Serial (RS-232) 00083 # 00084 # @author Ryan Gariepy 00085 # @author Malcolm Robert 00086 # @date 18/01/10 00087 # @todo Would be nice to support version translation in IP Servers, 00088 # however, that would require additional work to detect the 00089 # version that clients are using which is currently not supported 00090 # by the current framework. 00091 # @todo FIX encryption 00092 # @req clearpath.utils \n 00093 # clearpath.horizon.messages \n 00094 # clearpath.horizon.payloads \n 00095 # pySerial [http://pyserial.sourceforge.net/] \n 00096 # -- for Serial 00097 # @version 1.0 00098 # 00099 # @section USE 00100 # 00101 # The intended purpose of this module is to provide functionality to send 00102 # and receive Horizon messages over the various transports with minimal 00103 # knowledge of messages and no knowledge of message payloads. Further, as 00104 # some transports support a one-to-many connection scheme, this module also 00105 # provides message routing. Note that as this module implements the transports 00106 # as defined in the Horizon specification, there is no support for version 00107 # translation. If version translation is desired, refer to the module 00108 # clearpath.horizon.forward. 00109 # 00110 # @section HISTORY 00111 # 00112 # Versions 0.1 - 0.3 {Ryan Gariepy} 00113 # - Initial Creation as protocol.py 00114 # 00115 # Version 0.4 {Malcolm Robert} 00116 # - Moved to horizon_protocol.py 00117 # - Added protocol abstraction 00118 # - Added TCP/IP 00119 # - Added UDP 00120 # - Added logging 00121 # - Added version support 00122 # - Added Doxygen documentation 00123 # - Changed version scheme to match Horizon doc 00124 # - Horizon support for v0.4 00125 # 00126 # Version 0.5 00127 # - Horizon support for v0.5 00128 # 00129 # Version 0.6 00130 # - Moved to protocol.py 00131 # - Horizon support for v0.6 00132 # - Improved search for header in read 00133 # - Added TCP encryption 00134 # 00135 # Version 0.7 00136 # - Moved to transport.py 00137 # - Added Server transports 00138 # - Added IPv6 support 00139 # - Horizon support for v 0.1 - 0.7 00140 # - Python 2.6+ & 3.x compatible 00141 # 00142 # Version 0.8 00143 # - Horizon support for v 0.1 - 0.8 00144 # 00145 # Version 1.0 00146 # - Added one-to-many support 00147 # - Horizon support for v 0.1 - 1.0 00148 # 00149 # @section License 00150 # @copydoc public_license 00151 # 00152 # @defgroup hardware Hardware 00153 # @ingroup doc 00154 # 00155 # @copydoc serial 00156 # 00157 # If higher bandwidth communication is required, the protocol may also be 00158 # packaged verbatim within TCP/IP or UDP packets if desired, with one protocol 00159 # message per packet. The default Horizon port for TCP/UDP is 7284. 00160 # 00161 # @copydoc tcp 00162 # 00163 # @copydoc udp 00164 # 00165 """Horizon Message Transport Controllers 00166 00167 Copyright © 2010 Clearpath Robotics, Inc. 00168 All rights reserved 00169 00170 Created: 17/03/10 00171 Authors: Ryan Gariepy & Malcolm Robert 00172 Version: 1.0 00173 """ 00174 00175 00176 # Required Clearpath Modules 00177 from .. import utils # Clearpath Utilities 00178 from . import messages # Horizon Protocol Message Definition 00179 from . import codes # Horizon Protocol Versions 00180 00181 # Required Python Modules 00182 import logging # Logging Utilities 00183 import socket # UDP Port Control 00184 import sys # Python Interpreter Functionality 00185 import threading # Python Thread Support 00186 import time # System Date & Time 00187 import collections # deque 00188 import math 00189 00190 # Version Dependent Modules 00191 try: 00192 import serial # Serial Port Control 00193 except ImportError: 00194 pass 00195 00196 MessageRecord = collections.namedtuple('MessageRecord', 'message, expiry') 00197 00198 00199 # Module Support 00200 ## Module Version 00201 __version__ = "1.0" 00202 """Module Version""" 00203 ## SVN Code Revision 00204 __revision__ = "$Revision: 898 $" 00205 """ SVN Code Revision""" 00206 00207 00208 ## Message Log 00209 logger = logging.getLogger('clearpath.horizon.transports') 00210 """Horizon Transports Module Log""" 00211 logger.setLevel(logging.NOTSET) 00212 logger.addHandler(utils.NullLoggingHandler()) 00213 logger.propagate = False 00214 logger.debug("Loading clearpath.horizon.transports ...") 00215 00216 00217 00218 00219 ################################################################################ 00220 # Horizon Transport Controller 00221 00222 00223 00224 ## Transport 00225 # 00226 class Transport(object): 00227 """Horizon Transport Base Class""" 00228 pass 00229 00230 00231 ## Horizon Serial Controller 00232 # 00233 # Provides a method to send and receive messages over RS-232. \n 00234 # Guarantees order of arrival and arrival. \n 00235 # Low Bandwidth use only. 00236 # 00237 # @req pySerial [http://pyserial.sourceforge.net/] 00238 # @since 0.1 00239 # 00240 # @pydoc 00241 class Serial(Transport): 00242 """Horizon Transport Controller - Serial Device""" 00243 00244 def __init__(self, port = None, store_timeout = 0, receive_callback = None): 00245 """Create A Horizon Serial Transport""" 00246 00247 # Dependency check 00248 try: 00249 serial 00250 except NameError: 00251 logger.error("%s: Cannot create Horizon Serial Transport without"\ 00252 "pySerial!" % self.__class__.__name__) 00253 raise utils.TransportError ("pySerial not found!") 00254 00255 if port == None: 00256 raise utils.TransportError \ 00257 ("Serial transport creation failed: port not specified!\n") 00258 00259 # Class Variables 00260 self.port = port 00261 self._serial = None 00262 self._opened = False 00263 self.store_timeout = store_timeout 00264 self.receive_callback = receive_callback 00265 self.serial_write_lock = threading.Lock() 00266 00267 # Initialization 00268 try: 00269 self._serial = serial.Serial() 00270 self._serial.port = port 00271 self._serial.baudrate = 115200 00272 self._serial.timeout = 0 00273 00274 # Creation failed 00275 except serial.SerialException as ex: 00276 raise utils.TransportError \ 00277 ("Serial Transport creation failed!\n" + str(ex)) 00278 00279 00280 @classmethod 00281 def autodetect(cls, **kwargs): 00282 ports = utils.list_serial_ports() 00283 logger.info("%s: Attempting autodetect with %s." % 00284 (cls.__name__, ' '.join(ports))) 00285 00286 for trynum in range(5): 00287 logger.info("%s: Autodetect try #%d." % 00288 (cls.__name__, trynum + 1)) 00289 for port in ports: 00290 kwargs['port'] = port 00291 transport = cls(**kwargs) 00292 try: 00293 transport.open() 00294 return transport 00295 except utils.TransportError: 00296 # Not this one, move on. 00297 pass 00298 00299 raise utils.TransportError("Unable to autodetect a serial Horizon device.") 00300 00301 00302 def __str__(self): 00303 """Return the transport device name.""" 00304 return self.port 00305 00306 00307 def open(self): 00308 if (not self._opened): 00309 logger.debug("%s: Beginning transport opening for %s." % 00310 (self.__class__.__name__, self._serial.portstr)) 00311 00312 try: 00313 self._serial.open() 00314 if not self._serial.isOpen(): 00315 raise serial.SerialException 00316 except serial.SerialException: 00317 logger.debug("%s: Transport opening failed." % self.__class__.__name__) 00318 raise utils.TransportError("Serial Port opening failed.") 00319 00320 self._opened = True 00321 self.receiver = self.Receiver(self._serial, self.store_timeout, self.receive_callback) 00322 self.receiver.start() 00323 time.sleep(0.1) 00324 00325 logger.debug("%s: Sending ping request." % (self.__class__.__name__)) 00326 message = messages.Message.request('echo') 00327 try: 00328 self.send_message(message) 00329 except utils.TransportError as ex: 00330 # Must catch this for the sake of closing the serial port and also 00331 # killing the receiver thread. 00332 self.close() 00333 raise utils.TransportError("Serial Port message send failed.\n" + str(ex)) 00334 00335 logger.debug("%s: Ping request sent." % (self.__class__.__name__)) 00336 00337 for sleeping in range(5): 00338 time.sleep(0.1) 00339 waiting = self.receiver.get_waiting() 00340 for message in waiting: 00341 logger.debug("%s: Message received." % (self.__class__.__name__)) 00342 if codes.names[message.code] == 'echo': 00343 # Success 00344 logger.debug("%s: Transport opened." % self.__class__.__name__) 00345 return 00346 00347 logger.debug("%s: No response to ping request." % self.__class__.__name__) 00348 self.close() 00349 raise utils.TransportError("Could not communicate with Clearpath platform.") 00350 00351 00352 def close(self): 00353 logger.debug("%s: Beginning transport closing for %s." % 00354 (self.__class__.__name__, self._serial.portstr)) 00355 self.receiver.stop() 00356 self.receiver.join() 00357 self._serial.close() 00358 self._opened = False 00359 logger.debug("%s: Transport closed." % self.__class__.__name__) 00360 00361 00362 def is_open(self): 00363 return self._opened; 00364 00365 00366 def send_message(self, message): 00367 """Serial Transport Device Send Horizon Message""" 00368 self.send_raw(utils.to_bytes(message.data())) 00369 00370 00371 def send_raw(self, raw): 00372 if not self._opened: 00373 raise utils.TransportError ("Cannot send while closed!") 00374 00375 try: 00376 with self.serial_write_lock: 00377 try: 00378 getattr(serial, "serial_for_url") 00379 sent = self._serial.write(raw) 00380 if sent == None: 00381 raise utils.TransportError ("Write Failed!") 00382 00383 except AttributeError: 00384 if sys.version_info[0] > 2: 00385 self._serial.write(list(map(chr, raw))) 00386 else: 00387 self._serial.write(raw) 00388 sent = len(raw) 00389 00390 if sent < len(raw): 00391 raise utils.TransportError ("Write Incomplete!") 00392 00393 # Send Failed 00394 except serial.SerialException as ex: 00395 raise utils.TransportError \ 00396 ("Serial Message send failed!\n" + str(ex)) 00397 00398 00399 class Receiver(threading.Thread): 00400 00401 def __init__(self, serial, store_timeout, callback): 00402 threading.Thread.__init__(self, name = 'clearpath.horizon.transports.Serial.Receiver') 00403 self._running = False 00404 self._buffer = [] 00405 self._serial = serial 00406 self._callback = callback 00407 self.start_time = time.time() 00408 self.store_timeout = store_timeout 00409 00410 # Received Messages 00411 self._acks_lock = threading.Lock() 00412 self._acks = {} # Format: {timestamp: (MessageRecord)} 00413 self._received_lock = threading.Lock() 00414 self._received = collections.deque() # Format: MessageRecord 00415 00416 # self.daemon = True 00417 00418 00419 def stop(self): 00420 self._running = False 00421 00422 00423 # Only for internal use by Receiver methods. These timestamps have nothing to 00424 # do with platform time or API time. 00425 def _timestamp(self): 00426 return math.floor((time.time() - self.start_time) * 1000) 00427 00428 00429 def get_waiting(self, code=0x0000): 00430 """Horizon Protocol Get Waiting Messages""" 00431 msgs = [] 00432 skip = collections.deque() 00433 with self._received_lock: 00434 try: 00435 while True: 00436 message_record = self._received.popleft() 00437 if code == 0 or message_record.message.code == code: 00438 msgs.append(message_record.message) 00439 else: 00440 skip.append(message_record) 00441 except IndexError: 00442 # No more items in the list 00443 pass 00444 00445 # All the ones we didn't return throw back in the received bin 00446 self._received = skip 00447 return msgs 00448 00449 00450 def run(self): 00451 logger.debug("%s: Entering receive loop for %s." % 00452 (self.__class__.__name__, self._serial.portstr)) 00453 self._running = True 00454 00455 while self._running: 00456 # Check for message 00457 try: 00458 message = None 00459 message = self._get_message() 00460 if message != None: 00461 logger.debug("%s: received message:\n %s" % \ 00462 (self.__class__.__name__, str(message))) 00463 except IOError as ex: # TransportError 00464 # Silently swallow 00465 logger.warning( 00466 "%s: IO error in attempting to retrieve message:\n%s" %\ 00467 (self.__class__.__name__, ex)) 00468 except ValueError as ex: # ChecksumError 00469 # Silently swallow 00470 logger.info( 00471 "%s: Value error in received message:\n%s" %\ 00472 (self.__class__.__name__, ex)) 00473 00474 # If it's a good message with a payload, handle it. 00475 if message != None and message.payload != None: 00476 if message.payload.__class__ == codes.payloads.Ack: 00477 # Mark the ack as received. It will be up to the main 00478 # thread to examine this for status and throw any necessary 00479 # exceptions. 00480 with self._acks_lock: 00481 # It's keyed to the message's baked-in timestamp, because that's how 00482 # acks are matched. But it's also stored beside the current time per 00483 # this computer, for the purposes of expiring it. 00484 self._acks[message.timestamp] = MessageRecord(message = message, 00485 expiry = self._timestamp() + 00486 self.store_timeout) 00487 00488 elif self._callback != None and self._callback(message): 00489 # Okay, handled by asyncronous handlers. 00490 pass 00491 00492 else: 00493 # Unhandled. Store it for synchronous access. 00494 with self._received_lock: 00495 self._received.append(MessageRecord(message = message, 00496 expiry = self._timestamp() + 00497 self.store_timeout)) 00498 00499 # Check timeouts 00500 current = self._timestamp() 00501 with self._received_lock: # stored messages 00502 try: 00503 while current > self._received[0].expiry: 00504 self._received.popleft() 00505 except IndexError: 00506 # No more items in list. 00507 pass 00508 with self._acks_lock: # stored acks 00509 for ts, message_record in list(self._acks.items()): 00510 if current > message_record.expiry: 00511 del self._acks[ts] 00512 00513 # Release Processor 00514 time.sleep(0.001) 00515 00516 logger.debug("%s: Exiting receive loop for %s." % 00517 (self.__class__.__name__, self._serial.portstr)) 00518 00519 def has_ack(self, timestamp): 00520 with self._acks_lock: 00521 if timestamp in self._acks: 00522 ack = self._acks[timestamp][0] 00523 del self._acks[timestamp] 00524 return ack 00525 else: 00526 return False 00527 00528 00529 def _get_message(self): 00530 read = 0 00531 00532 # read as much as possible without blocking (as timeout = 0) 00533 chars = self._serial.read(1000) 00534 00535 if len(chars) > 0: 00536 try: 00537 getattr(serial, "serial_for_url") 00538 if sys.version_info[0] > 2: 00539 self._buffer += chars 00540 else: 00541 self._buffer += list(map(ord,chars)) 00542 except AttributeError: 00543 self._buffer += list(map(ord,chars)) 00544 00545 # Discard bytes from the buffer until we find a 00546 # SOH character followed by two characters which are 00547 # complements, representing the length. 00548 disc = [] 00549 while(len(self._buffer) > 3 and ( 00550 self._buffer[0] != messages.Message.SOH or 00551 self._buffer[1] != 0xFF & (~self._buffer[2]) or 00552 self._buffer[1] == 0)): 00553 disc.append(self._buffer.pop(0)) 00554 00555 if len(disc) > 0: 00556 logger.info("%s: Discarded %d bytes:\n%s" % ( 00557 self.__class__.__name__, len(disc), 00558 ' '.join(map(utils.hex,disc)))) 00559 00560 if len(self._buffer) < 3: 00561 # Not enough data in the buffer read a SOH + LEN + ~LEN. 00562 return None 00563 00564 length = self._buffer[1] + 3 00565 00566 # Now know message length. Is there enough in the buffer yet to read 00567 # the whole message? If not, wait. 00568 if len(self._buffer) < length: 00569 return None 00570 00571 # Create and return the Message 00572 raw = self._buffer[0:length] 00573 self._buffer = self._buffer[length:] 00574 logger.info("%s: Message of %d bytes found:\n%s" % ( 00575 self.__class__.__name__, len(raw), 00576 ' '.join(map(utils.hex,raw)))) 00577 00578 # This will throw errors if has a bad checksum, for example, 00579 # which will bubble up to the run() function. 00580 return messages.Message.parse(raw) 00581 00582 00583 logger.debug("... clearpath.horizon.transports loaded.") 00584