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 from xbee import XBee
00037 import serial
00038 
00039 from rosserial_python import SerialClient
00040 import rospy
00041 
00042 import threading
00043 import sys
00044 import time
00045 import struct
00046 
00047 client_ports=  {}
00048 clients = {}
00049 
00050 debug = False;
00051 
00052 import threading
00053 
00054 
00055 class FakeSerial():
00056         def __init__(self, id, xbee):
00057                 self.rxdata =''
00058                 self.xbee  = xbee
00059                 self.id = id
00060                 self.lock = threading.Lock()
00061                 self.timeout = 0.1
00062                 
00063         def read(self, size = 1):
00064                 t= 0
00065                 counts = self.timeout/0.01
00066                 #print "s%d   %s"%(size, self.rxdata)
00067                 while( ( len(self.rxdata) < size ) and  (not rospy.is_shutdown()) ):
00068                         time.sleep(0.01)
00069                         t = t +1
00070                         if (t >  counts):
00071                                 return ''
00072                         
00073                 with (self.lock):
00074                         out = self.rxdata[:size]
00075                         self.rxdata = self.rxdata[size:]
00076                         
00077                 #print "fake out " , out
00078                 return out
00079                 
00080         def write(self, data):
00081                 if (debug):
00082                         print "Sending ", [d for d in data]
00083                 self.xbee.send('tx', frame_id='0', options="\x01", dest_addr=self.id,data=data)
00084                 
00085         def putData(self, data):
00086                 with (self.lock):
00087                         self.rxdata = self.rxdata+data
00088         
00089         def flushInput(self):
00090                 self.rxdata = ''
00091                 
00092 
00093 if __name__== '__main__':
00094         print "RosSerial Xbee Network"
00095         rospy.init_node('xbee_network')
00096         sys.argv= rospy.myargv(argv=sys.argv) 
00097         
00098         xbee_port = ''
00099         network_ids = [] 
00100         
00101         if len(sys.argv) <3 :
00102                 print """
00103 This program connects to rosserial xbee nodes.  The program must be called
00104 like :
00105 
00106 ./xbee_network.py <xbee_serial_port> ID1 [ ID2 ID3 ....] 
00107 """
00108                 exit()
00109         else :
00110                 xbee_port = sys.argv[1]         
00111                 network_ids  = [ struct.pack('>h', int(id) ) for id in sys.argv[2:] ]
00112         
00113         print "Contacting Xbees : " , network_ids
00114 
00115                 
00116         # Open serial port
00117         ser = serial.Serial(xbee_port, 57600, timeout=0.01)
00118         ser.flush()
00119         ser.flushInput()
00120         ser.flushOutput()
00121         time.sleep(1)
00122         # Create API object
00123         xbee = XBee(ser, escaped= True)
00124         
00125         for xid in network_ids:
00126                 client_ports[xid] = FakeSerial(xid, xbee)
00127                 time.sleep(.2)
00128                 clients[xid] = SerialClient(client_ports[xid])
00129                  
00130         threads = [ threading.Thread(target=c.run) for c in clients.values()]
00131         
00132         for t in threads:
00133                 t.deamon =True 
00134                 t.start()
00135 
00136         while not rospy.is_shutdown():
00137                 try:
00138                         msg = xbee.wait_read_frame()
00139                         if (debug):
00140                                 print "Received " , msg
00141         
00142                         if  msg['id'] == 'rx':
00143                                 src = msg['source_addr']
00144                                 data = msg['rf_data']
00145                                 try:
00146                                         client_ports[src].putData(data)
00147                                 except KeyError as e:
00148                                         print "Rcv ID corrupted"
00149                 except KeyboardInterrupt:
00150                         break
00151         ser.close()
00152         
00153         print "Quiting the Sensor Network"
00154 
00155 


rosserial_xbee
Author(s): Adam Stambler
autogenerated on Fri Aug 28 2015 12:44:47