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)