def timer_callback(self):
self.poses = self.system.getDeviceToAbsoluteTrackingPose(
openvr.TrackingUniverseStanding, 0, self.poses)
########
# # ETrackedDeviceClass = ENUM_TYPE
# # TrackedDeviceClass_Invalid = ENUM_VALUE_TYPE(0)
# # TrackedDeviceClass_HMD = ENUM_VALUE_TYPE(1)
# # TrackedDeviceClass_Controller = ENUM_VALUE_TYPE(2)
# # TrackedDeviceClass_GenericTracker = ENUM_VALUE_TYPE(3)
# # TrackedDeviceClass_TrackingReference = ENUM_VALUE_TYPE(4)
# # TrackedDeviceClass_DisplayRedirect = ENUM_VALUE_TYPE(5)
# # TrackedDeviceClass_Max = ENUM_VALUE_TYPE(6)
########
# # TrackedControllerRole_LeftHand = 1, // Tracked device associated with the left hand
# # TrackedControllerRole_RightHand = 2, // Tracked device associated with the right hand
########
for idx, controller in enumerate(self.poses):
# Needed as the order of the devices may change ( based on which thing got turned on first)
if not self.system.isTrackedDeviceConnected(idx):
continue
if self.system.getTrackedDeviceClass(idx) == 1 and len(
self.devices["hmd"]) <= 1:
self.devices["hmd"].append(("hmd", controller))
elif self.system.getTrackedDeviceClass(idx) == 2 and len(
self.devices["controller"]) <= 2:
controller_role = self.system.getControllerRoleForTrackedDeviceIndex(
idx)
hand = ""
if (controller_role == 1):
hand = "LeftHand"
if controller_role == 2:
hand = "RightHand"
self.devices["controller"].append((hand, controller))
for key, device in self.devices.items():
for idx, (name, el) in enumerate(device):
if key == "controller":
result, pControllerState = self.system.getControllerState(
idx)
if result:
d = from_controller_state_to_dict(pControllerState)
name = f"{key}/{name}/trigger"
msg = Bool()
msg.data = d["trigger"] > 0.0
self.publish(name, msg, Bool)
pose = utils.convert_to_quaternion(
el.mDeviceToAbsoluteTracking)
point = Point()
point.x = pose[0][0]
point.y = pose[0][1]
point.z = pose[0][2]
time = datetime.now()
dtime = (time - self.prev_time).total_seconds()
if self.point is not None:
self.velocity.x = (point.x - self.point.x) / dtime
self.velocity.y = (point.y - self.point.y) / dtime
self.velocity.z = (point.z - self.point.z) / dtime
self.point = point
rot = Quaternion()
q1 = pyq.Quaternion(pose[1])
q1 = q1.normalised
rot.w = q1[0]
rot.x = q1[1]
rot.y = q1[2]
rot.z = q1[3]
if self.rot is not None:
diffQuater = q1 - self.rot
conjBoxQuater = q1.inverse
velQuater = ((diffQuater * 2.0) / dtime) * conjBoxQuater
self.ang_velocity.x = velQuater[1]
self.ang_velocity.y = velQuater[2]
self.ang_velocity.z = velQuater[3]
# print(self.ang_velocity)
self.rot = q1
msg = Pose()
msg.orientation = rot
msg.position = point
name = f"{key}/{name}"
self.publish(name, msg, Pose)
vel = Twist()
if self.velocity.x == 0.0 and self.velocity.y == 0.0 and self.velocity.z == 0.0 and \
self.ang_velocity.x == 0.0 and self.ang_velocity.y == 0.0 and self.ang_velocity.z == 0.0:
continue
vel.linear = self.velocity
vel.angular = self.ang_velocity
name += "/vel"
self.publish(name, vel, Twist)
