Пример #1
0
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)
Пример #2
0
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)
Пример #3
0
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)
Пример #4
0
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)
Пример #5
0
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))
Пример #6
0
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))
Пример #7
0
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)
Пример #8
0
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)
Пример #9
0
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))
Пример #10
0
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))
Пример #11
0
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))
Пример #12
0
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))
Пример #14
0
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)