def __init__(self):
rospy.init_node('decawave_driver')
#Init DecaWave port
dwPort = rospy.get_param('~port','/dev/ttyUSB0')
dwRate = rospy.get_param('~baud',9600)
dwID = rospy.get_param('~id','t0')
dwPub = rospy.Publisher('dw/' + dwID + '/data', DecaWaveMsg, queue_size = 5)
try:
ser = serial.Serial(
port = dwPort,
timeout = 10,
baudrate = dwRate
)
dwMsg = DecaWaveMsg()
dwMsg.header.frame_id = "base_decawave_" + dwID
ser.close()
ser.open()
d0 = 0
d1 = 0
d2 = 0
d3 = 0
while not rospy.is_shutdown():
# Check to make sure the bitstream is at the beginning of a message
match = '\x6D' # x6D is the ascii character 'm'
sync = ser.read()
if sync != match:
print "Sync didn't match."
continue
else:
print "Sync matched. Starting Read."
i = 0
while i <= 2:
if i != 0:
sync = ser.read() # read the m from the beginning of the message
raw_data = ser.readline()
if ((i == 0) and (raw_data[0:3] != b'a00')):
print "Advance until at a00."
break
if raw_data[0:3] == b'a00':
d0 = int(raw_data[8:16], 16) / 1000.00
elif raw_data[0:3] == b'a01':
d1 = int(raw_data[8:16], 16) / 1000.00
elif raw_data[0:3] == b'a02':
d2 = int(raw_data[8:16], 16) / 1000.00
elif raw_data[0:3] == b'a03':
d3 = int(raw_data[8:16], 16) / 1000.00
i+=1
dwMsg.dist = (d0, d1, d2, d3)
dwMsg.header.stamp = rospy.get_rostime()
dwPub.publish(dwMsg)
rospy.loginfo(dwMsg.dist)
ser.close()
except SerialException:
print("Could not connect to the serial port")
def __init__(self):
rospy.init_node('decawave_driver')
# Init DecaWave port
self.dwPort = rospy.get_param('~port', 'dev/ttyACM0')
self.dwRate = rospy.get_param('~baud', 115200)
self.dwID = rospy.get_param('~id', 'a0')
self.anchor_angle = rospy.get_param('~anchor_angle', 0)
self.tag_angle = rospy.get_param('~tag_angle', 0)
self.beacon_distance = rospy.get_param('~beacon_distance', 0)
self.dwPub = rospy.Publisher('/ranger_finder/data', DecaWaveMsg, queue_size=5)
#filepath = str(rospkg.RosPack().get_path('decawave_driver')) + '/src/ranging_tests/range.csv'
#fh = open(filepath, "w")
self.last_time = rospy.rostime.Time(0,0)
try:
ser = serial.Serial(
port=self.dwPort,
timeout=10,
baudrate=self.dwRate
)
dwMsg = DecaWaveMsg()
dwMsg.header.frame_id = "base_decawave_" + self.dwID
ser.close()
ser.open()
range0 = 0
i=1
while (not rospy.is_shutdown()):
raw_data = ser.readline()
if raw_data == serial.to_bytes([]):
print "serial timeout"
else:
data = raw_data.split(' ')
if data[0] == 'mc':
dwMsg.header.stamp = rospy.Time.now()
#now = dwMsg.header.stamp
rospy.loginfo("UW time %f", rospy.get_time())
mask = int(data[1], 16)
if (mask & 0x01):
#global range0
range0 = int(data[2], 16) / 1000.0
if range0 != 0:
print range0
dwMsg.dist = range0
self.dwPub.publish(dwMsg) # publish the topic
#i=i+1
else:
print "range0 bad"
ser.close()
#fh.close()
except SerialException:
print("Could not connect to the serial port")
def __init__(self):
rospy.init_node('decawave_driver')
#Init DecaWave port
self.dwPort = rospy.get_param('~port', 'dev/ttyACM0')
self.dwRate = rospy.get_param('~baud', 115200)
self.dwID = rospy.get_param('~id', 'a0')
self.anchor_angle = rospy.get_param('~anchor_angle', 0)
self.tag_angle = rospy.get_param('~tag_angle', 0)
self.beacon_distance = rospy.get_param('~beacon_distance', 0)
self.dwPub = rospy.Publisher('/ranger_finder/data',
DecaWaveMsg,
queue_size=5)
try:
ser = serial.Serial(port=self.dwPort,
timeout=10,
baudrate=self.dwRate)
dwMsg = DecaWaveMsg()
dwMsg.header.frame_id = "base_decawave_" + self.dwID
ser.close()
ser.open()
range0 = 0
loop_rate = rospy.Rate(3.33)
time_array = np.array([])
while (not rospy.is_shutdown()):
raw_data = ser.readline()
if raw_data == serial.to_bytes([]):
print "serial timeout"
else:
data = raw_data.split(' ')
if data[0] == 'mc':
dwMsg.header.stamp = rospy.Time.now()
now = dwMsg.header.stamp
rospy.loginfo("UW time %f", rospy.get_time())
mask = int(data[1], 16)
if (mask & 0x01):
global range0
range0 = int(data[2], 16) / 1000.0
if range0 != 0:
print range0
dwMsg.dist = range0
self.dwPub.publish(dwMsg) #publish the topic
else:
print "range0 bad"
ser.close()
file.flush()
file.close()
except SerialException:
print("Could not connect to the serial port")
def __init__(self):
rospy.init_node('decawave_driver')
#Init DecaWave port
dwPort = rospy.get_param('~port', '/dev/ttyUSB0')
dwRate = rospy.get_param('~baud', 9600)
dwID = rospy.get_param('~id', 't0')
dwPub = rospy.Publisher('dw/' + dwID + '/data',
DecaWaveMsg,
queue_size=5)
anchor_angle = rospy.get_param('~anchor_angle', 0)
tag_angle = rospy.get_param('~tag_angle', 0)
beacon_distance = rospy.get_param('~beacon_distance', 0)
try:
ser = serial.Serial(port=dwPort, timeout=10, baudrate=dwRate)
dwMsg = DecaWaveMsg()
dwMsg.header.frame_id = "base_decawave_" + dwID
ser.close()
ser.open()
# initialize range values in case a measurement is dropped the first time
range0 = 0
# create a file location to store data
filename = '/home/snowmower/d' + str(
Decimal(beacon_distance).quantize(Decimal('1'))) + 'm_t' + str(
Decimal(tag_angle).quantize(Decimal('1'))) + 'd_a' + str(
Decimal(anchor_angle).quantize(Decimal('1'))) + 'd.csv'
# erase all the data in that file
f = open(filename, 'w').close()
# then open it again to append to it.
f = open(filename, 'a')
# Create an empty numpy array to store data (for mean and std)
data_array = np.array([])
# Take 100 good data points and store them in the file
i = 0
while (i < 300):
raw_data = ser.readline()
if raw_data == serial.to_bytes([]):
print "serial timeout"
else:
data = raw_data.split()
if data[0] == 'mc':
#print "read data as a list:"
#print data
mask = int(data[1], 16)
if (mask & 0x01):
#print "range0 good"
range0 = int(data[2], 16) / 1000.0
print range0
# write to file
f.write(str(range0) + '\n')
# add data to the array
data_array = np.append(data_array, range0)
# increase counter for while loop
i = i + 1
else:
print "range0 bad"
ser.close()
f.flush()
f.close()
print('mean = ' + str(np.mean(data_array)))
print('std = ' + str(np.std(data_array)))
except SerialException:
print("Could not connect to the serial port")
def __init__(self):
rospy.init_node('decawave_driver')
#Init DecaWave port
dwPort = rospy.get_param('~port','/dev/ttyUSB0')
dwRate = rospy.get_param('~baud',9600)
dwID = rospy.get_param('~id','t0')
dwPub = rospy.Publisher('dw/' + dwID + '/data', DecaWaveMsg, queue_size = 5)
try:
ser = serial.Serial(
port = dwPort,
timeout = 10,
baudrate = dwRate
)
dwMsg = DecaWaveMsg()
dwMsg.header.frame_id = "base_decawave_" + dwID
ser.close()
ser.open()
# initialize range values in case a measurement is dropped the first time
range0 = 0
range1 = 0
range2 = 0
range3 = 0
while not rospy.is_shutdown():
raw_data = ser.readline()
if raw_data == serial.to_bytes([]):
print "serial timeout"
else:
data = raw_data.split()
if data[0] == 'mc':
#print "read data as a list:"
#print data
mask = int(data[1],16)
if (mask & 0x01):
#print "range0 good"
range0 = int(data[2],16)/1000.0
else:
print "range0 bad"
# range0 = -1
if (mask & 0x02):
#print "range1 good"
range1 = int(data[3],16)/1000.0
else:
print "range1 bad"
# range1 = -1
if (mask & 0x04):
#print "range2 good"
range2 = int(data[4],16)/1000.0
else:
print "range2 bad"
# range2 = -1
if (mask & 0x08):
#print "range3 good"
range3 = int(data[5],16)/1000.0
else:
print "range3 bad"
# range3 = -1
dwMsg.dist = (range0, range1, range2, range3)
dwMsg.header.stamp = rospy.get_rostime()
dwPub.publish(dwMsg)
rospy.loginfo(dwMsg.dist)
ser.close()
except SerialException:
print("Could not connect to the serial port")
