def sender():
li = leap_interface.Runner()
li.start()
# pub = rospy.Publisher('leapmotion/raw',leap)
pub_ros = rospy.Publisher('leapmotion/data', leapros)
rospy.init_node('leap_pub')
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()
msg = leapros()
msg.direction.x = hand_direction_[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]
msg.ypr.x = hand_yaw_
msg.ypr.y = hand_pitch_
msg.ypr.z = hand_roll_
# 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)
# Save some CPU time, circa 100Hz publishing.
rospy.sleep(0.01)
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)
pub_cpr = rospy.Publisher('CPRMoverJointVel', JointState, queue_size=10)
#pub_cmd = rospy.Publisher('cmd_vel', Twist, queue_size=10)
rospy.init_node(NODENAME)
while not rospy.is_shutdown():
#hand_name = "Left hand" #if hand.is_left else "Right hand"
#if hand.is_valid: #hand.is_left or hand.is_right:
#hand_normal_ = li.get_hand_normal()
joint_vel = JointState()
#command = String()
#vel = Twist()
hand_palm_pos_ = li.get_hand_palmpos()
#joint_vel = JointState()
if (hand_palm_pos_[0] > 120) or (hand_palm_pos_[2] > 120.00) or (
hand_palm_pos_[0] < -120) or (hand_palm_pos_[2] < -120.00):
joint_vel.velocity = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
elif (hand_palm_pos_[1] > 160.00) & (hand_palm_pos_[1] < 200.00):
joint_vel.velocity = [
hand_palm_pos_[2] * -0.5, hand_palm_pos_[0] * -0.5,
hand_palm_pos_[0] * 0.5, 0.0, 0.0, 0.0
]
elif (hand_palm_pos_[1] < 160.00) & (hand_palm_pos_[1] > 10.00):
joint_vel.velocity = [
hand_palm_pos_[2] * -0.5, 0.0, 40.0, -40.0, 0.0, 0.0
]
elif (hand_palm_pos_[1] > 200.00):
joint_vel.velocity = [
hand_palm_pos_[2] * -0.5, 0.0, -40.0, 40.0, 0.0, 0.0
]
else:
joint_vel = JointState()
joint_vel.velocity = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
pub_cpr.publish(joint_vel)
# 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_cmd.publish(vel)
rospy.sleep(
rospy.get_param(PARAMNAME_FREQ_ENTIRE, FREQUENCY_ROSTOPIC_DEFAULT))
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()
msg = leapros()
msg.direction.x = hand_direction_[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]
msg.ypr.x = hand_yaw_
msg.ypr.y = hand_pitch_
msg.ypr.z = hand_roll_
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])
# 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))
def sender():
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)
pub_leap_pos = rospy.Publisher('/left_controller_as_posestamped',
PoseStamped,
queue_size=2)
rospy.init_node('leap_pub')
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()
msg = leapros()
# msg.direction.x = hand_direction_[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]
msg.ypr.x = hand_yaw_
msg.ypr.y = hand_pitch_
msg.ypr.z = hand_roll_
ps = PoseStamped()
ps.header.frame_id = "map"
ps.header.stamp = rospy.Time.now()
ps.pose.position.x = -msg.palmpos.z / 1000
ps.pose.position.y = -msg.palmpos.x / 1000
ps.pose.position.z = msg.palmpos.y / 1000
quaternion = tf.transformations.quaternion_from_euler(
-msg.ypr.z, -msg.ypr.x, msg.ypr.y)
ps.pose.orientation.x = quaternion[0]
ps.pose.orientation.y = quaternion[1]
ps.pose.orientation.z = quaternion[2]
ps.pose.orientation.w = quaternion[3]
pub_leap_pos.publish(ps)
# Save some CPU time, circa 100Hz publishing.
rospy.sleep(0.01)
def sender():
li = leap_interface.Runner()
li.setDaemon(True)
li.start()
# pub = rospy.Publisher('leapmotion/raw',leap)
# pub_ros = rospy.Publisher('leapmotion/data',leapros)
rospy.init_node('leap_skeleton_pub')
while not rospy.is_shutdown():
timenow = rospy.Time.now()
if li.listener.left_hand:
broadcast_hand(li.listener.left_hand, "left_hand", timenow)
if li.listener.right_hand:
broadcast_hand(li.listener.right_hand, "right_hand", timenow)
# save some CPU time, circa 100Hz publishing.
rospy.sleep(0.01)
def catchHandData():
li = leap_interface.Runner()
li.setDaemon(True)
li.start()
# pub = rospy.Publisher('leapmotion/raw',leap)
# pub_ros = rospy.Publisher('leapmotion/data',leapros)
rospy.init_node('leap_skeleton_pub')
while not rospy.is_shutdown():
timenow=rospy.Time.now()
if li.listener.left_hand:
analyzeHandData(li.listener.left_hand)
else if li.listener.right_hand:
analyzeHandData(li.listener.right_hand)
# save some CPU time, circa 100Hz publishing.
rospy.sleep(0.01)
def sender(freq=FREQUENCY_ROSTOPIC_DEFAULT):
'''
@param freq: Frequency to publish sensed info as ROS message
'''
rospy.set_param(PARAMNAME_FREQ, freq)
rospy.loginfo("Parameter set on server: PARAMNAME_FREQ={}, freq={}".format(
rospy.get_param(PARAMNAME_FREQ, FREQUENCY_ROSTOPIC_DEFAULT), freq))
li = leap_interface.Runner()
li.setDaemon(True)
li.start()
# pub = rospy.Publisher('leapmotion/raw',leap)
pub_ros = rospy.Publisher('leapmotion/data', leapros)
rospy.init_node('leap_pub')
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()
msg = leapros()
msg.direction.x = hand_direction_[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]
msg.ypr.x = hand_yaw_
msg.ypr.y = hand_pitch_
msg.ypr.z = hand_roll_
# 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,
FREQUENCY_ROSTOPIC_DEFAULT))
def sender():
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()
control=0
pub_ros = rospy.Publisher('leapmotion/data',leapros,queue_size=10)
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()
msg = leapros()
msg.direction.x = hand_direction_[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]
msg.ypr.x = hand_yaw_
msg.ypr.y = hand_pitch_
msg.ypr.z = hand_roll_
#arm data
elbow_pos = li.get_elbow_position()
wrist_pos = li.get_wrist_position()
arm_direc = li.get_arm_direction()
msg.elbow_position.x = elbow_pos[0]
msg.elbow_position.y = elbow_pos[1]
msg.elbow_position.z = elbow_pos[2]
msg.wrist_position.x = wrist_pos[0]
msg.wrist_position.y = wrist_pos[1]
msg.wrist_position.z = wrist_pos[2]
msg.arm_direccion.x = arm_direc[0]
msg.arm_direccion.y = arm_direc[1]
msg.arm_direccion.z = arm_direc[2]
msg.time = li.get_time_from_init()
print 'time: %f'%(msg.time)
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])
pub_ros.publish(msg)
rospy.sleep(rospy.get_param(PARAMNAME_FREQ_ENTIRE, FREQUENCY_ROSTOPIC_DEFAULT))
def sender():
li = leap_interface.Runner()
li.setDaemon(True)
li.start()
# pub = rospy.Publisher('leapmotion/raw',leap)
pub_ros = rospy.Publisher('leapmotion/data',leapros)
cmd_pub = rospy.Publisher('cmd_vel_mux/input/teleop', Twist, queue_size=5)
rospy.init_node('leap_pub')
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()
is_hand_ = li.get_is_hand()
msg = leapros()
msg.direction.x = hand_direction_[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]
msg.ypr.x = hand_yaw_
msg.ypr.y = hand_pitch_
msg.ypr.z = hand_roll_
moveBindings = {
'n':(1,0),
'ne':(1,-1),
'w':(0,1),
'e':(0,-1),
'nw':(1,1),
's':(-1,0),
'se':(-1,1),
'sw':(-1,-1),
}
#TODO: Adjust movement parameters
if is_hand_ == True:
print "THERE IS A HAND"
if msg.ypr.y < .2 and msg.ypr.y >-.2: #PITCH FOR LINEAR MOVEMENT
print "NO LINEAR MOVEMENT"
x = 0
elif msg.ypr.y > .2:
print "FORWARD"
x = 1
elif msg.ypr.y <-.2:
print "BACKWARD"
x = -1
if msg.ypr.x < .2 and msg.ypr.x >-.2: #ROLL FOR ANGULAR MOVEMENT
print "NO ANGULAR MOVEMENT"
th = 0
elif msg.ypr.x > .2:
print "RIGHT"
th = -1
elif msg.ypr.x <-.2:
print "LEFT"
th = 1
x_move = .1 * x
th_move = .1 * th
twist = Twist()
twist.linear.x = x_move; twist.linear.y = 0; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th_move
cmd_pub.publish(twist)
else:
twist = Twist()
twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
cmd_pub.publish(twist)
# 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)
# Save some CPU time, circa 100Hz publishing.
rospy.sleep(0.01)
