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
