diff --git a/rt_gene/CMakeLists.txt b/rt_gene/CMakeLists.txt
index 44750e1..cfdf5b8 100644
--- a/rt_gene/CMakeLists.txt
+++ b/rt_gene/CMakeLists.txt
@@ -112,7 +112,7 @@ generate_dynamic_reconfigure_options(
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES rt_gene
- CATKIN_DEPENDS rospy std_msgs geometry_msgs sensor_msgs image_geometry cv_bridge image_transport tf tf_conversions message_runtime
+ CATKIN_DEPENDS rospy std_msgs geometry_msgs sensor_msgs image_geometry cv_bridge image_transport tf tf2_ros message_runtime
# DEPENDS system_lib
)
diff --git a/rt_gene/launch/estimate_gaze.launch b/rt_gene/launch/estimate_gaze.launch
index 700eb9b..4ce9ce0 100644
--- a/rt_gene/launch/estimate_gaze.launch
+++ b/rt_gene/launch/estimate_gaze.launch
@@ -7,11 +7,11 @@
-
+
-
-
+
+
@@ -23,11 +23,12 @@
-
+
+
@@ -35,7 +36,7 @@
-
+
['model_nets/Model_allsubjects1.h5']
/
diff --git a/rt_gene/launch/start_video.launch b/rt_gene/launch/start_video.launch
index 425636e..6fcbc99 100644
--- a/rt_gene/launch/start_video.launch
+++ b/rt_gene/launch/start_video.launch
@@ -1,6 +1,6 @@
-
+
diff --git a/rt_gene/launch/start_webcam.launch b/rt_gene/launch/start_webcam.launch
index 8b22fdc..0d67924 100644
--- a/rt_gene/launch/start_webcam.launch
+++ b/rt_gene/launch/start_webcam.launch
@@ -3,7 +3,7 @@
-
+
diff --git a/rt_gene/package.xml b/rt_gene/package.xml
index dcc68c4..1cb2a5a 100644
--- a/rt_gene/package.xml
+++ b/rt_gene/package.xml
@@ -37,7 +37,7 @@
cv_bridge
image_transport
tf
- tf_conversions
+ tf2_ros
uvc_camera
dynamic_reconfigure
diff --git a/rt_gene/scripts/estimate_gaze.py b/rt_gene/scripts/estimate_gaze.py
index f034813..a69b9e6 100755
--- a/rt_gene/scripts/estimate_gaze.py
+++ b/rt_gene/scripts/estimate_gaze.py
@@ -13,13 +13,15 @@
import os
import numpy as np
from tqdm import tqdm
+import collections
import rospkg
import rospy
+from geometry_msgs.msg import TransformStamped
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
-from tf import TransformBroadcaster, TransformListener
-import tf.transformations
+import tf2_ros
+from tf import transformations
import rt_gene.gaze_tools as gaze_tools
from rt_gene.subject_ros_bridge import SubjectListBridge
@@ -33,12 +35,13 @@ def __init__(self, device_id_gaze, model_files):
self.bridge = CvBridge()
self.subjects_bridge = SubjectListBridge()
- self.tf_broadcaster = TransformBroadcaster()
- self.tf_listener = TransformListener()
+ self.tf2_broadcaster = tf2_ros.TransformBroadcaster()
+ self.tf2_buffer = tf2_ros.Buffer()
+ self.tf2_listener = tf2_ros.TransformListener(self.tf2_buffer)
self.tf_prefix = rospy.get_param("~tf_prefix", "gaze")
self.headpose_frame = self.tf_prefix + "/head_pose_estimated"
- self.ros_tf_frame = rospy.get_param("~ros_tf_frame", "/kinect2_ros_frame")
+ self.ros_tf_frame = rospy.get_param("~ros_tf_frame", "kinect2_ros_frame")
self.image_subscriber = rospy.Subscriber("/subjects/images", MSG_SubjectImagesList, self.image_callback, queue_size=3, buff_size=2 ** 24)
self.subjects_gaze_img = rospy.Publisher("/subjects/gazeimages", Image, queue_size=3)
@@ -46,6 +49,8 @@ def __init__(self, device_id_gaze, model_files):
self.visualise_eyepose = rospy.get_param("~visualise_eyepose", default=True)
self._last_time = rospy.Time().now()
+ self._freq_deque = collections.deque(maxlen=30) # average frequency statistic over roughly one second
+ self._latency_deque = collections.deque(maxlen=30)
def publish_image(self, image, image_publisher, timestamp):
"""This image publishes the `image` to the `image_publisher` with the given `timestamp`."""
@@ -68,8 +73,9 @@ def image_callback(self, subject_image_list):
valid_subject_list = []
for subject_id, s in subjects_dict.items():
try:
- (trans_head, rot_head) = self.tf_listener.lookupTransform(self.ros_tf_frame, self.headpose_frame + str(subject_id), timestamp)
- euler_angles_head = list(tf.transformations.euler_from_quaternion(rot_head))
+ transform_msg = self.tf2_buffer.lookup_transform(self.ros_tf_frame, self.headpose_frame + str(subject_id), timestamp)
+ rot_head = transform_msg.transform.rotation
+ euler_angles_head = list(transformations.euler_from_quaternion([rot_head.x, rot_head.y, rot_head.z, rot_head.w]))
euler_angles_head = gaze_tools.limit_yaw(euler_angles_head)
phi_head, theta_head = gaze_tools.get_phi_theta_from_euler(euler_angles_head)
@@ -77,7 +83,7 @@ def image_callback(self, subject_image_list):
input_r_list.append(self.input_from_image(s.right))
input_l_list.append(self.input_from_image(s.left))
valid_subject_list.append(subject_id)
- except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, tf.Exception):
+ except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException, tf2_ros.TransformException):
pass
if len(valid_subject_list) == 0:
@@ -105,21 +111,39 @@ def image_callback(self, subject_image_list):
_now = rospy.Time().now()
_freq = 1.0 / (_now - self._last_time).to_sec()
+ self._freq_deque.append(_freq)
+ self._latency_deque.append(_now.to_sec() - timestamp.to_sec())
self._last_time = _now
tqdm.write(
- '\033[2K\033[1;32mTime now: {:.2f} message color: {:.2f} diff: {:.2f}s for {} subjects {:.0f}Hz\033[0m'.format((_now.to_sec()), timestamp.to_sec(),
- _now.to_sec() - timestamp.to_sec(),
- len(valid_subject_list),
- _freq), end="\r")
+ '\033[2K\033[1;32mTime now: {:.2f} message color: {:.2f} latency: {:.2f}s for {} subject(s) {:.0f}Hz\033[0m'.format(
+ (_now.to_sec()), timestamp.to_sec(), np.mean(self._latency_deque), len(valid_subject_list), np.mean(self._freq_deque)), end="\r")
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))
+ 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
if __name__ == "__main__":
diff --git a/rt_gene/scripts/extract_landmarks_node.py b/rt_gene/scripts/extract_landmarks_node.py
index b9a28e4..da84898 100755
--- a/rt_gene/scripts/extract_landmarks_node.py
+++ b/rt_gene/scripts/extract_landmarks_node.py
@@ -13,9 +13,11 @@
from cv_bridge import CvBridge
from rt_gene.extract_landmarks_method_base import LandmarkMethodBase
from sensor_msgs.msg import Image, CameraInfo
+from geometry_msgs.msg import TransformStamped
from tqdm import tqdm
from image_geometry import PinholeCameraModel
-from tf import TransformBroadcaster, transformations
+from tf2_ros import TransformBroadcaster, TransformListener, Buffer
+from tf import transformations
from dynamic_reconfigure.server import Server
import rospy
@@ -39,9 +41,12 @@ def __init__(self, img_proc=None):
self.bridge = CvBridge()
self.__subject_bridge = SubjectListBridge()
- self.camera_frame = rospy.get_param("~camera_frame", "/kinect2_link")
+ self.camera_frame = rospy.get_param("~camera_frame", "kinect2_link")
+ self.ros_tf_frame = rospy.get_param("~ros_tf_frame", "kinect2_ros_frame")
- self.tf_broadcaster = TransformBroadcaster()
+ self.tf2_broadcaster = TransformBroadcaster()
+ self.tf2_buffer = Buffer()
+ self.tf2_listener = TransformListener(self.tf2_buffer)
self.tf_prefix = rospy.get_param("~tf_prefix", default="gaze")
self.visualise_headpose = rospy.get_param("~visualise_headpose", default=True)
@@ -108,11 +113,16 @@ def process_image(self, color_msg):
self.publish_pose(timestamp, translation_vector, head_rpy, subject_id)
if self.visualise_headpose:
- roll_pitch_yaw = gaze_tools.limit_yaw(head_rpy)
+ # pitch roll yaw
+ trans_msg = self.tf2_buffer.lookup_transform(self.ros_tf_frame, self.tf_prefix + "/head_pose_estimated" + str(subject_id), timestamp)
+ rotation = trans_msg.transform.rotation
+ euler_angles_head = list(transformations.euler_from_quaternion([rotation.x, rotation.y, rotation.z, rotation.w]))
+ euler_angles_head = gaze_tools.limit_yaw(euler_angles_head)
+
face_image_resized = cv2.resize(subject.face_color, dsize=(224, 224), interpolation=cv2.INTER_CUBIC)
final_head_pose_images.append(
- LandmarkMethodROS.visualize_headpose_result(face_image_resized, gaze_tools.get_phi_theta_from_euler(roll_pitch_yaw)))
+ LandmarkMethodROS.visualize_headpose_result(face_image_resized, gaze_tools.get_phi_theta_from_euler(euler_angles_head)))
if len(self.subject_tracker.get_tracked_elements().items()) > 0:
self.publish_subject_list(timestamp, self.subject_tracker.get_tracked_elements())
@@ -187,9 +197,29 @@ def publish_subject_list(self, timestamp, subjects):
self.subject_pub.publish(subject_list_message)
def publish_pose(self, timestamp, nose_center_3d_tf, head_rpy, subject_id):
- self.tf_broadcaster.sendTransform(nose_center_3d_tf, transformations.quaternion_from_euler(*head_rpy), timestamp,
- self.tf_prefix + "/head_pose_estimated" + str(subject_id),
- self.camera_frame)
+ t = TransformStamped()
+ t.header.frame_id = self.camera_frame
+ t.header.stamp = timestamp
+ t.child_frame_id = self.tf_prefix + "/head_pose_estimated" + str(subject_id)
+ t.transform.translation.x = nose_center_3d_tf[0]
+ t.transform.translation.y = nose_center_3d_tf[1]
+ t.transform.translation.z = nose_center_3d_tf[2]
+
+ rotation = transformations.quaternion_from_euler(*head_rpy)
+ t.transform.rotation.x = rotation[0]
+ t.transform.rotation.y = rotation[1]
+ t.transform.rotation.z = rotation[2]
+ t.transform.rotation.w = rotation[3]
+
+ try:
+ self.tf2_broadcaster.sendTransform([t])
+ except rospy.ROSException as exc:
+ if str(exc) == "publish() to a closed topic":
+ pass
+ else:
+ raise exc
+
+ self.tf2_buffer.set_transform(t, 'extract_landmarks')
def update_subject_tracker(self, color_img):
faceboxes = self.get_face_bb(color_img)