xbee_network.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2011, Willow Garage, Inc.
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of Willow Garage, Inc. nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 
00034 __author__ = "astambler@willowgarage.com (Adam Stambler)"
00035 
00036 
00037 import roslib; roslib.load_manifest('rosserial_xbee')
00038 roslib.load_manifest('rosserial_xbee')
00039 
00040 from xbee import XBee
00041 import serial
00042 
00043 from rosserial_python import SerialClient
00044 import rospy
00045 
00046 import threading
00047 import sys
00048 import time
00049 import struct
00050 
00051 client_ports=  {}
00052 clients = {}
00053 
00054 debug = False;
00055 
00056 import threading
00057 
00058 
00059 class FakeSerial():
00060         def __init__(self, id, xbee):
00061                 self.rxdata =''
00062                 self.xbee  = xbee
00063                 self.id = id
00064                 self.lock = threading.Lock()
00065                 self.timeout = 0.1
00066                 
00067         def read(self, size = 1):
00068                 t= 0
00069                 counts = self.timeout/0.01
00070                 #print "s%d   %s"%(size, self.rxdata)
00071                 while( ( len(self.rxdata) < size ) and  (not rospy.is_shutdown()) ):
00072                         time.sleep(0.01)
00073                         t = t +1
00074                         if (t >  counts):
00075                                 return ''
00076                         
00077                 with (self.lock):
00078                         out = self.rxdata[:size]
00079                         self.rxdata = self.rxdata[size:]
00080                         
00081                 #print "fake out " , out
00082                 return out
00083                 
00084         def write(self, data):
00085                 if (debug):
00086                         print "Sending ", [d for d in data]
00087                 self.xbee.send('tx', frame_id='0', options="\x01", dest_addr=self.id,data=data)
00088                 
00089         def putData(self, data):
00090                 with (self.lock):
00091                         self.rxdata = self.rxdata+data
00092         
00093         def flushInput(self):
00094                 self.rxdata = ''
00095                 
00096 
00097 if __name__== '__main__':
00098         print "RosSerial Xbee Network"
00099         rospy.init_node('xbee_network')
00100         sys.argv= rospy.myargv(argv=sys.argv) 
00101         
00102         xbee_port = ''
00103         network_ids = [] 
00104         
00105         if len(sys.argv) <3 :
00106                 print """
00107 This program connects to rosserial xbee nodes.  The program must be called
00108 like :
00109 
00110 ./xbee_network.py <xbee_serial_port> ID1 [ ID2 ID3 ....] 
00111 """
00112                 exit()
00113         else :
00114                 xbee_port = sys.argv[1]         
00115                 network_ids  = [ struct.pack('>h', int(id) ) for id in sys.argv[2:] ]
00116         
00117         print "Contacting Xbees : " , network_ids
00118 
00119                 
00120         # Open serial port
00121         ser = serial.Serial(xbee_port, 57600, timeout=0.01)
00122         ser.flush()
00123         ser.flushInput()
00124         ser.flushOutput()
00125         time.sleep(1)
00126         # Create API object
00127         xbee = XBee(ser, escaped= True)
00128         
00129         for xid in network_ids:
00130                 client_ports[xid] = FakeSerial(xid, xbee)
00131                 time.sleep(.2)
00132                 clients[xid] = SerialClient(client_ports[xid])
00133                  
00134         threads = [ threading.Thread(target=c.run) for c in clients.values()]
00135         
00136         for t in threads:
00137                 t.deamon =True 
00138                 t.start()
00139 
00140         while not rospy.is_shutdown():
00141                 try:
00142                         msg = xbee.wait_read_frame()
00143                         if (debug):
00144                                 print "Received " , msg
00145         
00146                         if  msg['id'] == 'rx':
00147                                 src = msg['source_addr']
00148                                 data = msg['rf_data']
00149                                 try:
00150                                         client_ports[src].putData(data)
00151                                 except KeyError as e:
00152                                         print "Rcv ID corrupted"
00153                 except KeyboardInterrupt:
00154                         break
00155         ser.close()
00156         
00157         print "Quiting the Sensor Network"
00158 
00159 


rosserial_xbee
Author(s): Adam Stambler
autogenerated on Fri Dec 6 2013 20:35:48