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_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():
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():
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.hand_direction
hand_normal_ = li.hand_normal
hand_palm_pos_ = li.hand_palm_pos
hand_pitch_ = li.hand_pitch
hand_roll_ = li.hand_roll
hand_yaw_ = li.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_
# add finger data for each finger
msg.fingers = []
finger_data = li.finger_data
for finger in finger_data:
finger_msg = Finger()
finger_msg.position.x = finger['pos'][0]
finger_msg.position.y = finger['pos'][1]
finger_msg.position.z = finger['pos'][2]
finger_msg.velocity.x = finger['vel'][0]
finger_msg.velocity.x = finger['vel'][1]
finger_msg.velocity.x = finger['vel'][2]
finger_msg.direction.x = finger['dir'][0]
finger_msg.direction.y = finger['dir'][1]
finger_msg.direction.z = finger['dir'][2]
msg.fingers.append(finger_msg)
# 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 callback_ros(data):
msg = leapros() # Get the messages from the Leap Motion
msg = data
yaw = msg.ypr.z * math.pi / 180
pitch = msg.ypr.y * math.pi / 180
roll = msg.ypr.x * math.pi / 180
sendOrientation(yaw, pitch, roll)
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(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_
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, 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 callback_ros(data):
global pub
msg = leapros()
msg = data
yaw = msg.ypr.x
pitch = msg.ypr.y
roll = msg.ypr.z
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
#Just sending Twist message
if(pitch > pitch_low_range and pitch < pitch_low_range + 30):
twist.linear.x = high_speed; twist.linear.y = 0; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
rospy.loginfo("Pitch high")
elif(pitch > pitch_high_range and pitch < pitch_high_range + 30):
twist.linear.x = low_speed; twist.linear.y = 0; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
rospy.loginfo("Pitch low")
if(roll > roll_low_range and roll < roll_low_range + 30):
twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = high_turn
rospy.loginfo("Roll left")
elif(roll > roll_high_range and roll < roll_high_range + 30):
twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = low_turn
rospy.loginfo("Roll right")
pub.publish(twist)
#rospy.loginfo(rospy.get_name() + ": Roll %s" % msg.ypr.x)
#rospy.loginfo("\n")
rospy.loginfo(rospy.get_name() + ": Pitch %s" % msg.ypr.y)
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(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():
'''
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)
rospy.init_node(NODENAME)
pub_ros = rospy.Publisher('leapmotion/data',leapros,queue_size=1)
lpub_ros = rospy.Publisher('leapmotion/left',leapros,queue_size=1)
rpub_ros = rospy.Publisher('leapmotion/right',leapros,queue_size=1)
while not rospy.is_shutdown():
hand_visible = li.get_hand_visible()
lhand_visible = li.get_left_hand_visible()
rhand_visible = li.get_right_hand_visible()
# rospy.logwarn("Hand {} Left {} Right {}".format(hand_visible,lhand_visible,rhand_visible))
if hand_visible is True:
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)
if lhand_visible is True:
lhand_direction_ = li.get_left_hand_direction()
lhand_normal_ = li.get_left_hand_normal()
lhand_palm_pos_ = li.get_left_hand_palmpos()
lhand_pitch_ = li.get_left_hand_pitch()
lhand_roll_ = li.get_left_hand_roll()
lhand_yaw_ = li.get_left_hand_yaw()
msg = leapros()
msg.direction.x = lhand_direction_[0]
msg.direction.y = lhand_direction_[1]
msg.direction.z = lhand_direction_[2]
msg.normal.x = lhand_normal_[0]
msg.normal.y = lhand_normal_[1]
msg.normal.z = lhand_normal_[2]
msg.palmpos.x = lhand_palm_pos_[0]
msg.palmpos.y = lhand_palm_pos_[1]
msg.palmpos.z = lhand_palm_pos_[2]
msg.ypr.x = lhand_yaw_
msg.ypr.y = lhand_pitch_
msg.ypr.z = lhand_roll_
fingerNames = ['thumb', 'index', 'middle', 'ring', 'pinky']
fingerPointNames = ['metacarpal', 'proximal',
'intermediate', 'distal', 'tip']
for fingerName in fingerNames:
for fingerPointName in fingerPointNames:
pos = li.get_left_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_)
lpub_ros.publish(msg)
if rhand_visible is True:
rhand_direction_ = li.get_right_hand_direction()
rhand_normal_ = li.get_right_hand_normal()
rhand_palm_pos_ = li.get_right_hand_palmpos()
rhand_pitch_ = li.get_right_hand_pitch()
rhand_roll_ = li.get_right_hand_roll()
rhand_yaw_ = li.get_right_hand_yaw()
msg = leapros()
msg.direction.x = rhand_direction_[0]
msg.direction.y = rhand_direction_[1]
msg.direction.z = rhand_direction_[2]
msg.normal.x = rhand_normal_[0]
msg.normal.y = rhand_normal_[1]
msg.normal.z = rhand_normal_[2]
msg.palmpos.x = rhand_palm_pos_[0]
msg.palmpos.y = rhand_palm_pos_[1]
msg.palmpos.z = rhand_palm_pos_[2]
msg.ypr.x = rhand_yaw_
msg.ypr.y = rhand_pitch_
msg.ypr.z = rhand_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_)
rpub_ros.publish(msg)
rospy.sleep(rospy.get_param(PARAMNAME_FREQ_ENTIRE, FREQUENCY_ROSTOPIC_DEFAULT))
def ardrone_service():
li = leap_interface.Runner()
li.setDaemon(True)
li.start()
rospy.init_node('ardrone_service')
#rospy.wait_for_service('/ardrone/setledanimation')
#led = rospy.ServiceProxy('/ardrone/setledanimation',LedAnim)
pubLand = rospy.Publisher('/ardrone/land',Empty)
pubTakeOff = rospy.Publisher('/ardrone/takeoff',Empty)
pubCommand = rospy.Publisher('/cmd_vel',Twist)
rospy.init_node('ardrone_service')
x = Empty()
vel=Twist()
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_
if hand_palm_pos_[1]>250:
pubTakeOff.publish(x)
else:
vel.linear.x=0
vel.linear.y=0
vel.linear.z=0
vel.angular.z=0
pubCommand.publish(vel)
pubLand.publish(x)
if hand_pitch_<-25:
vel.linear.x=0.45
vel.linear.y=0
vel.linear.z=0
pubCommand.publish(vel)
elif hand_pitch_>25:
vel.linear.x=-0.45
vel.linear.y=0
vel.linear.z=0
pubCommand.publish(vel)
elif hand_normal_[0]<-0.65:
vel.linear.x=0
vel.linear.y=-0.45
vel.linear.z=0
#vel.angular.z=0.5
pubCommand.publish(vel)
elif hand_normal_[0]>0.65:
vel.linear.x=0
vel.linear.y=0.45
vel.linear.z=0
#vel.angular.z=-0.5
pubCommand.publish(vel)
else:
vel.linear.x=0
vel.linear.y=0
vel.linear.z=0
vel.angular.z=0
pubCommand.publish(vel)
rospy.sleep(0.1)
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)
