tracker_command.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004     Send gesture-based commands to other nodes using a skeleton tracker such
00005     as the OpenNI tracker package in junction with a Kinect RGB-D camera.
00006     
00007     Created for the Pi Robot Project: http://www.pirobot.org
00008     Copyright (c) 2011 Patrick Goebel.  All rights reserved.
00009 
00010     This program is free software; you can redistribute it and/or modify
00011     it under the terms of the GNU General Public License as published by
00012     the Free Software Foundation; either version 2 of the License, or
00013     (at your option) any later version.
00014     
00015     This program is distributed in the hope that it will be useful,
00016     but WITHOUT ANY WARRANTY; without even the implied warranty of
00017     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018     GNU General Public License for more details at:
00019     
00020     http://www.gnu.org/licenses/gpl.html
00021 """
00022 
00023 import rospy
00024 import pi_tracker_lib as PTL
00025 from skeleton_markers.msg import Skeleton
00026 from pi_tracker.srv import *
00027 import PyKDL as KDL
00028 from math import copysign
00029 import time
00030 
00031 class TrackerCommand():
00032     def __init__(self):
00033         rospy.init_node('tracker_command')
00034         rospy.on_shutdown(self.shutdown)
00035         
00036         # How frequently do we publish      
00037         self.rate = rospy.get_param("~command_rate", 1)
00038         rate = rospy.Rate(self.rate)
00039         
00040         # Subscribe to the skeleton topic.
00041         rospy.Subscriber('skeleton', Skeleton, self.skeleton_handler)
00042         
00043         # Store the current skeleton configuration in a local dictionary.
00044         self.skeleton = dict()
00045         self.skeleton['confidence'] = dict()
00046         self.skeleton['position'] = dict()
00047         self.skeleton['orientation'] = dict()
00048         
00049         """ Define a dictionary of gestures to look for.  Gestures are defined by the corresponding function
00050             which generally tests for the relative position of various joints. """
00051             
00052         self.gestures = {'right_foot_up': self.right_foot_up,
00053                          'left_foot_up': self.left_foot_up,
00054                          'arms_crossed': self.arms_crossed
00055                          }
00056                                     
00057         # Connect to the base controller set_command service
00058         #rospy.wait_for_service('tracker_base_controller/set_command')
00059         self.base_controller_proxy = rospy.ServiceProxy('tracker_base_controller/set_command', SetCommand, persistent=True)
00060         self.base_active = False
00061         
00062         # Connect to the joint controller set_command service
00063         #rospy.wait_for_service('tracker_joint_controller/set_command')
00064         self.joint_controller_proxy = rospy.ServiceProxy('tracker_joint_controller/set_command', SetCommand, persistent=True)
00065         self.joints_active = False
00066         
00067         # Initialize the robot in the stopped state.
00068         self.tracker_command = "STOP"
00069         
00070         rospy.loginfo("Initializing Tracker Command Node...")
00071         
00072         while not rospy.is_shutdown():                           
00073             """ Get the scale of a body dimension, in this case the shoulder width, so that we can scale
00074                 interjoint distances when computing gestures. """
00075             self.shoulder_width = PTL.get_body_dimension(self.skeleton, 'left_shoulder', 'right_shoulder', 0.4)
00076             
00077             # Compute the tracker command from the user's gesture
00078             self.tracker_command = self.get_command(self.tracker_command)
00079      
00080             rate.sleep()
00081             
00082     def right_foot_up(self):
00083         if self.confident(['right_foot', 'left_foot']):
00084             if (self.skeleton['position']['right_foot'].y() - self.skeleton['position']['left_foot'].y()) / self.shoulder_width > 0.7:
00085                 return True
00086         return False
00087             
00088     def left_foot_up(self):
00089         if self.confident(['right_foot', 'left_foot']):
00090             if (self.skeleton['position']['left_foot'].y() - self.skeleton['position']['right_foot'].y()) / self.shoulder_width > 0.7:
00091                 return True
00092         return False
00093         
00094     def arms_crossed(self):
00095         if self.confident(['left_elbow', 'right_elbow', 'left_hand', 'right_hand']):
00096             if copysign(1.0, self.skeleton['position']['left_elbow'].x() - self.skeleton['position']['right_elbow'].x()) == \
00097                  -copysign(1.0, self.skeleton['position']['left_hand'].x() - self.skeleton['position']['right_hand'].x()):
00098                 return True
00099         return False
00100     
00101     def confident(self, joints):
00102         try:
00103             for joint in joints:
00104                 if self.skeleton['confidence'][joint] < 0.5:
00105                     return False
00106             return True
00107         except:
00108             return False
00109         
00110     def get_command(self, current_command): 
00111         try:
00112             # Raise right knee to engage base controller
00113             if self.gestures['right_foot_up']():
00114                 if self.base_active:
00115                     command = 'STOP'
00116                     self.base_active = False
00117                 else:
00118                     command = 'DRIVE_BASE_FEET'
00119                     self.base_active = True
00120                 try:
00121                     self.base_controller_proxy(command)
00122                 except:
00123                     pass
00124                 
00125             # Raise left knee to engage joint teleoperation
00126             elif self.gestures['left_foot_up']():
00127                 if self.joints_active:
00128                     command = 'STOP'
00129                     self.joints_active = False
00130                 else:
00131                     command = 'TELEOP_JOINTS'
00132                     self.joints_active = True
00133                 try:
00134                     self.joint_controller_proxy(command)
00135                 except:
00136                     pass
00137                     
00138             # Cross the arms to stop the robot base 
00139             elif self.gestures['arms_crossed']():
00140                 if self.base_active:
00141                     command = "STOP"
00142                     self.base_active = False
00143                     try:
00144                         self.base_controller_proxy(command)
00145                     except:
00146                         pass
00147                 else:
00148                     command = current_command   
00149   
00150             else:
00151                 command = current_command
00152     
00153             if command != current_command:
00154                 rospy.loginfo("Gesture Command: " + command)    
00155                 
00156             return command
00157         except:
00158             return current_command
00159         
00160     def skeleton_handler(self, msg):
00161         for joint in msg.name:  
00162             self.skeleton['confidence'][joint] = msg.confidence[msg.name.index(joint)]
00163             self.skeleton['position'][joint] = KDL.Vector(msg.position[msg.name.index(joint)].x, msg.position[msg.name.index(joint)].y, msg.position[msg.name.index(joint)].z)
00164             self.skeleton['orientation'][joint] = msg.orientation[msg.name.index(joint)]
00165 
00166     def shutdown(self):
00167         rospy.loginfo("Shutting down Tracker Command Node.")
00168         
00169 if __name__ == '__main__':
00170     try:
00171         TrackerCommand()
00172     except rospy.ROSInterruptException:
00173         pass


pi_tracker
Author(s): Patrick Goebel
autogenerated on Fri Aug 28 2015 12:00:12