1 #!/usr/bin/env python
   2 
   3 
   4 #libraries for kinect
   5 import rospy
   6 from skeleton_markers.msg import Skeleton
   7 from visualization_msgs.msg import Marker
   8 from geometry_msgs.msg import Point
   9 from geometry_msgs.msg import Twist
  10 from std_msgs.msg import String
  11 
  12 # Import sthe messages we're interested in sending and receiving
  13 from geometry_msgs.msg import Twist      # for sending commands to the drone
  14 from std_msgs.msg import Empty           # for land/takeoff/emergency
  15 from ardrone_autonomy.msg import Navdata # for receiving navdata feedback
  16 
  17 # An enumeration of Drone Statuses
  18 from drone_status import DroneStatus
  19 
  20 p = Point()
  21 
  22 y_left =0.00
  23 y_right=0.00
  24 x_right=0.00
  25 z_right=0.00
  26 y_torso=0.00
  27 x_torso=0.00
  28 z_torso=0.00
  29 pose=Twist()
  30 st=String()
  31 lt=String()
  32 rt=String()
  33 def callback(msg):
  34    for joint in msg.name:           
  35             global st
  36             st=msg.name[0]
  37            
  38             p = msg.position[msg.name.index(joint)]
  39             global x_left
  40             global y_left
  41             global x_right
  42             global y_right
  43             global x_torso
  44             global y_torso
  45             if joint=="left_hand":
  46               y_left=p.y
  47               x_left=p.x
  48               z_left=p.z
  49               rospy.loginfo(joint)
  50             elif joint=="right_hand":
  51               y_right=p.y
  52               x_right=p.x
  53               z_right=p.z
  54               rospy.loginfo(joint)
  55             
  56             elif joint=="torso":
  57               x_torso=p.x
  58               z_torso=p.z
  59               y_torso=p.y
  60               rospy.loginfo(joint)
  61 
  62 
  63             x_right = x_right - x_torso
  64             rospy.loginfo(x_right)
  65 
  66    #rospy.loginfo(m)
  67 
  68 
  69 def kinect_drone():
  70   pubLand=rospy.Publisher('/ardrone/land',Empty)
  71   pubReset   = rospy.Publisher('/ardrone/reset',Empty)
  72   pubTakeoff= rospy.Publisher('/ardrone/takeoff',Empty)
  73   pubCommand = rospy.Publisher('/cmd_vel',Twist)
  74   pub=rospy.Publisher('/turtle1/cmd_vel',Twist)
  75 
  76   rospy.Subscriber("/skeleton", Skeleton, callback)
  77   rospy.init_node('kinect_drone')
  78   
  79   
  80   
  81   
  82   global count
  83   global x_left
  84   global y_left
  85   global x_right
  86   global y_right
  87   global x_torso
  88   global y_torso
  89   lt="left_hand"
  90   
  91   r = rospy.Rate(10) # 10hz
  92   while not rospy.is_shutdown():
  93       
  94        uniq=Empty()
  95        vel=Twist()
  96        if y_left>0.10:      
  97             #left hand to take off   
  98             rospy.loginfo(p.y)
  99             pubTakeoff.publish(uniq)  
 100        
 101        else:
 102             #left hand to land
 103             pubLand.publish(uniq)  
 104        
 105        if y_right>0.44:
 106             #right hand pitch backward
 107             vel.linear.x=-0.15
 108             vel.linear.y=0
 109             vel.linear.z=0
 110             vel.angular.z=0
 111             pubCommand.publish(vel)
 112             pub.publish(vel)       
 113                 
 114        elif y_right<-0.04:
 115             #right hand pitch forward
 116             vel.linear.x=0.15
 117             vel.linear.y=0
 118             vel.linear.z=0
 119             vel.angular.z=0
 120             pubCommand.publish(vel)
 121             pub.publish(vel)
 122        
 123        elif x_right>0.60 :
 124 
 125                 #right hand roll right 
 126                 vel.linear.x=0
 127                 vel.linear.y=-0.20
 128                 vel.linear.z=0
 129                 vel.angular.z=0
 130                 pubCommand.publish(vel)
 131                 pub.publish(vel)
 132 
 133        elif x_right< 0.10:
 134                 #right hand roll left
 135                 vel.linear.x=0
 136                 vel.linear.y=0.20
 137                 vel.linear.z=0
 138                 vel.angular.z=0
 139                 pubCommand.publish(vel)
 140                 pub.publish(vel)
 141        
 142        else:
 143            # no motion no command
 144            vel.linear.x=0
 145            vel.linear.y=0
 146            vel.linear.z=0
 147            vel.angular.z=0
 148            pubCommand.publish(vel)        
 149            pub.publish(vel)
 150        
 151        
 152        rospy.sleep(0.1)
 153 
 154 
 155 if __name__ == '__main__':
 156     try:
 157         kinect_drone()
 158     except rospy.ROSInterruptException: 
 159         pass