def vicon_handler(holder,channel,data): msg = body_t.decode(data) t = msg.utime position = msg.trans s,x,y,z = msg.quat a = 2*atan2(z,s) if a<0: a = 2*pi+a state = np.matrix([position[0]*1000.0,position[1]*1000.0,a]).transpose() #[mm,mm,rad,ms since epoch] #print 'handeled:', state holder.setState(state,t)
def vicon_handler(self, channel, data): self.viconmsg = body_t.decode(data) self.drawParticles(offset=self.viconmsg.pos) # plot true orientation of IMU via vicon vicon_orient = self.viconmsg.orientation w = vicon_orient[0] x = vicon_orient[1] y = vicon_orient[2] z = vicon_orient[3] # quaternion to rotation matrix wRv = np.zeros([3, 3], dtype=np.double) wRv[0, 0] = 1 - 2 * y ** 2 - 2 * z ** 2 wRv[0, 1] = 2 * x * y - 2 * z * w wRv[0, 2] = 2 * x * z + 2 * y * w wRv[1, 0] = 2 * x * y + 2 * z * w wRv[1, 1] = 1 - 2 * x ** 2 - 2 * z ** 2 wRv[1, 2] = 2 * y * z - 2 * x * w wRv[2, 0] = 2 * x * z - 2 * y * w wRv[2, 1] = 2 * y * z + 2 * x * w wRv[2, 2] = 1 - 2 * x ** 2 - 2 * y ** 2 # rotational transformation between microstrain and vicon (collocated) vRm = np.zeros([3, 3], dtype=np.double) vRm[0, 0] = 0.747293477674224 vRm[0, 1] = 0.663765523047521 vRm[0, 2] = 0.031109301487093 vRm[1, 0] = 0.663949387400485 vRm[1, 1] = -0.747757418253684 vRm[1, 2] = 0.005482190903960 vRm[2, 0] = 0.026901100276478 vRm[2, 1] = 0.016558196158918 vRm[2, 2] = -0.999500953948458 wRm = np.dot(wRv, vRm) self.vicon_lRb = wRm p = Pose.from_rigid_transform(2, RigidTransform.from_Rt(wRm, self.viconmsg.pos)) publish_pose_list("VICONpose", [p], frame_id="origin") # drawing true max as point in new image and in local frame if self.imumsg != None: im2 = np.zeros([180, 360]) PV = np.array([[30], [30], [4]]) - np.array(self.viconmsg.pos)[:, np.newaxis] self.vicon_max = PV / (np.linalg.norm(PV)) publish_cloud("particle_vicon_max", self.vicon_max.T + self.viconmsg.pos, c="g", frame_id="origin") # lRb_val = self.prtObj.lRb() # bRl_val = lRb_val.T # dot_prod = np.dot(bRl_val, self.vicon_max) # use IMU local-to-body transform dot_prod = np.dot(self.vicon_lRb.T, self.vicon_max) # use vicon local-to-body transform az_el = self.prtObj.peRb_pt(dot_prod) if az_el[0] < 0: az_el[0] = az_el[0] + 2 * np.pi # print az_el*180/np.pi self.gen_heatmap(az_el[0], az_el[1]) cv2.circle(im2, (int(az_el[0] * 180 / np.pi), int(az_el[1] * 180 / np.pi)), 5, (255, 0, 0)) cv2.imshow("img2", im2) cv2.waitKey(1)
def vicon_handler(self,channel, data): self.viconmsg = body_t.decode(data) self.drawParticles(offset=self.viconmsg.pos) # plot true orientation of IMU via vicon vicon_orient = self.viconmsg.orientation w = vicon_orient[0] x = vicon_orient[1] y = vicon_orient[2] z = vicon_orient[3] # quaternion to rotation matrix wRv = np.zeros([3,3],dtype=np.double) wRv[0,0] = 1 - 2*y**2 - 2*z**2 wRv[0,1] = 2*x*y - 2*z*w wRv[0,2] = 2*x*z + 2*y*w wRv[1,0] = 2*x*y + 2*z*w wRv[1,1] = 1 - 2*x**2 - 2*z**2 wRv[1,2] = 2*y*z - 2*x*w wRv[2,0] = 2*x*z - 2*y*w wRv[2,1] = 2*y*z + 2*x*w wRv[2,2] = 1 - 2*x**2 - 2*y**2 # rotational transformation between microstrain and vicon (collocated) vRm = np.zeros([3,3],dtype=np.double) vRm[0,0] = 0.747293477674224 vRm[0,1] = 0.663765523047521 vRm[0,2] = 0.031109301487093 vRm[1,0] = 0.663949387400485 vRm[1,1] = -0.747757418253684 vRm[1,2] = 0.005482190903960 vRm[2,0] = 0.026901100276478 vRm[2,1] = 0.016558196158918 vRm[2,2] = -0.999500953948458 wRm = np.dot(wRv, vRm) self.vicon_lRb = wRm p = Pose.from_rigid_transform(2, RigidTransform.from_Rt(wRm,self.viconmsg.pos)) publish_pose_list('VICONpose', [p], frame_id='origin') #drawing true max as point in new image and in local frame if self.imumsg != None: im2 = np.zeros([180,360]) PV = np.array([[30],[30],[4]]) - np.array(self.viconmsg.pos)[:,np.newaxis] self.vicon_max = PV/(np.linalg.norm(PV)) publish_cloud('particle_vicon_max', self.vicon_max.T+self.viconmsg.pos, c='g', frame_id='origin') # lRb_val = self.prtObj.lRb() # bRl_val = lRb_val.T # dot_prod = np.dot(bRl_val, self.vicon_max) # use IMU local-to-body transform dot_prod = np.dot(self.vicon_lRb.T, self.vicon_max) # use vicon local-to-body transform az_el = self.prtObj.peRb_pt(dot_prod) if az_el[0] < 0: az_el[0] = az_el[0]+2*np.pi # print az_el*180/np.pi self.gen_heatmap(az_el[0], az_el[1]) cv2.circle(im2, (int(az_el[0]*180/np.pi),int(az_el[1]*180/np.pi)), 5, (255,0,0)) cv2.imshow('img2', im2) cv2.waitKey(1)
def vicon_handler(channel, data): global vicon_pos, vicon_orient msg = body_t.decode(data) vicon_pos = msg.pos vicon_orient = msg.orientation