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 drawParticles2d(self,
V,
frame_id='origin',
name='particles',
lname='yaw',
c=['b'],
lc=['r'],
ll=0.05):
LD = []
if len(V) < 1:
print 'drawParticles2d -- no data? skip'
return
d, N = V[0].shape
l = len(V)
for v in V:
Dv = np.vstack((v[0:2, :], np.zeros((1, N)))).T
LD.append(Dv)
publish_cloud(name, LD, c=c, frame_id=frame_id)
if d > 2:
count = -1
DV = np.zeros((d, l * N))
DT = np.zeros((d, l * N))
for v in V:
tt = np.zeros((d, N))
for i in range(0, N):
t = Pose.from_rigid_transform(
0,
RigidTransform(xyzw=Quaternion.from_roll_pitch_yaw(
0, 0, v[2, i]).to_xyzw(),
tvec=[v[0, i], v[1, i], 0]))
pert = Pose.from_rigid_transform(
0, RigidTransform(tvec=[ll, 0, 0]))
tv = (t.oplus(pert)).tvec
tt[0:2, i] = tv[0:2]
count += 1
DT[0:d, (count * N):((count + 1) * N)] = tt
Dv = np.vstack((v[0:2, :], np.zeros((1, N))))
DV[0:d, (count * N):((count + 1) * N)] = Dv
publish_line_segments(lname,
DV.T,
DT.T,
c=lc[0],
frame_id=frame_id)
