def publish_gaze(self, est_gaze, msg_stamp, subject_id):
"""Publish the gaze vector as a PointStamped."""
theta_gaze = est_gaze[0]
phi_gaze = est_gaze[1]
euler_angle_gaze = gaze_tools.get_euler_from_phi_theta(
phi_gaze, theta_gaze)
quaternion_gaze = transformations.quaternion_from_euler(
*euler_angle_gaze)
t = TransformStamped()
t.header.stamp = msg_stamp
t.header.frame_id = self.headpose_frame + str(subject_id)
t.child_frame_id = self.tf_prefix + "/world_gaze" + str(subject_id)
t.transform.translation.x = 0
t.transform.translation.y = 0
t.transform.translation.z = 0.05 # publish it 5cm above the head pose's origin (nose tip)
t.transform.rotation.x = quaternion_gaze[0]
t.transform.rotation.y = quaternion_gaze[1]
t.transform.rotation.z = quaternion_gaze[2]
t.transform.rotation.w = quaternion_gaze[3]
try:
self.tf2_broadcaster.sendTransform([t])
except rospy.ROSException as exc:
if str(exc) == "publish() to a closed topic":
pass
else:
raise exc
def publish_gaze(self, est_gaze, msg_stamp, subject_id):
"""Publish the gaze vector as a PointStamped."""
theta_gaze = est_gaze[0]
phi_gaze = est_gaze[1]
euler_angle_gaze = gaze_tools.get_euler_from_phi_theta(phi_gaze, theta_gaze)
quaternion_gaze = tf.transformations.quaternion_from_euler(*euler_angle_gaze)
self.tf_broadcaster.sendTransform((0, 0, 0.05), # publish it 5cm above the head pose's origin (nose tip)
quaternion_gaze, msg_stamp, self.tf_prefix + "/world_gaze" + str(subject_id), self.headpose_frame + str(subject_id))