def imu_handler(self, channel, data):
self.imumsg = ins_t.decode(data)
#lRb, self.prtObj.yaw = self.prtObj.propNavIMU(0.01, np.array(self.imumsg.gyro)[:,np.newaxis], np.array(self.imumsg.accel)[:,np.newaxis], yaw=self.prtObj.yaw)
self.prtObj.upStateIMU_mod(0.01, np.array(self.imumsg.gyro)[:,np.newaxis], np.array(self.imumsg.accel)[:,np.newaxis])
lRb = self.prtObj.lRb()
#self.prtObj.a_prev[:,0] = self.imumsg.accel
if self.viconmsg:
p = Pose.from_rigid_transform(0, RigidTransform(tvec=self.viconmsg.pos))
# print 'acc', self.imumsg.accel
# bRa = self.prtObj.bRa(acc=np.matrix(self.imumsg.accel).T)
# lRb = self.prtObj.lRb(np.matrix(self.imumsg.accel).T)
bRl = lRb.T
rt = RigidTransform.from_Rt(lRb , self.viconmsg.pos) # bRl
#print( "R, P, Y, calc_Y: %.3f, %.3f, %.3f, %.3f" % (rt.to_roll_pitch_yaw_x_y_z()[0]*180/np.pi, rt.to_roll_pitch_yaw_x_y_z()[1]*180/np.pi, rt.to_roll_pitch_yaw_x_y_z()[2]*180/np.pi, self.prtObj.yaw*180/np.pi) )
o = Pose.from_rigid_transform(1, rt)
# plot accelerometer estimated orientation of IMU
publish_pose_list('Localpose', [p], frame_id='origin')
publish_pose_list('IMUpose', [o], frame_id='origin')
#publish_cloud('particle_imu_max', self.imu_max.T+self.viconmsg.pos, c='b', frame_id='origin')
#test magnetometer
lMag = self.prtObj.magUpdateYaw_mod(np.array(self.imumsg.mag)[:,np.newaxis])
#lMag = self.prtObj.magUpdateYaw_mod(np.array([[1.0],[0.0],[0.0]]))
#lMag /= np.linalg.norm(lMag)
#lMag = np.dot(bRl, np.array([[1.0],[0.0],[0.0]]))
publish_cloud('mag', np.array(self.imumsg.mag)+self.viconmsg.pos, c='b', frame_id='origin')
def imu_handler(self, channel, data):
self.imumsg = ins_t.decode(data)
# lRb, self.prtObj.yaw = self.prtObj.propNavIMU(0.01, np.array(self.imumsg.gyro)[:,np.newaxis], np.array(self.imumsg.accel)[:,np.newaxis], yaw=self.prtObj.yaw)
self.prtObj.upStateIMU_mod(
0.01, np.array(self.imumsg.gyro)[:, np.newaxis], np.array(self.imumsg.accel)[:, np.newaxis]
)
lRb = self.prtObj.lRb()
# self.prtObj.a_prev[:,0] = self.imumsg.accel
if self.viconmsg:
p = Pose.from_rigid_transform(0, RigidTransform(tvec=self.viconmsg.pos))
# print 'acc', self.imumsg.accel
# bRa = self.prtObj.bRa(acc=np.matrix(self.imumsg.accel).T)
# lRb = self.prtObj.lRb(np.matrix(self.imumsg.accel).T)
bRl = lRb.T
rt = RigidTransform.from_Rt(lRb, self.viconmsg.pos) # bRl
# print( "R, P, Y, calc_Y: %.3f, %.3f, %.3f, %.3f" % (rt.to_roll_pitch_yaw_x_y_z()[0]*180/np.pi, rt.to_roll_pitch_yaw_x_y_z()[1]*180/np.pi, rt.to_roll_pitch_yaw_x_y_z()[2]*180/np.pi, self.prtObj.yaw*180/np.pi) )
o = Pose.from_rigid_transform(1, rt)
# plot accelerometer estimated orientation of IMU
publish_pose_list("Localpose", [p], frame_id="origin")
publish_pose_list("IMUpose", [o], frame_id="origin")
# publish_cloud('particle_imu_max', self.imu_max.T+self.viconmsg.pos, c='b', frame_id='origin')
# test magnetometer
lMag = self.prtObj.magUpdateYaw_mod(np.array(self.imumsg.mag)[:, np.newaxis])
# lMag = self.prtObj.magUpdateYaw_mod(np.array([[1.0],[0.0],[0.0]]))
# lMag /= np.linalg.norm(lMag)
# lMag = np.dot(bRl, np.array([[1.0],[0.0],[0.0]]))
publish_cloud("mag", np.array(self.imumsg.mag) + self.viconmsg.pos, c="b", frame_id="origin")
def my_handler(self, channel, data):
global uts
firstpass = False
if uts < 1:
firstpass = True
msg = ins_t.decode(data)
dt = (msg.utime - uts)*1e-6
uts = msg.utime
if firstpass:
return
bRi = self.prtObj.bRi
acc = np.array(msg.accel)[:,np.newaxis]
gyr = np.array(msg.gyro)[:,np.newaxis]
acc = np.dot(bRi, acc)
gyr = np.dot(bRi, gyr)
self.prtObj.yaw = self.prtObj.propNavIMU(dt, gyr, acc, self.prtObj.yaw)
self.prtObj.a_prev[:,0:1] = acc
aRb = self.prtObj.bRa(acc=np.array(msg.accel)[:,np.newaxis]).T
rt = RigidTransform.from_Rt( aRb , [0,0,0])
o = Pose.from_rigid_transform(1, rt)
publish_pose_list('aRb', [o], frame_id='origin')
lRb = self.prtObj.lRb()
rt2 = RigidTransform.from_Rt( lRb , [0,0,0])
o2 = Pose.from_rigid_transform(2, rt2)
publish_pose_list('lRb', [o2], frame_id='origin')
def my_handler(self, channel, data):
global uts
firstpass = False
if uts < 1:
firstpass = True
msg = ins_t.decode(data)
dt = (msg.utime - uts) * 1e-6
uts = msg.utime
if firstpass:
return
bRi = self.prtObj.bRi
acc = np.array(msg.accel)[:, np.newaxis]
gyr = np.array(msg.gyro)[:, np.newaxis]
acc = np.dot(bRi, acc)
gyr = np.dot(bRi, gyr)
self.prtObj.yaw = self.prtObj.propNavIMU(dt, gyr, acc, self.prtObj.yaw)
self.prtObj.a_prev[:, 0:1] = acc
aRb = self.prtObj.bRa(acc=np.array(msg.accel)[:, np.newaxis]).T
rt = RigidTransform.from_Rt(aRb, [0, 0, 0])
o = Pose.from_rigid_transform(1, rt)
publish_pose_list('aRb', [o], frame_id='origin')
lRb = self.prtObj.lRb()
rt2 = RigidTransform.from_Rt(lRb, [0, 0, 0])
o2 = Pose.from_rigid_transform(2, rt2)
publish_pose_list('lRb', [o2], frame_id='origin')
def imu_handler(channel, data): global imu_gyro, imu_mag, imu_accel, imu_quat, start_time, curr_time msg = ins_t.decode(data) imu_gyro = msg.gyro imu_mag = msg.mag imu_accel = msg.accel imu_quat = msg.quat if start_time == None: start_time = msg.utime curr_time = msg.utime
def __init__(self, channel='MICROSTRAIN_INS'):
Decoder.__init__(self, channel=channel)
from microstrain import ins_t
self.ins_t_decode_ = lambda data : ins_t.decode(data)
