0
The question is all about importing a module called leap_interface.py to send.py and using a of the functions defined in leap_interface. I ask this question, because the leap_interface module is a bit complicated. It contains multi classes.
First, I defined function get_extended_fingers(self) in class LeapInterface in leap_interface module as below:
class LeapInterface(Leap.Listener):
def on_init(self, controller):
...
def on_disconnect(self, controller):
...
def on_exit(self, controller):
print "Exited Leap Motion Controller"
def on_frame(self, controller):
# Get the most recent frame and report some basic information
frame = controller.frame()
# Number of extended fingers Steven
def get_extended_fingers(self):
#print "extendedfingers: %d" % len(extended_finger_list)
return len(extended_finger_list)
Secondly, I want to use this function as an if condition in the file sender.py:
if li.get_extended_fingers < 2:
msg.direction.x = hand_direction_[0]# - initial_pose[0]
msg.direction.y = hand_direction_[1]
msg.direction.z = hand_direction_[2]
print "works1"
if li.get_extended_fingers > 4:
msg.ypr.x = hand_yaw_
msg.ypr.y = hand_pitch_
msg.ypr.z = hand_roll_
print "works2"
However, my solution doesn't work properly. It my terminal, it keeps printing works2, no matter how many extended fingers I show in the field of view of my leap motion sensor. Can you please tell me what I did wrong?
leap_interface.py
import sys
import time
import threading
import Leap
from Leap import CircleGesture, KeyTapGesture, ScreenTapGesture, SwipeGesture
class LeapFinger():
def __init__(self, finger=None):
self.boneNames = ['metacarpal',
'proximal',
'intermediate',
'distal']
for boneName in self.boneNames:
setattr(self, boneName, [0.0, 0.0, 0.0])
self.tip = [0.0, 0.0, 0.0]
self.leapBoneNames = [Leap.Bone.TYPE_METACARPAL,
Leap.Bone.TYPE_PROXIMAL,
Leap.Bone.TYPE_INTERMEDIATE,
Leap.Bone.TYPE_DISTAL]
if finger is not None:
self.importFinger(finger)
def importFinger(self, finger):
for boneName in self.boneNames:
# Get the base of each bone
bone = finger.bone(getattr(Leap.Bone, 'TYPE_%s' % boneName.upper()))
setattr(self, boneName, bone.prev_joint.to_float_array())
# For the tip, get the end of the distal bone
self.tip = finger.bone(Leap.Bone.TYPE_DISTAL).next_joint.to_float_array()
class LeapInterface(Leap.Listener):
def on_init(self, controller):
# These variables as probably not thread safe
# TODO: Make thread safe ;)
self.hand = [0,0,0]
self.right_hand = False
self.left_hand = False
self.hand_direction = [0,0,0]
self.hand_normal = [0,0,0]
self.hand_palm_pos = [0,0,0]
# Velocity Steven
self.hand_palm_vel = [0,0,0]
# Finger tip velocity Steven
self.index_tip_vel = [0,0,0]
self.hand_pitch = 0.0
self.hand_yaw = 0.0
self.hand_roll = 0.0
self.fingerNames = ['thumb', 'index', 'middle', 'ring', 'pinky']
for fingerName in self.fingerNames:
setattr(self, fingerName, LeapFinger())
print "Initialized Leap Motion Device"
def on_connect(self, controller):
print "Connected to Leap Motion Controller"
# Enable gestures
controller.enable_gesture(Leap.Gesture.TYPE_CIRCLE);
controller.enable_gesture(Leap.Gesture.TYPE_KEY_TAP);
controller.enable_gesture(Leap.Gesture.TYPE_SCREEN_TAP);
controller.enable_gesture(Leap.Gesture.TYPE_SWIPE);
def on_disconnect(self, controller):
# Note: not dispatched when running in a debugger.
print "Disconnected Leap Motion"
def on_exit(self, controller):
print "Exited Leap Motion Controller"
def on_frame(self, controller):
# Get the most recent frame and report some basic information
frame = controller.frame()
'''print "Frame id: %d, timestamp: %d, hands: %d, fingers: %d, tools: %d, gestures: %d" % (
frame.id, frame.timestamp, len(frame.hands), len(frame.fingers), len(frame.tools), len(frame.gestures()))'''
if not frame.hands.is_empty: #recently changed in API
# Get the first hand
#we are seeking one left and one right hands
there_is_right_hand=False
there_is_left_hand=False
for hand in frame.hands:
if hand.is_right:
there_is_right_hand=True
self.right_hand=hand
elif hand.is_left:
there_is_left_hand=True
self.left_hand=hand
if not there_is_right_hand:
self.right_hand=False
if not there_is_left_hand:
self.left_hand=False
self.hand = frame.hands[0] #old way
# Check if the hand has any fingers
fingers = self.hand.fingers
extended_finger_list = fingers.extended()
#print type(extended_finger_list)
print "extendedfingers: %d" % len(extended_finger_list)
if not fingers.is_empty:
for fingerName in self.fingerNames:
#finger = fingers.finger_type(Leap.Finger.TYPE_THUMB)[0]
#self.thumb.importFinger(finger)
# finger_type(type)Returns a FingerList object containing only the specified finger types, i.e. only thumbs, or only index fingers, etc. Steven
finger = fingers.finger_type(getattr(Leap.Finger, 'TYPE_%s' % fingerName.upper()))[0]
getattr(self, fingerName).importFinger(finger)
# Get the hand's sphere radius and palm position
# print "Hand sphere radius: %f mm, palm position: %s" % (self.hand.sphere_radius, hand.palm_position)
# Get the hand's normal vector and direction
normal = self.hand.palm_normal
direction = self.hand.direction
pos = self.hand.palm_position
# How about the velocity of hand Steven
vel = self.hand.palm_velocity
#Pointable represents fingers here
pointable = self.hand.pointables[0]
tip_vel = pointable.tip_velocity
self.hand_direction[0] = direction.x
self.hand_direction[1] = direction.y
self.hand_direction[2] = direction.z
self.hand_normal[0] = normal.x
self.hand_normal[1] = normal.y
self.hand_normal[2] = normal.z
self.hand_palm_pos[0] = pos.x
self.hand_palm_pos[1] = pos.y
self.hand_palm_pos[2] = pos.z
self.hand_pitch = direction.pitch * Leap.RAD_TO_DEG
self.hand_yaw = normal.yaw * Leap.RAD_TO_DEG
self.hand_roll = direction.roll * Leap.RAD_TO_DEG
# Velocity Steven
self.hand_palm_vel[0] = vel.x
self.hand_palm_vel[1] = vel.y
self.hand_palm_vel[2] = vel.z
# Pointables Steven
self.index_tip_vel[0] = tip_vel.x
self.index_tip_vel[1] = tip_vel.y
self.index_tip_vel[2] = tip_vel.z
# use the following if condition to clear the data after moveing hand out of the FOV Steven
if frame.hands.is_empty:
self.hand_direction[0] = 0
self.hand_direction[1] = 0
self.hand_direction[2] = 0
self.hand_normal[0] = 0
self.hand_normal[1] = 0
self.hand_normal[2] = 0
self.hand_palm_pos[0] = 0
self.hand_palm_pos[1] = 0
self.hand_palm_pos[2] = 0
self.hand_pitch = 0
self.hand_yaw = 0
self.hand_roll = 0
# Velocity
self.hand_palm_vel[0] = 0
self.hand_palm_vel[1] = 0
self.hand_palm_vel[2] = 0
# Pointables Steven
self.index_tip_vel[0] = 0
self.index_tip_vel[1] = 0
self.index_tip_vel[2] = 0
def get_hand_direction(self):
return self.hand_direction
def get_hand_normal(self):
return self.hand_normal
def get_hand_palmpos(self):
return self.hand_palm_pos
def get_hand_yaw(self):
return self.hand_yaw
def get_hand_pitch(self):
return self.hand_pitch
def get_hand_roll(self):
return self.hand_roll
def get_finger_point(self, fingerName, fingerPointName):
return getattr(getattr(self, fingerName), fingerPointName)
# Velocity Steven
def get_hand_palmvel(self):
return self.hand_palm_vel
# Finger tip velocity Steven
def get_index_tipvel(self):
return self.index_tip_vel
# Number of extended fingers Steven
def get_extended_fingers(self):
#print "extendedfingers: %d" % len(extended_finger_list)
return len(extended_finger_list)
class Runner(threading.Thread):
def __init__(self,arg=None):
threading.Thread.__init__(self)
self.arg=arg
self.listener = LeapInterface()
self.controller = Leap.Controller()
self.controller.add_listener(self.listener)
def __del__(self):
self.controller.remove_listener(self.listener)
def get_hand_direction(self):
return self.listener.get_hand_direction()
def get_hand_normal(self):
return self.listener.get_hand_normal()
def get_hand_palmpos(self):
return self.listener.get_hand_palmpos()
def get_hand_roll(self):
return self.listener.get_hand_roll()
def get_hand_pitch(self):
return self.listener.get_hand_pitch()
def get_hand_yaw(self):
return self.listener.get_hand_yaw()
def get_finger_point(self, fingerName, fingerPointName):
return self.listener.get_finger_point(fingerName, fingerPointName)
# Velocity
def get_hand_palmvel(self):
return self.listener.get_hand_palmvel()
# Finger tip velocity Steven
def get_index_tipvel(self):
return self.listener.get_index_tipvel()
# Number of extended fingers Steven
def get_extended_fingers(self):
return len(extended_finger_list)
#print "extendedfingers: %d" % len(extended_finger_list)
#return self.listener.get_extended_fingers()
def run (self):
while True:
# Save some CPU time
time.sleep(0.001)
sender.py
#!/usr/bin/env python
import argparse
import rospy
import leap_interface
from leap_motion.msg import leap
from leap_motion.msg import leapros
'''FREQUENCY_ROSTOPIC_DEFAULT = 0.01(original rate)'''
FREQUENCY_ROSTOPIC_DEFAULT = 1
NODENAME = 'leap_pub'
PARAMNAME_FREQ = 'freq'
PARAMNAME_FREQ_ENTIRE = '/' + NODENAME + '/' + PARAMNAME_FREQ
def sender():
'''
This method publishes the data defined in leapros.msg to /leapmotion/data
'''
rospy.loginfo("Parameter set on server: PARAMNAME_FREQ=
{}".format(rospy.get_param(PARAMNAME_FREQ_ENTIRE, FREQUENCY_ROSTOPIC_DEFAULT)))
li = leap_interface.Runner()
li.setDaemon(True)
li.start()
# pub = rospy.Publisher('leapmotion/raw',leap)
pub_ros = rospy.Publisher('leapmotion/data',leapros, queue_size=2)
rospy.init_node(NODENAME)
while not rospy.is_shutdown():
hand_direction_ = li.get_hand_direction()
hand_normal_ = li.get_hand_normal()
hand_palm_pos_ = li.get_hand_palmpos()
hand_pitch_ = li.get_hand_pitch()
hand_roll_ = li.get_hand_roll()
hand_yaw_ = li.get_hand_yaw()
# Velocity Steven
hand_palm_vel_ = li.get_hand_palmvel()
index_tip_vel_ = li.get_index_tipvel()
msg = leapros()
#frame.hands[0].fingers.extended()
#print "extendedfingers: %d" % li.get_extended_fingers()
if li.get_extended_fingers < 2:
msg.direction.x = hand_direction_[0]# - initial_pose[0]
msg.direction.y = hand_direction_[1]
msg.direction.z = hand_direction_[2]
msg.normal.x = hand_normal_[0]
msg.normal.y = hand_normal_[1]
msg.normal.z = hand_normal_[2]
msg.palmpos.x = hand_palm_pos_[0]
msg.palmpos.y = hand_palm_pos_[1]
msg.palmpos.z = hand_palm_pos_[2]
# Velocity Steven
msg.palmvel.x = hand_palm_vel_[0]
msg.palmvel.y = hand_palm_vel_[1]
msg.palmvel.z = hand_palm_vel_[2]
# Pointable/Finger tip velocity Steven
msg.tipvel.x = index_tip_vel_[0]
msg.tipvel.y = index_tip_vel_[1]
msg.tipvel.z = index_tip_vel_[2]
print "works1"
#fingerNames = ['thumb', 'index', 'middle', 'ring', 'pinky']
#fingerPointNames = ['metacarpal', 'proximal',
#'intermediate', 'distal', 'tip']
#for fingerName in fingerNames:
#for fingerPointName in fingerPointNames:
#pos = li.get_finger_point(fingerName, fingerPointName)
#for iDim, dimName in enumerate(['x', 'y', 'z']):
#setattr(getattr(msg, '%s_%s' % (fingerName, fingerPointName)),
#dimName, pos[iDim])
'''if frame.hands.is_empty:
msg.index_tip.x = 0
msg.index_tip.y = 0
msg.index_tip.z = 0'''
if li.get_extended_fingers > 4:
msg.ypr.x = hand_yaw_
msg.ypr.y = hand_pitch_
msg.ypr.z = hand_roll_
print "works2"
# We don't publish native data types, see ROS best practices
# pub.publish(hand_direction=hand_direction_,hand_normal = hand_normal_, hand_palm_pos = hand_palm_pos_, hand_pitch = hand_pitch_, hand_roll = hand_roll_, hand_yaw = hand_yaw_)
pub_ros.publish(msg)
rospy.sleep(rospy.get_param(PARAMNAME_FREQ_ENTIRE, FREQUENCY_ROSTOPIC_DEFAULT))
if __name__ == '__main__':
try:
sender()
except rospy.ROSInterruptException:
pass
↧