def log():
#SERIAL_PORT = '/dev/ttyUSB0'
SERIAL_PORT = 'COM3'
BAUD_RATE = 9600
# Instantiate an instance for the serial port
ser_port = serial.Serial(SERIAL_PORT, BAUD_RATE)
# Instantiate an instance of the ZigBee class
# and pass it an instance of the Serial class
xbee1 = xbee.ZigBee(ser_port)
print("initialising serial port listening...")
node1 = "bx00x13xa2x00Abx9cZ"
node2 = "bx00x13xa2x00Abx9cxb8"
timer = threading.Timer(15.0, timer_test)
while True:
data_samples = xbee1.wait_read_frame()
samples = data_samples['samples'][0]
#node_id = str(data_samples['source_addr_long'])
node_id = str(data_samples['source_addr_long'])
node_id = re.sub('[^A-Za-z0-9]+', '', node_id)
time = str(datetime.now())
time = time[11:-10]
vwc_600 = vwc_conversion(float(samples['adc-0']))
vwc_300 = vwc_conversion(float(samples['adc-1']))
temp_100 = temp_conversion(float(samples['adc-2']))
battery = battery_conversion(float(samples['adc-3']))
if(not timer.is_alive()):
readings = [0.0, 0.0, 0.0, 0.0, 0]
timer = threading.Timer(15.0, timer_test, args=[readings])
timer.start()
print('timer started')
print(str(datetime.now()))
if(node_id == node1):
readings[0] += vwc_600
readings[1] += vwc_300
readings[2] += temp_100
readings[3] += battery
readings[4] += 1
print(readings)
if(node_id == node2):
# charge[1] += battery
print("node2 not updating log")
def OpenPort():
try:
global db
global cur
cur = db.cursor()
global serial_port
serial_port = serial.Serial(ports, brate)
global conn
conn = xbee.ZigBee(serial_port)
except Exception as e:
print(e)
def __init__(self, port):
"""Start SensorNet."""
self._logger = logging.getLogger(__name__)
self.serial = serial.Serial(port, baudrate=19200)
self.config = yaml.safe_load(open(os.path.dirname(
os.path.realpath(__file__)) + '/' + 'config.yml'))
self.XB = xbee.ZigBee(self.serial,
shorthand=True,
callback=self.process_packet, error_callback=self.error_callback escaped=True)
self.units = {}
self.XB.send('at', command='ND'.encode('ascii'))
time.sleep(3)
def __init__(self, serial_port, baud_rate, dispatcher, serializer):
self.ser = serial.Serial(serial_port, baud_rate)
self.zb = xbee.ZigBee(self.ser, escaped=True)
self.dispatch = Dispatch(xbee=self.zb)
self.dispatch.register('rx', self.rx_handler,
lambda p: p['id'] == 'rx')
self.dispatch.register('rx_explicit', self.default_handler,
lambda p: p['id'] == 'rx_explicit')
self.dispatch.register('rx_io_data_long_addr', self.default_handler,
lambda p: p['id'] == 'rx_io_data_long_addr')
self.dispatch.register('tx_status', self.default_handler,
lambda p: p['id'] == 'tx_status')
self.dispatch.register('status', self.default_handler,
lambda p: p['id'] == 'status')
self.dispatch.register('at_response', self.at_response_handler,
lambda p: p['id'] == 'at_response')
self.dispatch.register('remote_at_response', self.default_handler,
lambda p: p['id'] == 'remote_at_response')
self.dispatch.register('node_id_indicator', self.default_handler,
lambda p: p['id'] == 'node_id_indicator')
self.dispatcher = dispatcher
self.serializer = serializer
def Connect(self, deviceId):
if self._getConnected():
raise XBee_ZigBee_Exception("Module is already connected.")
self._serial = serial.Serial(self._getSocketDev(self._socket),
self._setup[BAUDRATE])
self._module = xbee.ZigBee(self._serial,
escaped=self._setup[APIMODE2],
callback=self.async_data_callback)
writeChanges = False
for option in self._setup[ATCMDS]:
cmd = list(option.keys())[0]
param = list(option.values())[0]
cmdEnc = cmd.encode("UTF-8")
if (cmdEnc == CMDWRITE):
writeChanges = True
break
paramEnc = b"\x00"
blen = (param.bit_length() + 7) // 8
if blen != 0:
paramEnc = param.to_bytes(blen, byteorder="big")
self._module.send("at",
frame_id=b"R",
command=cmdEnc,
parameter=paramEnc)
rx = self._module.wait_read_frame()
if not rx["status"]:
raise XBee_ZigBee_Exception(
"Did not receive response from AT command")
if rx["status"] != b"\x00":
raise XBee_ZigBee_Exception(
"Wrong AT command/parameter ({}/{})".format(cmd, param))
if writeChanges:
self._module.send("at", frame_id=b"R", command=CMDWRITE)
rx = self._module.wait_read_frame()
if not rx["status"]:
raise XBee_ZigBee_Exception(
"Did not receive response from AT command")
self._setConnected(True)
def talker():
#XBee config
ser = serial.Serial("/dev/ttyUSB0", 9600)
ser.write('\n')
ser.write('b\n')
ser.close()
ser = serial.Serial("/dev/ttyUSB0", 115200)
sourceaddrlist = {} # node set dictionary
pubMarker = {} # publisher dictionary
pubText = {} # publisher dictionary
xbradio1 = xbee.ZigBee(ser) # the DAQ radio
rospy.init_node('RelocNode')
pubIRdata1 = rospy.Publisher('/IRdebug/point1', Point)
pubIRdata2 = rospy.Publisher('/IRdebug/point2', Point)
pubIRdata3 = rospy.Publisher('/IRdebug/point3', Point)
pubIRdata4 = rospy.Publisher('/IRdebug/point4', Point)
pubUSdata = rospy.Publisher('/USdebug', Quaternion)
br = tf.TransformBroadcaster()
robId = "??"
IRx1 = 0
IRy1 = 0
IRx2 = 0
IRy2 = 0
IRx3 = 0
IRy3 = 0
IRx4 = 0
IRy4 = 0
IRang = 0
US1 = 0
US2 = 0
US3 = 0
US4 = 0
while not rospy.is_shutdown():
response = xbradio1.wait_read_frame() #response is a python dictionary
#sorce address
name = binascii.hexlify(response['source_addr_long'])
sourceaddr = name.decode("utf-8")
# check if its a new source address and initialize publishers
nodeseen = 0
for j in range(len(sourceaddrlist)):
if sourceaddrlist[j + 1] == sourceaddr:
nodeseen = 1
Rfdata = response['rf_data'].decode("utf-8")
str2 = Rfdata.split(',')
#packet data extraction
range_flag = 0
bearing_flag = 0
packetValid = 0
try:
for i in range(len(str2)):
if str2[i][0:2] == "RL":
robID = str2[i][2:]
if str2[i] == "IR":
IRx1 = int(str2[i + 1])
IRy1 = int(str2[i + 2])
IRx2 = int(str2[i + 3])
IRy2 = int(str2[i + 4])
IRx3 = int(str2[i + 5])
IRy3 = int(str2[i + 6])
IRx4 = int(str2[i + 7])
IRy4 = int(str2[i + 8])
bearing_flag = 1
if str2[i] == "ANG":
IRang = int(
str2[i + 1][0:len(str2[i + 1]) - 2]) - 90
#print IRang/180.0*math.pi
br.sendTransform(
(0, 0, 0.3),
tf.transformations.quaternion_from_euler(
0, 0, IRang / 180.0 * math.pi),
rospy.Time.now(), "/pioneer1/IRcam",
"/pioneer1/base_link")
if str2[i] == "US":
US1 = float(str2[i + 1])
US2 = float(str2[i + 2])
US3 = float(str2[i + 3])
US4 = float(str2[i + 4])
range_flag = 1
packetValid = 1
except:
#print "Bad Packet\n"
packetValid = 0
#Measurement processing
if packetValid == 1:
slog = "No Data"
USmin = 0
if range_flag == 1 and bearing_flag == 0:
IRx = 0
IRy = 0
USmin = min(US1, US2, US3, US4) / 1000.0
fc = 1380
slog = "Range only"
if USmin == 10725:
slog = "No Data"
USmin = 0
if range_flag == 0 and bearing_flag == 1:
IRx = IRx1 - 1024 / 2
IRy = -IRy1 + 768 / 2
USmin = 10
fc = 1380
slog = "Bearing only"
if range_flag == 1 and bearing_flag == 1:
IRx = IRx1 - 1024 / 2
IRy = -IRy1 + 768 / 2
USmin = min(US1, US2, US3, US4) / 1000.0
fc = 1380
slog = "Range Bearing"
#print USmin
rpx = math.sqrt(pow(IRx, 2) + pow(IRy, 2) + pow(fc, 2))
robx = fc / rpx * USmin
roby = IRx / rpx * USmin
robz = IRy / rpx * USmin
#Data publish
text_marker = Marker()
text_marker.header.frame_id = "/pioneer1/base_link"
text_marker.type = Marker.TEXT_VIEW_FACING
text_marker.text = "RobID : %s\nx : %f\ny : %f\nz : %f\ntheta :\n %s" % (
robID, robx, roby, robz, slog)
text_marker.pose.position.x = robx
text_marker.pose.position.y = roby
text_marker.pose.position.z = robz
text_marker.scale.z = 0.1
text_marker.color.r = 0.0
text_marker.color.g = 0.5
text_marker.color.b = 0.5
text_marker.color.a = 1
arrow_marker = Marker()
arrow_marker.header.frame_id = "/pioneer1/base_link"
arrow_marker.type = Marker.ARROW
arrow_marker.points = {
Point(0, 0, 0),
Point(robx, roby, robz)
}
arrow_marker.scale.x = 0.1
arrow_marker.scale.y = 0.1
arrow_marker.scale.z = 0.2
arrow_marker.color.r = 0.0
arrow_marker.color.g = 0.5
arrow_marker.color.b = 0.5
arrow_marker.color.a = 0.5
IRdebug1 = Point()
IRdebug1.x = IRx1
IRdebug1.y = IRy1
IRdebug2 = Point()
IRdebug2.x = IRx2
IRdebug2.y = IRy2
IRdebug3 = Point()
IRdebug3.x = IRx3
IRdebug3.y = IRy3
IRdebug4 = Point()
IRdebug4.x = IRx4
IRdebug4.y = IRy4
USdebug = Quaternion()
USdebug.x = US1
USdebug.y = US2
USdebug.z = US3
USdebug.w = US4
pubIRdata1.publish(IRdebug1)
pubIRdata2.publish(IRdebug2)
pubIRdata3.publish(IRdebug3)
pubIRdata4.publish(IRdebug4)
pubUSdata.publish(USdebug)
pubText[j + 1].publish(text_marker)
pubMarker[j + 1].publish(arrow_marker)
if str2[0][0:5] == "RL002":
print str2
#print str2
if nodeseen == 0: # New nodes handling
sourceaddrlist[len(sourceaddrlist) + 1] = sourceaddr
pubText[len(pubText) + 1] = rospy.Publisher(
'/relocNode' + str(len(pubText) + 1) + '/relocVizDataText',
Marker)
pubMarker[len(pubMarker) + 1] = rospy.Publisher(
'/relocNode' + str(len(pubMarker) + 1) + '/relocVizData',
Marker)
print "New node registered:" + sourceaddr
#publish data of new node
Rfdata = response['rf_data'].decode("utf-8")
#print(sourceaddr+' :'+Rfdata)
#Publish all topics
rospy.sleep(0.01)
def connection_made(self, transport):
self.transport = transport
self.init_frame()
self.zigbee = xbee.ZigBee(ser=None)
Log.info(f'port opened {transport}')
transport.serial.rts = False # You can manipulate Serial object via transport
