Go to the documentation of this file.00001
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
00038 self.rate = rospy.get_param("~command_rate", 1)
00039 rate = rospy.Rate(self.rate)
00040
00041
00042 rospy.Subscriber('skeleton', Skeleton, self.skeleton_handler)
00043
00044
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
00059
00060 self.base_controller_proxy = rospy.ServiceProxy('tracker_base_controller/set_command', SetCommand, persistent=True)
00061 self.base_active = False
00062
00063
00064
00065 self.joint_controller_proxy = rospy.ServiceProxy('tracker_joint_controller/set_command', SetCommand, persistent=True)
00066 self.joints_active = False
00067
00068
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
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
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
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
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