If you're using openni_tracker
, the general way to do this would be to create a tf listener, and in some sort of loop use the listener to lookup the transform between the joints that you're interested in. This method will return a vector and a quaternion representing the transform between the two frames. If all you're interested in is distance, then you can simply calculate that using the transformation vector. Here's a simple example adapted from the previously-linked tf listener tutorial:
#!/usr/bin/env python
import rospy
import tf
import geometry_msgs.msg
import numpy as np
if __name__ == '__main__':
rospy.init_node('tf_lookup_example')
listener = tf.TransformListener()
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
try:
(trans,rot) = listener.lookupTransform('/right_hand_1', '/left_hand_1', rospy.Time(0))
rospy.loginfo("Distance between the hands is = {0:f}".format(np.linalg.norm(trans)))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
rate.sleep()
The only key to making this example work is ensuring that your tf tree is fully connected. My version of openni_tracker
uses, by default, openni_depth_frame
as the frame it produces transforms in, but the openni.launch
file uses camera
as its default name to define all frames and topics. This inconsistency can be fixed, for example, by running openni.launch
in the following way:
roslaunch openni_launch openni.launch camera:=openni
Then openni_tracker
could be started with just rosrun openni_tracker openni_tracker