def data_handler(self, ctx, data):
newData = str(parse_value(data)).strip('{}').replace(',', '').split()
self.sensor_data.append([data.contents.epoch, 0, float(newData[2]), float(newData[5]), float(newData[8])])
# print(newData)
# f.write('%.3f,%.3f,%.3f\n' % (float(newData[2]), float(newData[5]), float(newData[8])))
print("%s -> %s,\ttime: %s" % (self.device.address, parse_value(data), data.contents.epoch))
self.samples+= 1
def handle_data(self, ctx, data, data_type):
self.log("DEBUG",
"%s -> %s" % (self.device.address, parse_value(data)))
parsed_data = parse_value(data)
data_dict = {
'ts':
time.time(), # Note we can get this from the data contents itself
'type': data_type,
'contents': {
'x': parsed_data.x,
'y': parsed_data.y,
'z': parsed_data.z,
}
}
data_str = json.dumps(data_dict)
topics = "DATA"
for srv_id in self.target_servers.keys():
target = self.target_servers[srv_id]
mqtt_publish.single(topics,
payload=data_str,
hostname=target['address'],
port=target['port'])
return
def data_handler_gyro(self, ctx, data):
timeStamp = data.contents.epoch
date = datetime.datetime.fromtimestamp(
float(timeStamp) / 1000).strftime('%Y-%m-%d %H:%M:%S:%f')
self.gyro_x.append(parse_value(data).x)
self.gyro_y.append(parse_value(data).y)
self.gyro_z.append(parse_value(data).z)
self.timeStamp_gyro.append(timeStamp)
self.date_gyro.append(date)
def data_handler_acc(self, ctx, data):
timeStamp = data.contents.epoch
date = datetime.datetime.fromtimestamp(float(timeStamp)/1000).strftime('%Y-%m-%d %H:%M:%S:%f')
#print("%s -> %s" % (self.device.address, parse_value(data)))
self.acc_x.append(parse_value(data).x)
self.acc_y.append(parse_value(data).y)
self.acc_z.append(parse_value(data).z)
self.timeStamp_acc.append(timeStamp)
self.date_acc.append(date)
def data_handler(self, ctx, data):
print("%s -> %s" % (self.device.address, parse_value(data)))
self.samples += 1
temp_data = parse_value(data)
temp = json.dumps({
'x': temp_data.x,
'y': temp_data.y,
'z': temp_data.z
})
print(temp)
conn.sendall(temp)
def DevRun(self):
try:
## Get data and print to console ##
self.euler_signal = libmetawear.mbl_mw_sensor_fusion_get_data_signal(
self.board, SensorFusionData.EULER_ANGLE)
self.euler_callback = FnVoid_VoidP_DataP(
lambda context, data: print("epoch: %s, euler %s\n" % (
data.contents.epoch, parse_value(data))))
## Required for data extraction ##
libmetawear.mbl_mw_datasignal_subscribe(self.euler_signal, None,
self.euler_callback)
libmetawear.mbl_mw_sensor_fusion_enable_data(
self.board, SensorFusionData.EULER_ANGLE)
libmetawear.mbl_mw_sensor_fusion_set_mode(self.board,
SensorFusionMode.NDOF)
libmetawear.mbl_mw_sensor_fusion_write_config(self.board)
libmetawear.mbl_mw_sensor_fusion_start(self.board)
#input('') # Don't seem to need this ? ?
except OSError as err:
print("OS ERROR {}".format(err))
self.DevClose()
print("Device closed properly...")
sys.exit()
except ValueError:
print("Error with variable...")
self.DevClose()
print("Device closed properly...")
sys.exit()
except:
print("Unexpected Error:", sys.exc_info()[0])
self.DevClose()
print("Device closed properly...")
sys.exit()
def reset_handler(self, ctx, data):
values = parse_value(data)
if values == 0:
self.Send_data("/reset", 0, 0, 0)
sleep(0.5)
self.velocity['x'] = 0
self.velocity['y'] = 0
self.velocity['z'] = 0
self.position['x'] = 0
self.position['y'] = 0
self.position['z'] = 0
self.samples = 0
self.neglect_counter = 0
self.pre_X = 0
self.pre_Y = 0
self.pre_Z = 0
self.pre_velocityX = 0
self.pre_velocityY = 0
self.pre_velocityZ = 0
self.preSentX = -10000
self.preSentY = -10000
self.preSentZ = -10000
self.StopCountX = 0
self.StopCountY = 0
self.StopCountZ = 0
self.flagX = 0
self.flagY = 0
self.flagZ = 0
self.sumVelocityX = [0, 0, 0, 0, 0]
self.sumVelocityY = [0, 0, 0, 0, 0]
self.sumVelocityZ = [0, 0, 0, 0, 0]
self.flag_suppressX = 0
self.flag_suppressY = 0
self.flag_suppressZ = 0
self.flag_move_end = 0
def data_handler(self, ctx, data):
values = parse_value(data, n_elem=2)
# f.write('%.4f,%.4f,%.4f\n' % (values[1].x, values[1].y, values[1].z))
print("here:", self.samples)
if (
(self.samples == 0) or
((data.contents.epoch - self.sensor_data[self.samples - 1][0]) <
1000)
): #add initial point, compare current epoch to previous epoch to ensure timestamp not erroneous
self.sensor_data.append([
data.contents.epoch, 0, values[0].x, values[0].y, values[0].z,
values[1].x, values[1].y, values[1].z
])
else:
epochEstimate = self.sensor_data[self.samples - 1][
0] + 10 # use previous epoch to estimate missing timestamp, add 10ms
self.sensor_data.append([
epochEstimate, 0, values[0].x, values[0].y, values[0].z,
values[1].x, values[1].y, values[1].z
])
self.samples += 1
MESSAGE = str(values[1].z)
self.sock.sendto(bytes(MESSAGE, "utf-8"), (UDP_IP, self.UDP_PORT))
def data_handler(self, ctx, data):
values = parse_value(data, n_elem=2)
time_stamp = data.contents.epoch
status = self.mobitrack.processStep(
np.array([
time_stamp / 1000, values[0].x, values[0].y, values[0].z,
values[1].x, values[1].y, values[1].z
]))
# print(np.array([time_stamp / 1000, values[1].x, values[1].y, values[1].z]))
if self.led_on_bool == True:
self.steps_since_led_on = self.steps_since_led_on + 1
if self.steps_since_led_on >= self.steps_to_keep_led_on:
# turn LED OFF
libmetawear.mbl_mw_led_stop_and_clear(self.device.board)
self.steps_since_led_on = 0
self.led_on_bool = False
self.steps_to_keep_led_on = 30
if self.led and status["isRep"] != -1:
# turn LED ON
self.led_on_bool = True
self.steps_since_led_on = 0
self.led_on(LedColor.BLUE, 0, False)
def data_handlerz(self, ctx, data):
#print("%s -> %s" % (self.device.address, parse_value(data)))
# print("%s" % (parse_value(data)))
z = parse_value(data)
print("z = ", z)
self.samples += 1
def data_ghandler(self, ctx, data):
msg = parse_value(data)
if (self.samples == 1000):
self.samples = 0
else:
self.samples += 1
#print("%s -> %s" % (self.device.address, msg))
self.connection.send("G" + str(msg))
def data_handler(ptr):
global counter
value = parse_value(ptr)
print({"epoch": ptr.contents.epoch})
c.execute("INSERT INTO acc VALUES (?,?,?,?)",
(ptr.contents.epoch, value.x, value.y, value.z))
conn.commit()
counter += 1
def data_handlerax(self, ctx, data):
global ax, ay, az
# print("%s -> %s" % (self.device.address, parse_value(data)))
self.samples += 1
z = parse_value(data)
ax = np.append(ax, [z.x])
ay = np.append(ay, [z.y])
az = np.append(az, [z.z])
def data_handler(self, ctx, data):
global pitch, roll, heading
# print("%s -> %s" % (self.device.address, parse_value(data)))
self.samples += 1
z = parse_value(data)
pitch = np.append(pitch, [z.pitch])
roll = np.append(roll, [z.roll])
heading = np.append(heading, [z.heading])
def data_handler(ctx, data):
global samples
#ws = create_connection("ws://localhost:65432")
#ws.send("%s" % (parse_value(data)))
var = json.dumps("%s" % (parse_value(data))).encode()
s.sendall(var)
samples += 1
def write(self, ctx, ptr):
d_struct = parse_value(ptr)
if self.file is None:
print("Opening %s for write." % (self.filename))
self.file = open(self.filename, 'w')
self.file.write('epoch,' +
','.join([f[0] for f in d_struct._fields_]) + '\n')
data = [str(getattr(d_struct, f[0])) for f in d_struct._fields_]
self.file.write(str(ptr.contents.epoch) + ',' + ','.join(data) + '\n')
def data_handler(self, ctx, data):
raw_point = parse_value(data)
point = Point(raw_point.x, raw_point.y, raw_point.z,
self.device.address)
if BATCH_STORE:
self.data.append(point)
else:
self.strage.store([point])
self.samples += 1
def __init__(self, signal):
self.identifier = libmetawear.mbl_mw_anonymous_datasignal_get_identifier(
signal)
self.data_handler_fn = FnVoid_VoidP_DataP(
lambda ctx, ptr: print({
"identifier": self.identifier,
"epoch": ptr.contents.epoch,
"value": parse_value(ptr)
}))
def data_handler(self, ctx, data):
values = parse_value(data, n_elem=2)
# f.write('%.4f,%.4f,%.4f\n' % (values[1].x, values[1].y, values[1].z))
self.sensor_data.append([
data.contents.epoch, 0, values[0].x, values[0].y, values[0].z,
values[1].x, values[1].y, values[1].z
])
# print("time: %s\tacc: (%.4f,%.4f,%.4f),\tgyro; (%.4f,%.4f,%.4f)" % (data.contents.epoch, values[0].x, values[0].y, values[0].z, values[1].x, values[1].y, values[1].z))
self.samples += 1
def data_handler(self, ctx, data):
values = parse_value(data, n_elem=2)
time_stamp = data.contents.epoch
data_current = [
time_stamp, values[0].x, values[0].y, values[0].z, values[1].x,
values[1].y, values[1].z
]
sensor_data.append("%d,%f,%f,%f,%f,%f,%f" %
(time_stamp, values[0].x, values[0].y, values[0].z,
values[1].x, values[1].y, values[1].z))
def data_handler(self, ctx, data):
values = parse_value(data, n_elem = 2)
self.sensor_data.append([data.contents.epoch, 0, values[0].x, values[0].y, values[0].z, values[1].x, values[1].y, values[1].z])
# self.xs.pop(0)
# self.xs.append(data.contents.epoch)
self.ys.pop(0)
self.ys.append(values[1].z)
self.samples += 1
def acc_cb(self, ctx, data):
self.samples_acc += 1
temp_data = parse_value(data)
acc = Vector3Stamped()
acc.header.stamp = rospy.Time.now()
acc.header.frame_id = self.frame_id
acc.vector.x = temp_data.x
acc.vector.y = temp_data.y
acc.vector.z = temp_data.z
self.pub_accel.publish(acc)
def data_handler(ptr):
value = parse_value(ptr)
print({
"epoch": ptr.contents.epoch,
"value x": value.x,
"value y": value.y,
"value z": value.z
})
c.execute("INSERT INTO acc VALUES (?,?,?,?)",
(ptr.contents.epoch, value.x, value.y, value.z))
conn.commit()
def __init__(self, signal):
raw = libmetawear.mbl_mw_anonymous_datasignal_get_identifier(signal)
self.identifier = cast(raw, c_char_p).value.decode("ascii")
self.data_handler_fn = FnVoid_DataP(
lambda ptr: print({
"identifier": self.identifier,
"epoch": ptr.contents.epoch,
"value": parse_value(ptr)
}))
libmetawear.mbl_mw_memory_free(raw)
def gyro_cb(self, ctx, data):
self.samples_gyro += 1
temp_data = parse_value(data)
gyro = Vector3Stamped()
gyro.header.stamp = rospy.Time.now()
gyro.header.frame_id = self.frame_id
gyro.vector.x = temp_data.x
gyro.vector.y = temp_data.y
gyro.vector.z = temp_data.z
self.pub_gyro.publish(gyro)
def data_handler(self, ctx, data):
parsed = parse_value(data)
# self.file.write("%s -> %s\n" % (self.device.address, parsed))
xyz_shot = {
"timeStamp": datetime.datetime.utcnow().isoformat(),
"x": parsed.x,
"y": parsed.y,
"z": parsed.z,
"xyzShotPosition": self.position
}
self.gui.xyz_shots.append(xyz_shot)
def g_cb(self, ctx, data):
self.samples_acc += 1
temp_data = parse_value(data)
# print("acc; (%.4f,%.4f,%.4f) " % (temp_data.x, temp_data.y, temp_data.z) )
# print(temp_data)
acc = Vector3Stamped()
acc.header.stamp = rospy.Time.now()
acc.header.frame_id = self.frame_id
acc.vector.x = temp_data.x
acc.vector.y = temp_data.y
acc.vector.z = temp_data.z
self.pub_g.publish(acc)
def gpio_handler(self, ctx, data):
values = parse_value(data)
if values == 0 and self.IsPressed == 0:
self.IsPressed = 1
if values == 1 and self.IsPressed == 1:
self.IsPressed = 0
sendX = round(self.position['x'], 1)
sendY = round(self.position['y'], 1)
sendZ = round(self.position['z'], 1)
self.Send_data("/data", sendX, sendY, sendZ)
print("gpio = ", values)
print("IsPressed = ", self.IsPressed)
def calibration_handler(ctx, pointer):
value = parse_value(pointer)
print("state: %s" % (value))
if (value.accelrometer == Const.SENSOR_FUSION_CALIBRATION_ACCURACY_HIGH and \
value.gyroscope == Const.SENSOR_FUSION_CALIBRATION_ACCURACY_HIGH and \
value.magnetometer == Const.SENSOR_FUSION_CALIBRATION_ACCURACY_HIGH):
# read
libmetawear.mbl_mw_sensor_fusion_read_calibration_data(
device.board, None, fn_wrapper_01)
else:
sleep(1.0)
libmetawear.mbl_mw_datasignal_read(signal)
def gyro_cb(self, ctx, data):
# print("%s -> %s" % (self.device.address, parse_value(data)))
self.samples_gyro += 1
temp_data = parse_value(data)
gyro = Vector3Stamped()
gyro.header.stamp = rospy.Time.now()
gyro.header.frame_id = self.frame_id
gyro.vector.x = temp_data.x
gyro.vector.y = temp_data.y
gyro.vector.z = temp_data.z
# print("gyro; (%.4f,%.4f,%.4f) " % (temp_data.x, temp_data.y, temp_data.z) )
self.pub_gyro.publish(gyro)
