bridge.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      @bridge.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 # ROS
00030 import rospy
00031 from novatel_msgs.msg import *
00032 from std_msgs.msg import String
00033 
00034 # Package modules
00035 from novatel_span_driver.data import DataPort
00036 from novatel_span_driver.monitor import Monitor
00037 
00038 # Standard
00039 import socket
00040 import struct
00041 from cStringIO import StringIO
00042 import time
00043 
00044 from novatel_span_driver import translator
00045 
00046 DEFAULT_IP = '198.161.73.9'
00047 DEFAULT_PORT = 3001
00048 
00049 SOCKET_TIMEOUT = 100.0
00050 socks = []
00051 ports = {}
00052 monitor = Monitor(ports)
00053 
00054 
00055 def init():
00056     ip = rospy.get_param('~ip', DEFAULT_IP)
00057     data_port = rospy.get_param('~port', DEFAULT_PORT)
00058 
00059     # Pass this parameter to use pcap data rather than a socket to a device.
00060     # For testing the node itself--to exercise downstream algorithms, use a bag.
00061     pcap_file_name = rospy.get_param('~pcap_file', False)
00062 
00063     if not pcap_file_name:
00064         sock = create_sock('data', ip, data_port)
00065     else:
00066         sock = create_test_sock(pcap_file_name)
00067 
00068     ports['data'] = DataPort(sock)
00069 
00070     configure_receiver(sock)
00071 
00072     for name, port in ports.items():
00073         port.start()
00074         rospy.loginfo("Port %s thread started." % name)
00075     monitor.start()
00076 
00077     rospy.on_shutdown(shutdown)
00078 
00079 
00080 def create_sock(name, ip, port):
00081     try:
00082         sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
00083         ip_port = (ip, port)
00084         sock.connect(ip_port)
00085         rospy.loginfo("Successfully connected to %%s port at %s:%d" % ip_port % name)
00086     except socket.error as e:
00087         rospy.logfatal("Couldn't connect to %%s port at %s:%d: %%s" % ip_port % (name, str(e)))
00088         exit(1)
00089     sock.settimeout(SOCKET_TIMEOUT)
00090     socks.append(sock)
00091     return sock
00092 
00093 
00094 def create_test_sock(pcap_filename):
00095     rospy.sleep(0.1)
00096 
00097     try:
00098         import pcapy
00099     except ImportError:
00100         import pure_pcapy as pcapy
00101 
00102     from StringIO import StringIO
00103     from impacket import ImpactDecoder
00104 
00105     body_list = []
00106     if pcap_filename.endswith("gz"):
00107         # From: http://projects.honeynet.org/honeysnap/changeset/35/trunk/honeysnap/__init__.py
00108         import tempfile
00109         import gzip
00110         tmph, tmpf = tempfile.mkstemp()
00111         tmph = open(tmpf, 'wb')
00112         gfile = gzip.open(pcap_filename)
00113         tmph.write(gfile.read())
00114         gfile.close()
00115         tmph.close()
00116         pcap_filename = tmpf
00117 
00118     cap = pcapy.open_offline(pcap_filename)
00119     decoder = ImpactDecoder.EthDecoder()
00120 
00121     while True:
00122         header, payload = cap.next()
00123         if not header:
00124             break
00125         try:
00126             tcp = decoder.decode(payload).child().child()
00127             body_list.append(tcp.child().get_packet())
00128         except AttributeError:
00129             print decoder.decode(payload)
00130             raise
00131 
00132     data_io = StringIO(''.join(body_list))
00133 
00134     class MockSocket(object):
00135         def recv(self, byte_count):
00136             rospy.sleep(0.002)
00137             data = data_io.read(byte_count)
00138             if data == "":
00139                 rospy.signal_shutdown("Test completed.")
00140             return data
00141 
00142         def settimeout(self, timeout):
00143             pass
00144 
00145     return MockSocket()
00146 
00147 
00148 def configure_receiver(port):
00149     receiver_config = rospy.get_param('~configuration', None)
00150 
00151     if receiver_config is not None:
00152         imu_connect = receiver_config.get('imu_connect', None)
00153         if imu_connect is not None:
00154             rospy.loginfo("Sending IMU connection string to SPAN system.")
00155             port.send('connectimu ' + imu_connect['port'] + ' ' + imu_connect['type'] + '\r\n')
00156 
00157         logger = receiver_config.get('log_request', [])
00158         rospy.loginfo("Enabling %i log outputs from SPAN system." % len(logger))
00159         for log in logger:
00160             port.send('log ' + log + ' ontime ' + str(logger[log]) + '\r\n')
00161 
00162         commands = receiver_config.get('command', [])
00163         rospy.loginfo("Sending %i user-specified initialization commands to SPAN system." % len(commands))
00164         for cmd in commands:
00165             port.send(cmd + ' ' + str(commands[cmd]) + '\r\n')
00166 
00167 
00168 def shutdown():
00169     monitor.finish.set()
00170     monitor.join()
00171     rospy.loginfo("Thread monitor finished.")
00172     for name, port in ports.items():
00173         port.finish.set()
00174         port.join()
00175         rospy.loginfo("Port %s thread finished." % name)
00176     for sock in socks:
00177         sock.shutdown(socket.SHUT_RDWR)
00178         sock.close()
00179     rospy.loginfo("Sockets closed.")


novatel_span_driver
Author(s): NovAtel Support
autogenerated on Thu Sep 28 2017 03:12:25