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)