xbee_network.py
Go to the documentation of this file.
1 #! /usr/bin/env python
2 # Software License Agreement (BSD License)
3 #
4 # Copyright (c) 2011, Willow Garage, Inc.
5 # All rights reserved.
6 #
7 # Redistribution and use in source and binary forms, with or without
8 # modification, are permitted provided that the following conditions
9 # are met:
10 #
11 # * Redistributions of source code must retain the above copyright
12 # notice, this list of conditions and the following disclaimer.
13 # * Redistributions in binary form must reproduce the above
14 # copyright notice, this list of conditions and the following
15 # disclaimer in the documentation and/or other materials provided
16 # with the distribution.
17 # * Neither the name of Willow Garage, Inc. nor the names of its
18 # contributors may be used to endorse or promote products derived
19 # from this software without specific prior written permission.
20 #
21 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 # POSSIBILITY OF SUCH DAMAGE.
33 
34 __author__ = "astambler@willowgarage.com (Adam Stambler)"
35 
36 from xbee import XBee
37 import serial
38 
39 from rosserial_python import SerialClient
40 import rospy
41 
42 import threading
43 import sys
44 import time
45 import struct
46 
47 client_ports= {}
48 clients = {}
49 
50 debug = False;
51 
52 import threading
53 
54 
55 class FakeSerial():
56  def __init__(self, id, xbee):
57  self.rxdata =''
58  self.xbee = xbee
59  self.id = id
60  self.lock = threading.Lock()
61  self.timeout = 0.1
62 
63  def read(self, size = 1):
64  t= 0
65  counts = self.timeout/0.01
66  #print "s%d %s"%(size, self.rxdata)
67  while( ( len(self.rxdata) < size ) and (not rospy.is_shutdown()) ):
68  time.sleep(0.01)
69  t = t +1
70  if (t > counts):
71  return ''
72 
73  with (self.lock):
74  out = self.rxdata[:size]
75  self.rxdata = self.rxdata[size:]
76 
77  #print "fake out " , out
78  return out
79 
80  def write(self, data):
81  if (debug):
82  print "Sending ", [d for d in data]
83  self.xbee.send('tx', frame_id='0', options="\x01", dest_addr=self.id,data=data)
84 
85  def putData(self, data):
86  with (self.lock):
87  self.rxdata = self.rxdata+data
88 
89  def flushInput(self):
90  self.rxdata = ''
91 
92  # Returns the number of bytes available to be read
93  def inWaiting(self):
94  return len(self.rxdata)
95 
96 if __name__== '__main__':
97  print "RosSerial Xbee Network"
98  rospy.init_node('xbee_network')
99  sys.argv= rospy.myargv(argv=sys.argv)
100 
101  xbee_port = ''
102  network_ids = []
103 
104  if len(sys.argv) <3 :
105  print """
106 This program connects to rosserial xbee nodes. The program must be called
107 like :
108 
109 ./xbee_network.py <xbee_serial_port> ID1 [ ID2 ID3 ....]
110 """
111  exit()
112  else :
113  xbee_port = sys.argv[1]
114  network_ids = [ struct.pack('>h', int(id) ) for id in sys.argv[2:] ]
115 
116  print "Contacting Xbees : " , network_ids
117 
118 
119  # Open serial port
120  ser = serial.Serial(xbee_port, 57600, timeout=0.01)
121  ser.flush()
122  ser.flushInput()
123  ser.flushOutput()
124  time.sleep(1)
125  # Create API object
126  xbee = XBee(ser, escaped= True)
127 
128  for xid in network_ids:
129  client_ports[xid] = FakeSerial(xid, xbee)
130  time.sleep(.2)
131  clients[xid] = SerialClient(client_ports[xid])
132 
133  threads = [ threading.Thread(target=c.run) for c in clients.values()]
134 
135  for t in threads:
136  t.deamon =True
137  t.start()
138 
139  while not rospy.is_shutdown():
140  try:
141  msg = xbee.wait_read_frame()
142  if (debug):
143  print "Received " , msg
144 
145  if msg['id'] == 'rx':
146  src = msg['source_addr']
147  data = msg['rf_data']
148  try:
149  client_ports[src].putData(data)
150  except KeyError as e:
151  print "Rcv ID corrupted"
152  except KeyboardInterrupt:
153  break
154  ser.close()
155 
156  print "Quiting the Sensor Network"
157 
158 
def read(self, size=1)
Definition: xbee_network.py:63
def write(self, data)
Definition: xbee_network.py:80
def putData(self, data)
Definition: xbee_network.py:85
def __init__(self, id, xbee)
Definition: xbee_network.py:56


rosserial_xbee
Author(s): Adam Stambler
autogenerated on Fri Jun 7 2019 22:03:11