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)