bridge.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 # ROS
00044 import rospy
00045 import applanix_msgs.msg
00046 
00047 # Package modules
00048 from data import DataPort
00049 from control import ControlPort
00050 from monitor import Monitor
00051 
00052 # Standard
00053 import socket
00054 import struct
00055 from cStringIO import StringIO
00056 import time
00057 
00058 import translator
00059 from handlers import NullHandler, GroupHandler, MessageHandler, AckHandler
00060 
00061 PORTS_DATA = {
00062     "realtime": 5602,
00063     "logging": 5603
00064     }
00065 PORT_CONTROL = 5601
00066 
00067 DEFAULT_IP = '192.168.53.100'
00068 PREFIX_DEFAULTS = {
00069     "raw": True,
00070     "dmi": True,
00071     "status": True,
00072     "events": True
00073     }
00074 
00075 SOCKET_TIMEOUT=10.0
00076 socks = []
00077 ports = {}
00078 monitor = Monitor(ports)
00079 
00080 
00081 def main():
00082   rospy.init_node('applanix_bridge')
00083 
00084   # Where to find the device. It does not support DHCP, so you'll need
00085   # to connect initially to the factory default IP in order to change it.
00086   ip = rospy.get_param('ip', DEFAULT_IP)
00087 
00088   # Select between realtime and logging data ports. The logging data is
00089   # optimized for completeness, whereas the realtime data is optimized for
00090   # freshness.
00091   data_port = PORTS_DATA[rospy.get_param('data', "realtime")]
00092 
00093   # Disable this to not connect to the control socket (for example, if you
00094   # want to control the device using the Applanix POS-LV software rather
00095   # than ROS-based services.
00096   control_enabled = rospy.get_param('control', True)
00097 
00098   # Disable any of these to hide auxiliary topics which you may not be
00099   # interested in.
00100   exclude_prefixes = []
00101   for prefix, default in PREFIX_DEFAULTS.items():
00102     if not rospy.get_param('include_%s' % prefix, default):
00103       exclude_prefixes.append(prefix)
00104 
00105   # Pass this parameter to use pcap data rather than a socket to a device.
00106   # For testing the node itself--to exercise downstream algorithms, use a bag.
00107   pcap_file_name = rospy.get_param('pcap_file', False)
00108 
00109   if not pcap_file_name:
00110     sock = create_sock('data', ip, data_port)
00111   else:
00112     sock = create_test_sock(pcap_file_name)
00113 
00114   ports['data'] = DataPort(sock, exclude_prefixes=exclude_prefixes)
00115 
00116   if control_enabled:
00117     ports['control'] = ControlPort(create_sock('control', ip, PORT_CONTROL))
00118 
00119   for name, port in ports.items():
00120     port.start()
00121     rospy.loginfo("Port %s thread started." % name)
00122   monitor.start()
00123 
00124   rospy.on_shutdown(shutdown)
00125   rospy.spin()
00126 
00127 
00128 def create_sock(name, ip, port):
00129   try:
00130     sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
00131     ip_port = (ip, port)
00132     sock.connect(ip_port)
00133     rospy.loginfo("Successfully connected to %%s port at %s:%d" % ip_port % name)
00134   except socket.error as e:
00135     rospy.logfatal("Couldn't connect to %%s port at %s:%d: %%s" % ip_port % (name, str(e)))
00136     exit(1)
00137   socks.append(sock)
00138   return sock
00139 
00140 
00141 def create_test_sock(pcap_filename):
00142   rospy.sleep(0.1)
00143 
00144   import pcapy
00145   from StringIO import StringIO
00146   from impacket import ImpactDecoder
00147 
00148   body_list = []
00149   cap = pcapy.open_offline(pcap_filename)
00150   decoder = ImpactDecoder.EthDecoder()
00151 
00152   while True:
00153     header, payload = cap.next()
00154     if not header: break
00155     udp = decoder.decode(payload).child().child()
00156     body_list.append(udp.child().get_packet())
00157 
00158   data_io = StringIO(''.join(body_list))
00159 
00160   class MockSocket(object):
00161     def recv(self, byte_count):
00162       rospy.sleep(0.0001)
00163       data = data_io.read(byte_count)
00164       if data == "":
00165         rospy.signal_shutdown("Test completed.")
00166       return data  
00167     def settimeout(self, timeout):
00168       pass
00169 
00170   return MockSocket()
00171 
00172 
00173 def shutdown():
00174   monitor.finish.set()
00175   monitor.join()
00176   rospy.loginfo("Thread monitor finished.") 
00177   for name, port in ports.items():
00178     port.finish.set()
00179     port.join()
00180     rospy.loginfo("Port %s thread finished." % name) 
00181   for sock in socks:
00182     sock.shutdown(socket.SHUT_RDWR)
00183     sock.close()
00184   rospy.loginfo("Sockets closed.") 


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