def echo_socket(ws):
b = TransformBroadcaster()
f = open("orientation.txt", "a")
while True:
message = ws.receive()
orient_data = message.split(',')
orient_data = [float(data) for data in orient_data]
stamped_msg = SensorMsgStamped()
stamped_msg.data = orient_data
stamped_msg.header.stamp.secs = Time.now().secs
stamped_msg.header.stamp.nsecs = Time.now().nsecs
orient_pub_stamped.publish(stamped_msg)
### Publish to Pose topic for visualization ###
q = quaternion_from_euler(orient_data[1], orient_data[2],
orient_data[3])
pose_msg = Pose()
pose_msg.orientation.x = q[0]
pose_msg.orientation.y = q[1]
pose_msg.orientation.z = q[2]
pose_msg.orientation.w = q[3]
pose_pub.publish(pose_msg)
b.sendTransform((1, 1, 1), (q[0], q[1], q[2], q[3]), Time.now(),
'child_link', 'base_link')
### END HERE ###
print("[INFO:] Orientation{}".format(orient_data))
ws.send(message)
print >> f, message
f.close()
def gyro_cb(msg_gyro):
global rollG, pitchG, yawG, prev_time, q
# extract data
wx, wy, wz = msg_gyro.data
# we need to calculate dt
secs = Time.now().secs
nsecs = Time.now().nsecs
cur_time = secs + nsecs * 10**(-9)
dt = cur_time - prev_time
prev_time = cur_time
# process gyroscope data
current_gyro_quat = quaternion_from_euler(wx * dt, wy * dt, wz * dt)
q = quaternion_multiply(current_gyro_quat, q)
rollG, pitchG, yawG = euler_from_quaternion(q)
# lets publish this transform
# tf_br.sendTransform((0, 0, 0.5), (q[0],q[1],q[2],q[3]), Time.now(), "base_link", "world")
# print the rollG, pitchG, yawG
if DEBUG:
print("rollG : {}, pitchG : {}, yawG : {}".format(
m.degrees(rollG), m.degrees(pitchG), m.degrees(yawG)))
# publish the roll pitch yaw topic
stamped_msg = SensorMsgStamped()
stamped_msg.data = [rollG, pitchG, yawG]
stamped_msg.header.stamp.secs = secs
stamped_msg.header.stamp.nsecs = nsecs
rpyG_pub_stamped.publish(stamped_msg)
def gyro_cb(msg_gyro):
global q, prev_time, P, INI_SET
if not INI_SET:
# initial state is not set
return
# extract data
wx, wy, wz = msg_gyro.data
# we need to calculate time_elapsed
secs = Time.now().secs
nsecs = Time.now().nsecs
cur_time = secs + nsecs * 10**(-9)
time_elapsed = cur_time - prev_time
prev_time = cur_time
# integrate using eulers integration method to find the estimates and covariance
# prediction
# q, P = integrateTillT(q, dt, time_elapsed, wx, wy, wz, P)
current_gyro_quat = quaternion_from_euler(wx * dt, wy * dt, wz * dt)
q = quaternion_multiply(current_gyro_quat, q)
# make the covariance prediction
P += Q
rollF, pitchF, yawF = euler_from_quaternion(q)
print("[GYRO_CB] rollF : {}, pitchF : {}, yawF : {}, P: \n{}".format(
m.degrees(rollF), m.degrees(pitchF), m.degrees(yawF), P))
# publish to topic
stamped_msg = SensorMsgStamped()
stamped_msg.data = [rollF, pitchF, yawF]
stamped_msg.header.stamp.secs = secs
stamped_msg.header.stamp.nsecs = nsecs
rpyKF_pub_stamped.publish(stamped_msg)
def gyro_cb(msg_gyro):
global q, prev_time, INI_SET
if not INI_SET:
return
# extract data
wx, wy, wz = msg_gyro.data
# we need to calculate dt
secs = Time.now().secs
nsecs = Time.now().nsecs
cur_time = secs + nsecs * 10 ** (-9)
dt = cur_time - prev_time
prev_time = cur_time
print("the time gap is: {}".format(dt))
# process gyroscope data
current_gyro_quat = quaternion_from_euler(wx * dt, wy * dt, wz * dt)
q = quaternion_multiply(current_gyro_quat, q)
rollF, pitchF, yawF = euler_from_quaternion(q)
print("[GYRO_CB] rollF : {}, pitchF : {}, yawF : {}".format(m.degrees(rollF), m.degrees(pitchF), m.degrees(yawF)))
# publish to topic
stamped_msg = SensorMsgStamped()
stamped_msg.data = [rollF, pitchF, yawF]
stamped_msg.header.stamp.secs = secs
stamped_msg.header.stamp.nsecs = nsecs
rpyCF_pub_stamped.publish(stamped_msg)
def echo_socket(ws):
f = open("gyroscope.txt", "a")
while True:
message = ws.receive()
gyro_data = message.split(',')
gyro_data = [float(data) for data in gyro_data]
stamped_msg = SensorMsgStamped()
stamped_msg.data = gyro_data
stamped_msg.header.stamp.secs = Time.now().secs
stamped_msg.header.stamp.nsecs = Time.now().nsecs
gyro_pub_stamped.publish(stamped_msg)
print("[INFO:] Gyroscope{}".format(gyro_data))
ws.send(message)
print >> f, message
f.close()
def echo_socket(ws):
f = open("accelerometer.txt", "a")
while True:
message = ws.receive()
accel_data = message.split(',')
accel_data = [float(data) for data in accel_data]
stamped_msg = SensorMsgStamped()
stamped_msg.data = accel_data
stamped_msg.header.stamp.secs = Time.now().secs
stamped_msg.header.stamp.nsecs = Time.now().nsecs
accel_pub_stamped.publish(stamped_msg)
print("[INFO:] Accelerometer{}".format(accel_data))
ws.send(message)
print >> f, message
f.close()
def echo_socket(ws):
global magneto_data, magneto_pub
f = open("magnetometer.txt", "a")
while True:
message = ws.receive()
magneto_data = message.split(',')
magneto_data = [float(data) for data in magneto_data]
stamped_msg = SensorMsgStamped()
stamped_msg.data = magneto_data
stamped_msg.header.stamp.secs = Time.now().secs
stamped_msg.header.stamp.nsecs = Time.now().nsecs
magneto_pub_stamped.publish(stamped_msg)
print("[INFO:] Magnetometer{}".format(magneto_data))
ws.send(message)
print >> f, message
f.close()
def echo_socket(ws):
global geoloc_data, geoloc_pub
f = open("geolocation.txt", "a")
while True:
message = ws.receive()
geoloc_data = message.split(',')
geoloc_data = [float(data) for data in geoloc_data]
stamped_msg = SensorMsgStamped()
stamped_msg.data = geoloc_data
stamped_msg.header.stamp.secs = Time.now().secs
stamped_msg.header.stamp.nsecs = Time.now().nsecs
geoloc_pub_stamped.publish(stamped_msg)
print("[INFO:] Geolocation{}".format(geoloc_data))
ws.send(message)
print >> f, message
f.close()
def fusion_cb(msg_gyro, msg_accel, msg_mag):
global q, prev_time, P, INI_SET, KF_roll, KF_pitch, KF_yaw, KF_time, roll_correction, pitch_correction, yaw_correction
SKIP_GYRO = False
if not INI_SET:
SKIP_GYRO = True
## extract data
ax, ay, az = msg_accel.data
mx, my, mz = msg_mag.data
wx, wy, wz = msg_gyro.data
### ACCELEROMETER AND MAGNETOMETER CALCULATIONS ###
## normalize accelerometer data
norm_accel = m.sqrt(ax**2 + ay**2 + az**2)
ax = ax / norm_accel
ay = ay / norm_accel
az = az / norm_accel
## normalize the magnetometer data
norm_mag = m.sqrt(mx**2 + my**2 + mz**2)
mx = mx / norm_mag
my = my / norm_mag
mz = mz / norm_mag
## get the roll and pitch
# rollA = m.atan2(ay, az) + 5
rollA = m.atan2(ay, m.sqrt(ax**2 + az**2))
pitchA = m.atan2(-ax, m.sqrt(ay**2 + az**2))
# pitchA = m.atan2(ay, m.sqrt(ay**2 + az**2))
# rollA = m.atan2(-ax, m.sqrt(ax**2 + az**2))
## compensate for yaw using magnetometer
# Mx = mx * m.cos(pitchA) + mz * m.sin(pitchA)
# My = mx * m.sin(rollA) * m.sin(pitchA) + my * m.cos(rollA) - mz * m.sin(rollA) * m.cos(pitchA)
# yawM = m.atan2(-My, Mx)
# yawM = m.atan2(az, m.sqrt(ax**2 + az**2))
Mx = -mx * m.cos(pitchA) + mz * m.sin(pitchA)
My = mx * m.sin(rollA) * m.sin(pitchA) + my * m.cos(rollA) + mz * m.sin(
rollA) * m.cos(pitchA)
yawM = m.atan2(My, Mx)
# yawM = m.atan2(az, m.sqrt(ax**2 + az**2))
## PREDICTION IF NEEDED
if not SKIP_GYRO:
# carry out prediction step
# we need to calculate time_elapsed
secs = Time.now().secs
nsecs = Time.now().nsecs
cur_time = secs + nsecs * 10**(-9)
time_elapsed = cur_time - prev_time
prev_time = cur_time
# integrate using eulers integration method to find the estimates and covariance
q, P = integrateTillT(q, dt, time_elapsed, wx, wy, wz, P)
# extract angles
rollF, pitchF, yawF = euler_from_quaternion(q)
# compute kalman gain
K = np.dot(P, np.linalg.inv(P + R))
# carry out correction step
meas = np.array([[rollA], [pitchA], [yawM]])
#meas = np.array([[rollF],[pitchF],[yawF]]) #ADDED TO IGNORE ACC AND MAG
state = np.array([[rollF], [pitchF], [yawF]])
state = state + np.dot(K, meas - state)
state = [float(val) for val in state]
rollF, pitchF, yawF = state
# update the quaternion
q = quaternion_from_euler(rollF, pitchF, yawF)
# update covariance
P = np.dot((np.eye(3) - K), P)
else:
if comp_time_gt is not None:
roll_gt = GT_roll[0]
pitch_gt = GT_pitch[0]
yaw_gt = GT_yaw[0]
roll_ned = rollA
pitch_ned = pitchA
yaw_ned = yawM
roll_correction = (roll_gt - roll_ned)
#roll_correction = wrapToPi(0 - roll_ned)
pitch_correction = (pitch_gt - pitch_ned)
yaw_correction = (yaw_gt - yaw_ned)
#yaw_correction = wrapToPi(0 - yaw_ned)
# INI_SET = True
else:
return
INI_SET = True
# there is no gyro measurement yet
rollF = rollA
pitchF = pitchA
yawF = yawM
q = quaternion_from_euler(rollF, pitchF, yawF)
P = R # and thus the initial noise would be equal to noise in measurement
print("[FUSION_CB] rollF : {}, pitchF : {}, yawF : {}, P: \n{}".format(
m.degrees(rollF), m.degrees(pitchF), m.degrees(yawF), P))
# publish to topic
stamped_msg = SensorMsgStamped()
stamped_msg.data = [rollF, pitchF, yawF]
stamped_msg.header.stamp.secs = Time.now().secs
stamped_msg.header.stamp.nsecs = Time.now().nsecs
rpyKF_pub_stamped.publish(stamped_msg)
# get time
t = Time.now().secs + Time.now().nsecs * 10**(-9) - comp_time
# append to Global variables
KF_roll.append(rollF + roll_correction)
KF_pitch.append(pitchF + pitch_correction)
KF_yaw.append(yawF + yaw_correction)
M_roll.append(wrapToPi(rollA))
M_pitch.append(wrapToPi(pitchA))
M_yaw.append(wrapToPi(yawM))
KF_time.append(t)
def fusion_cb(msg_gyro, msg_accel, msg_mag):
global q, prev_time, INI_SET, KF_roll, KF_pitch, KF_yaw, KF_time, roll_correction, pitch_correcton, yaw_correction
SKIP_GYRO = False
if not INI_SET:
SKIP_GYRO = True
## extract data
ax, ay, az = msg_accel.data
mx, my, mz = msg_mag.data
wx, wy, wz = msg_gyro.data
### ACCELEROMETER AND MAGNETOMETER CALCULATIONS ###
## normalize accelerometer data
norm_accel = m.sqrt(ax**2 + ay**2 + az**2)
ax = ax / norm_accel
ay = ay / norm_accel
az = az / norm_accel
## normalize the magnetometer data
norm_mag = m.sqrt(mx**2 + my**2 + mz**2)
mx = mx / norm_mag
my = my / norm_mag
mz = mz / norm_mag
## get the roll and pitch
rollA = m.atan2(ay, az)
pitchA = m.atan2(-ax, m.sqrt(ay**2 + az**2))
## compensate for yaw using magnetometer
Mx = mx * m.cos(pitchA) + mz * m.sin(pitchA)
My = mx * m.sin(rollA) * m.sin(pitchA) + my * m.cos(rollA) - mz * m.sin(rollA) * m.cos(pitchA)
yawM = m.atan2(-My, Mx)
if not SKIP_GYRO:
### GYROSCOPE CALCULATIONS ###
## compute roll, pitch and yaw from gyro
secs = Time.now().secs
nsecs = Time.now().nsecs
cur_time = secs + nsecs * 10 ** (-9)
dt = cur_time - prev_time
prev_time = cur_time
print("the time gap is: {}".format(dt))
# process gyroscope data
current_gyro_quat = quaternion_from_euler(wx * dt, wy * dt, wz * dt)
q = quaternion_multiply(current_gyro_quat, q)
rollG, pitchG, yawG = euler_from_quaternion(q)
### COMPLIMENTARY FILTER ###
rollF = rollG * g_wt + rollA * am_wt
pitchF = pitchG * g_wt + pitchA * am_wt
yawF = yawG * g_wt + yawM * am_wt
# set back the quaternion
q = quaternion_from_euler(rollF, pitchF, yawF)
else:
if comp_time_gt is not None:
roll_gt = GT_roll[0]
pitch_gt = GT_pitch[0]
yaw_gt = GT_yaw[0]
roll_ned = rollA
pitch_ned = pitchA
yaw_ned = yawM
roll_correction = wrapToPi(roll_gt - roll_ned)
pitch_correcton = wrapToPi(pitch_gt - pitch_ned)
yaw_correction = wrapToPi(yaw_gt - yaw_ned)
else:
return
INI_SET = True
q = quaternion_from_euler(rollA, pitchA, yawM)
rollF, pitchF, yawF = euler_from_quaternion(q)
print("[FUSION_CB] rollF : {}, pitchF : {}, yawF : {}".format(m.degrees(rollF), m.degrees(pitchF), m.degrees(yawF)))
# publish to topic
stamped_msg = SensorMsgStamped()
stamped_msg.data = [rollF, pitchF, yawF]
stamped_msg.header.stamp.secs = Time.now().secs
stamped_msg.header.stamp.nsecs = Time.now().nsecs
rpyCF_pub_stamped.publish(stamped_msg)
# get time
t = Time.now().secs + Time.now().nsecs * 10 ** (-9) - comp_time
# append to Global variables
KF_roll.append(wrapToPi(rollF + roll_correction))
KF_pitch.append(wrapToPi(pitchF + pitch_correcton))
KF_yaw.append(wrapToPi(yawF + yaw_correction))
KF_time.append(t)
def got_accel_mag(msg_accel, msg_mag):
## GET ROLL AND PITCH FROM THE ACCELEROMETER
ax, ay, az = msg_accel.data
mx, my, mz = msg_mag.data
if DEBUG:
print("raw acceleration values ax : {}, ay : {}, az : {}".format(
ax, ay, az))
print("raw magnetometer values mx : {}, my : {}, mz : {}".format(
mx, my, mz))
## NORMALIZE THE ACCELEROMETER DATA
norm_accel = m.sqrt(ax**2 + ay**2 + az**2)
ax = ax / norm_accel
ay = ay / norm_accel
az = az / norm_accel
# ## NORMALIZE THE MAGNETOMETER DATA
norm_mag = m.sqrt(mx**2 + my**2 + mz**2)
mx = mx / norm_mag
my = my / norm_mag
mz = mz / norm_mag
if DEBUG:
print(
"normalized acceleration values ax : {}, ay : {}, az : {}".format(
ax, ay, az))
print(
"normalized magnetometer values mx : {}, my : {}, mz : {}".format(
mx, my, mz))
## GET THE ROLL AND PITCH
rollA = m.atan2(ay, az)
pitchA = m.atan2(-ax, m.sqrt(ay**2 + az**2))
## COMPENSATE FOR YAW USING MAGNETOMETER
Mx = mx * m.cos(pitchA) + mz * m.sin(pitchA)
My = mx * m.sin(rollA) * m.sin(pitchA) + my * m.cos(rollA) - mz * m.sin(
rollA) * m.cos(pitchA)
yawM = m.atan2(-My, Mx)
if DEBUG:
print("rollA : {}, pitchA : {}, yawM : {}".format(
m.degrees(rollA), m.degrees(pitchA), m.degrees(yawM)))
print(
"###############################################################################"
)
print(
" "
)
## CONVERT TO quaternion_from_euler
q = quaternion_from_euler(rollA, pitchA, yawM)
## LETS PUBLISH THIS TO THE BROADCASTER
tf_br.sendTransform((0.0, 0.0, 0.5), (q[0], q[1], q[2], q[3]), Time.now(),
'base_link', 'world')
# publish the roll pitch yaw topic
stamped_msg = SensorMsgStamped()
stamped_msg.data = [rollA, pitchA, yawM]
stamped_msg.header.stamp.secs = Time.now().secs
stamped_msg.header.stamp.nsecs = Time.now().nsecs * 10**(-9)
rpyAM_pub_stamped.publish(stamped_msg)
def fusion_cb(msg_gyro, msg_accel, msg_mag):
print("entered fusion")
global q, prev_time, P, INI_SET
SKIP_GYRO = False
if not INI_SET:
SKIP_GYRO = True
## extract data
ax, ay, az = msg_accel.data
mx, my, mz = msg_mag.data
wx, wy, wz = msg_gyro.data
### ACCELEROMETER AND MAGNETOMETER CALCULATIONS ###
## normalize accelerometer data
norm_accel = m.sqrt(ax**2 + ay**2 + az**2)
ax = ax / norm_accel
ay = ay / norm_accel
az = az / norm_accel
## normalize the magnetometer data
norm_mag = m.sqrt(mx**2 + my**2 + mz**2)
mx = mx / norm_mag
my = my / norm_mag
mz = mz / norm_mag
## get the roll and pitch
rollA = m.atan2(ay, az)
pitchA = m.atan2(-ax, m.sqrt(ay**2 + az**2))
## compensate for yaw using magnetometer
Mx = mx * m.cos(pitchA) + mz * m.sin(pitchA)
My = mx * m.sin(rollA) * m.sin(pitchA) + my * m.cos(rollA) - mz * m.sin(
rollA) * m.cos(pitchA)
yawM = m.atan2(-My, Mx)
## rollA, pitchA, yawM are the measurements
## PREDICTION IF NEEDED
if not SKIP_GYRO:
# carry out prediction step
# we need to calculate time_elapsed
secs = Time.now().secs
nsecs = Time.now().nsecs
cur_time = secs + nsecs * 10**(-9)
time_elapsed = cur_time - prev_time
prev_time = cur_time
print("starting integrate till T")
# integrate using eulers integration method to find the estimates and covariance
# q, P = integrateTillT(q, dt, time_elapsed, wx, wy, wz, P)
# make the state prediction
current_gyro_quat = quaternion_from_euler(wx * dt, wy * dt, wz * dt)
q = quaternion_multiply(current_gyro_quat, q)
# make the covariance prediction
P += Q
print("done integrating")
# extract angles
rollF, pitchF, yawF = euler_from_quaternion(q)
# compute kalman gain
K = np.dot(P, np.linalg.inv(P + R))
# carry out correction step
meas = np.array([[rollA], [pitchA], [yawM]])
state = np.array([[rollF], [pitchF], [yawF]])
state = state + np.dot(K, meas - state)
state = [float(val) for val in state]
rollF, pitchF, yawF = state
# update the quaternion
q = quaternion_from_euler(rollF, pitchF, yawF)
# update covariance
P = np.dot((np.eye(3) - K), P)
else:
INI_SET = True
# there is no gyro measurement yet
rollF = rollA
pitchF = pitchA
yawF = yawM
q = quaternion_from_euler(rollF, pitchF, yawF)
P = R # and thus the initial noise would be equal to noise in measurement
print("[FUSION_CB] rollF : {}, pitchF : {}, yawF : {}, P: \n{}".format(
m.degrees(rollF), m.degrees(pitchF), m.degrees(yawF), P))
# publish to topic
stamped_msg = SensorMsgStamped()
stamped_msg.data = [rollF, pitchF, yawF]
stamped_msg.header.stamp.secs = Time.now().secs
stamped_msg.header.stamp.nsecs = Time.now().nsecs
rpyKF_pub_stamped.publish(stamped_msg)
def fusion_cb(msg_gyro, msg_accel, msg_mag):
global q, prev_time, INI_SET
SKIP_GYRO = False
if not INI_SET:
SKIP_GYRO = True
## extract data
ax, ay, az = msg_accel.data
mx, my, mz = msg_mag.data
wx, wy, wz = msg_gyro.data
### ACCELEROMETER AND MAGNETOMETER CALCULATIONS ###
## normalize accelerometer data
norm_accel = m.sqrt(ax**2 + ay**2 + az**2)
ax = ax / norm_accel
ay = ay / norm_accel
az = az / norm_accel
## normalize the magnetometer data
norm_mag = m.sqrt(mx**2 + my**2 + mz**2)
mx = mx / norm_mag
my = my / norm_mag
mz = mz / norm_mag
## get the roll and pitch
rollA = m.atan2(ay, az)
pitchA = m.atan2(-ax, m.sqrt(ay**2 + az**2))
## compensate for yaw using magnetometer
Mx = mx * m.cos(pitchA) + mz * m.sin(pitchA)
My = mx * m.sin(rollA) * m.sin(pitchA) + my * m.cos(rollA) - mz * m.sin(rollA) * m.cos(pitchA)
yawM = m.atan2(-My, Mx)
if not SKIP_GYRO:
### GYROSCOPE CALCULATIONS ###
## compute roll, pitch and yaw from gyro
secs = Time.now().secs
nsecs = Time.now().nsecs
cur_time = secs + nsecs * 10 ** (-9)
dt = cur_time - prev_time
prev_time = cur_time
print("the time gap is: {}".format(dt))
# process gyroscope data
current_gyro_quat = quaternion_from_euler(wx * dt, wy * dt, wz * dt)
q = quaternion_multiply(current_gyro_quat, q)
rollG, pitchG, yawG = euler_from_quaternion(q)
### COMPLIMENTARY FILTER ###
rollF = rollG * g_wt + rollA * am_wt
pitchF = pitchG * g_wt + pitchA * am_wt
yawF = yawG * g_wt + yawM * am_wt
# set back the quaternion
q = quaternion_from_euler(rollF, pitchF, yawF)
else:
INI_SET = True
q = quaternion_from_euler(rollA, pitchA, yawM)
rollF, pitchF, yawF = euler_from_quaternion(q)
print("[FUSION_CB] rollF : {}, pitchF : {}, yawF : {}".format(m.degrees(rollF), m.degrees(pitchF), m.degrees(yawF)))
# publish to topic
stamped_msg = SensorMsgStamped()
stamped_msg.data = [rollF, pitchF, yawF]
stamped_msg.header.stamp.secs = Time.now().secs
stamped_msg.header.stamp.nsecs = Time.now().nsecs
# lets publish this transform
# tf_br.sendTransform((0, 0, 0.5), (q[0],q[1],q[2],q[3]), Time.now(), "base_link", "world")
rpyCF_pub_stamped.publish(stamped_msg)