skeleton_sender.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 __author__ = 'Igor Zubrycki'
00004 
00005 import rospy
00006 import leap_interface
00007 import tf
00008 import sys
00009 import Leap
00010 import math
00011 import roslib
00012 import rospy
00013 from std_msgs.msg import String
00014 from std_msgs.msg import Float64
00015 import std_msgs.msg
00016 import sensor_msgs.msg
00017 import actionlib_msgs.msg
00018 import trajectory_msgs.msg
00019 import collections
00020 import scipy.signal as signal
00021 import geometry_msgs.msg
00022 import PyKDL
00023 
00024 finger_names=["thumb","index","middle","ring","pinky"]
00025 bones_names=["metacarpal","proximal","intermediate","distal"]
00026 
00027 def make_kdl_frame(leap_basis_matrix,leap_position_vector,is_left=False):
00028     # Makes kdl frame from Leap Motion matrix and vector formats
00029     
00030     if is_left:
00031        basis=([-el for el in leap_basis_matrix.x_basis.to_float_array()]+
00032        leap_basis_matrix.y_basis.to_float_array()+
00033        leap_basis_matrix.z_basis.to_float_array())
00034        
00035     else:
00036        basis=leap_basis_matrix.to_array_3x3() 
00037     rotation_mat=PyKDL.Rotation(*basis).Inverse()
00038     # rotation_mat=PyKDL.Rotation(*[1,0,0,0,1,0,0,0,1])#.Inverse()
00039     placement=PyKDL.Vector(*leap_position_vector.to_float_array())/1000.0 #to m
00040     return PyKDL.Frame(rotation_mat,placement)
00041     
00042 def relative_frame(frame_A,frame_B):
00043 
00044     return (frame_A.Inverse())*frame_B
00045 
00046 
00047 hand_ground_tf=PyKDL.Frame(PyKDL.Rotation.EulerZYX(0, 0, math.pi/2.0),PyKDL.Vector(0,0,0))
00048 
00049 def make_tf_dict(hand,hand_name):
00050     
00051     hand_dict={}
00052     # hand_ground_tf=
00053     hand_dict["hand_ground"]=["ground",hand_ground_tf]
00054     
00055             
00056     hand_tf=make_kdl_frame(hand.basis,hand.palm_position,hand.is_left)
00057     # hand_tf=make_kdl_frame(hand.basis,hand.palm_position)
00058     hand_dict[hand_name]=["hand_ground",hand_tf]
00059     for finger in hand.fingers:
00060         finger_name=hand_name+"_"+finger_names[finger.type()]
00061         
00062         prev_bone_name=hand_name
00063         for num in range(0,4):
00064             
00065             bone=finger.bone(num)
00066 
00067             bone_absolute=make_kdl_frame(bone.basis,bone.prev_joint,hand.is_left)
00068             if num==0:
00069                 bone_tf=relative_frame(hand_tf,bone_absolute)
00070             else:
00071                 bone_tf=relative_frame(prev_bone_absolute,bone_absolute)
00072 
00073             bone_name=finger_name+"_"+bones_names[num]
00074             hand_dict[bone_name]=[prev_bone_name,bone_tf]
00075             
00076             prev_bone_name=bone_name
00077             prev_bone_absolute=bone_absolute
00078             
00079         tip=PyKDL.Frame(PyKDL.Rotation(1,0,0, 0,1,0, 0,0,1),PyKDL.Vector(0,0,-bone.length/1000.0))
00080         hand_dict[finger_name+"_tip"]=[prev_bone_name,tip]    
00081     return hand_dict    
00082      # now sending to ROS       
00083         
00084 
00085 def broadcast_hand(hand,hand_name,timenow):
00086     hand_dict=make_tf_dict(hand,hand_name)
00087     br = tf.TransformBroadcaster()
00088 
00089 
00090     for tf_name,tf_array in hand_dict.items():
00091         tf_matrix=tf_array[1]
00092         tf_prev_name=tf_array[0]
00093         br.sendTransform(tf_matrix.p,tf_matrix.M.GetQuaternion(),timenow,tf_name,tf_prev_name)
00094 
00095 def sender():
00096     li = leap_interface.Runner()
00097     li.setDaemon(True)
00098     li.start()
00099     # pub     = rospy.Publisher('leapmotion/raw',leap)
00100     # pub_ros   = rospy.Publisher('leapmotion/data',leapros)
00101     rospy.init_node('leap_skeleton_pub')
00102 
00103     
00104     while not rospy.is_shutdown():
00105         timenow=rospy.Time.now()
00106         if li.listener.left_hand:
00107             broadcast_hand(li.listener.left_hand,"left_hand",timenow)
00108         if li.listener.right_hand:
00109             broadcast_hand(li.listener.right_hand,"right_hand",timenow)
00110 
00111         # save some CPU time, circa 100Hz publishing.
00112         rospy.sleep(0.01)
00113 
00114 
00115 if __name__ == '__main__':
00116     try:
00117         sender()
00118     except rospy.ROSInterruptException:
00119         pass


leap_motion
Author(s): Florian Lier
autogenerated on Tue Jan 17 2017 21:06:16