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 roslib; roslib.load_manifest('pi_tracker')
00024 import rospy
00025 import pi_tracker_lib as PTL
00026 from pi_tracker.msg import Skeleton
00027 from pi_tracker.srv import *
00028 import PyKDL as KDL
00029 from math import copysign
00030 import time
00031 
00032 class TrackerCommand():
00033     def __init__(self):
00034         rospy.init_node('tracker_command')
00035         rospy.on_shutdown(self.shutdown)
00036         
00037         # How frequently do we publish      
00038         self.rate = rospy.get_param("~command_rate", 1)
00039         rate = rospy.Rate(self.rate)
00040         
00041         # Subscribe to the skeleton topic.
00042         rospy.Subscriber('skeleton', Skeleton, self.skeleton_handler)
00043         
00044         # Store the current skeleton configuration in a local dictionary.
00045         self.skeleton = dict()
00046         self.skeleton['confidence'] = dict()
00047         self.skeleton['position'] = dict()
00048         self.skeleton['orientation'] = dict()
00049         
00050         """ Define a dictionary of gestures to look for.  Gestures are defined by the corresponding function
00051             which generally tests for the relative position of various joints. """
00052             
00053         self.gestures = {'right_foot_up': self.right_foot_up,
00054                          'left_foot_up': self.left_foot_up,
00055                          'arms_crossed': self.arms_crossed
00056                          }
00057                                     
00058         # Connect to the base controller set_command service
00059         #rospy.wait_for_service('tracker_base_controller/set_command')
00060         self.base_controller_proxy = rospy.ServiceProxy('tracker_base_controller/set_command', SetCommand, persistent=True)
00061         self.base_active = False
00062         
00063         # Connect to the joint controller set_command service
00064         #rospy.wait_for_service('tracker_joint_controller/set_command')
00065         self.joint_controller_proxy = rospy.ServiceProxy('tracker_joint_controller/set_command', SetCommand, persistent=True)
00066         self.joints_active = False
00067         
00068         # Initialize the robot in the stopped state.
00069         self.tracker_command = "STOP"
00070         
00071         rospy.loginfo("Initializing Tracker Command Node...")
00072         
00073         while not rospy.is_shutdown():                           
00074             """ Get the scale of a body dimension, in this case the shoulder width, so that we can scale
00075                 interjoint distances when computing gestures. """
00076             self.shoulder_width = PTL.get_body_dimension(self.skeleton, 'left_shoulder', 'right_shoulder', 0.4)
00077             
00078             # Compute the tracker command from the user's gesture
00079             self.tracker_command = self.get_command(self.tracker_command)
00080      
00081             rate.sleep()
00082             
00083     def right_foot_up(self):
00084         if self.confident(['right_foot', 'left_foot']):
00085             if (self.skeleton['position']['right_foot'].y() - self.skeleton['position']['left_foot'].y()) / self.shoulder_width > 0.7:
00086                 return True
00087         return False
00088             
00089     def left_foot_up(self):
00090         if self.confident(['right_foot', 'left_foot']):
00091             if (self.skeleton['position']['left_foot'].y() - self.skeleton['position']['right_foot'].y()) / self.shoulder_width > 0.7:
00092                 return True
00093         return False
00094         
00095     def arms_crossed(self):
00096         if self.confident(['left_elbow', 'right_elbow', 'left_hand', 'right_hand']):
00097             if copysign(1.0, self.skeleton['position']['left_elbow'].x() - self.skeleton['position']['right_elbow'].x()) == \
00098                  -copysign(1.0, self.skeleton['position']['left_hand'].x() - self.skeleton['position']['right_hand'].x()):
00099                 return True
00100         return False
00101     
00102     def confident(self, joints):
00103         try:
00104             for joint in joints:
00105                 if self.skeleton['confidence'][joint] < 0.5:
00106                     return False
00107             return True
00108         except:
00109             return False
00110         
00111     def get_command(self, current_command): 
00112         try:
00113             # Raise right knee to engage base controller
00114             if self.gestures['right_foot_up']():
00115                 if self.base_active:
00116                     command = 'STOP'
00117                     self.base_active = False
00118                 else:
00119                     command = 'DRIVE_BASE_FEET'
00120                     self.base_active = True
00121                 try:
00122                     self.base_controller_proxy(command)
00123                 except:
00124                     pass
00125                 
00126             # Raise left knee to engage joint teleoperation
00127             elif self.gestures['left_foot_up']():
00128                 if self.joints_active:
00129                     command = 'STOP'
00130                     self.joints_active = False
00131                 else:
00132                     command = 'TELEOP_JOINTS'
00133                     self.joints_active = True
00134                 try:
00135                     self.joint_controller_proxy(command)
00136                 except:
00137                     pass
00138                     
00139             # Cross the arms to stop the robot base 
00140             elif self.gestures['arms_crossed']():
00141                 if self.base_active:
00142                     command = "STOP"
00143                     self.base_active = False
00144                     try:
00145                         self.base_controller_proxy(command)
00146                     except:
00147                         pass
00148                 else:
00149                     command = current_command   
00150   
00151             else:
00152                 command = current_command
00153     
00154             if command != current_command:
00155                 rospy.loginfo("Gesture Command: " + command)    
00156                 
00157             return command
00158         except:
00159             return current_command
00160         
00161     def skeleton_handler(self, msg):
00162         for joint in msg.name:  
00163             self.skeleton['confidence'][joint] = msg.confidence[msg.name.index(joint)]
00164             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)
00165             self.skeleton['orientation'][joint] = msg.orientation[msg.name.index(joint)]
00166 
00167     def shutdown(self):
00168         rospy.loginfo("Shutting down Tracker Command Node.")
00169         
00170 if __name__ == '__main__':
00171     try:
00172         TrackerCommand()
00173     except rospy.ROSInterruptException:
00174         pass


pi_tracker
Author(s): Patrick Goebel
autogenerated on Tue Jan 7 2014 11:27:49