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)