Go to the documentation of this file.00001
00002
00003 """
00004 Control a differential drive robot base using a skeleton tracker such
00005 as the OpenNI tracker package in conjunction 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 from geometry_msgs.msg import Twist
00026 import pi_tracker_lib as PTL
00027 from pi_tracker.msg import Skeleton
00028 from pi_tracker.srv import *
00029 import PyKDL as KDL
00030 from math import atan2, sqrt, copysign, pi
00031
00032 class TrackerBaseController():
00033 def __init__(self):
00034 rospy.init_node('tracker_base_controller')
00035 rospy.on_shutdown(self.shutdown)
00036
00037 rospy.loginfo("Initializing Base Controller Node...")
00038
00039 self.rate = rospy.get_param('~base_controller_rate', 1)
00040 rate = rospy.Rate(self.rate)
00041
00042 self.max_drive_speed = rospy.get_param('~max_drive_speed', 0.3)
00043 self.max_rotation_speed = rospy.get_param('~max_rotation_speed', 0.5)
00044 self.base_control_side = rospy.get_param('~base_control_side', "right")
00045 self.holonomic = rospy.get_param('~holonomic', False)
00046 self.scale_drive_speed = rospy.get_param('~scale_drive_speed', 1.0)
00047 self.scale_rotation_speed = rospy.get_param('~scale_rotation_speed', 1.0)
00048 self.reverse_rotation = rospy.get_param('~reverse_rotation', False)
00049
00050 self.HALF_PI = pi / 2.0
00051
00052
00053 rospy.Subscriber('skeleton', Skeleton, self.skeleton_handler)
00054
00055
00056 self.skeleton = dict()
00057 self.skeleton['confidence'] = dict()
00058 self.skeleton['position'] = dict()
00059 self.skeleton['orientation'] = dict()
00060
00061
00062 self.set_state = rospy.Service('~set_command', SetCommand, self.set_command_callback)
00063
00064
00065 self.cmd_vel = Twist()
00066
00067
00068 self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist)
00069
00070
00071 self.shoulder_width = PTL.get_body_dimension(self.skeleton, 'left_shoulder', 'right_shoulder', 0.4)
00072 self.count = 1.0
00073
00074 drive_vector = dict()
00075 drive_side_test = dict()
00076
00077
00078 self.tracker_command = "STOP"
00079
00080 while not rospy.is_shutdown():
00081
00082 if self.tracker_command == 'STOP' or (self.skeleton['confidence']['left_hand'] < 0.5 and self.skeleton['confidence']['right_hand'] < 0.5):
00083 self.cmd_vel.linear.x = 0.0
00084 self.cmd_vel.linear.y = 0.0
00085 self.cmd_vel.angular.z = 0.0
00086
00087 elif self.tracker_command == 'DRIVE_BASE_HANDS':
00088 self.drive_base_hands()
00089
00090 elif self.tracker_command == 'DRIVE_BASE_FEET':
00091 self.drive_base_feet()
00092
00093 else:
00094 pass
00095
00096
00097 self.cmd_vel.linear.x = self.cmd_vel.linear.x * self.scale_drive_speed
00098 self.cmd_vel.angular.z = self.cmd_vel.angular.z * self.scale_rotation_speed
00099 if self.reverse_rotation:
00100 self.cmd_vel.angular.z = -self.cmd_vel.angular.z
00101
00102
00103
00104
00105
00106
00107 self.cmd_vel_pub.publish(self.cmd_vel)
00108
00109 rate.sleep()
00110
00111 def drive_base_hands(self):
00112
00113 if self.base_control_side == "right":
00114 side = "right"
00115 else:
00116 side = "left"
00117
00118
00119 self.upper_arm_length = PTL.get_body_dimension(self.skeleton, side + '_shoulder', side + '_elbow', 0.25)
00120
00121
00122 self.forearm_length = PTL.get_body_dimension(self.skeleton, side + '_elbow', side + '_hand', 0.3)
00123
00124 if not self.confident([side + '_shoulder', side + '_hand']):
00125 return
00126
00127 drive_vector = self.skeleton['position'][side + '_shoulder'] - self.skeleton['position'][side + '_hand']
00128
00129 """ Split out the x, y motion components. The origin is set near the position of the elbow when the arm
00130 is held straigth out. """
00131 self.cmd_vel.linear.x = self.max_drive_speed * (drive_vector.z() - self.upper_arm_length ) / self.upper_arm_length
00132 self.cmd_vel.linear.y = -self.max_drive_speed * drive_vector.x() / self.upper_arm_length
00133
00134
00135 if self.holonomic:
00136 try:
00137
00138 try:
00139 torso_quaternion = self.skeleton['orientation']['torso']
00140 torso_rpy = torso_quaternion.GetRPY()
00141 torso_angle = torso_rpy[1]
00142 except:
00143 torso_angle = 0
00144 if abs(torso_angle) > 0.6:
00145 self.cmd_vel.angular.z = self.max_rotation_speed * torso_angle * 1.5 / self.HALF_PI
00146 self.cmd_vel.linear.x = 0.0
00147 self.cmd_vel.linear.y = 0.0
00148 else:
00149 self.cmd_vel.angular.z = 0.0
00150 except:
00151 pass
00152
00153 else:
00154
00155 try:
00156 drive_speed = self.cmd_vel.linear.x
00157
00158 drive_angle = atan2(self.cmd_vel.linear.y, self.cmd_vel.linear.x)
00159
00160 self.cmd_vel.linear.y = 0
00161
00162 if abs(drive_speed) < 0.07:
00163 self.cmd_vel.linear.x = 0.0
00164 self.cmd_vel.angular.z = 0.0
00165 return
00166
00167
00168
00169 if (drive_speed < 0 and abs(drive_angle) > 2.5) or (drive_angle > -0.7 and drive_angle < 1.5):
00170 self.cmd_vel.angular.z = 0.0
00171 self.cmd_vel.linear.x = drive_speed
00172
00173 elif drive_angle < -0.7:
00174 self.cmd_vel.angular.z = self.max_rotation_speed
00175 self.cmd_vel.linear.x = 0.0
00176
00177 elif drive_angle > 1.5:
00178 self.cmd_vel.angular.z = -self.max_rotation_speed
00179 self.cmd_vel.linear.x = 0.0
00180
00181 else:
00182 pass
00183 except:
00184 pass
00185
00186 def drive_base_feet(self):
00187
00188 if self.base_control_side == "right":
00189 drive_vector = self.skeleton['position']['left_foot'] - self.skeleton['position']['right_foot']
00190 else:
00191 drive_vector = self.skeleton['position']['right_foot'] - self.skeleton['position']['left_foot']
00192
00193 drive_angle = 0.0
00194 drive_speed = 0.0
00195 self.cmd_vel.linear.y = 0
00196
00197 drive_speed = sqrt(drive_vector.x() * drive_vector.x() + drive_vector.z() * drive_vector.z())
00198 drive_angle = atan2(-drive_vector.x(), drive_vector.z())
00199 drive_angle = drive_angle - self.HALF_PI
00200
00201 drive_speed = drive_speed / (self.shoulder_width * 4)
00202
00203 if drive_speed > self.max_drive_speed:
00204 drive_speed = self.max_drive_speed
00205 elif drive_speed < 0.05:
00206 drive_speed = 0.0
00207
00208 drive_speed = copysign(drive_speed, drive_vector.z())
00209
00210 rospy.loginfo('FEET SPD: ' + str(drive_speed) + ' ANG: ' + str(drive_angle))
00211
00212 if drive_angle > -0.7 and drive_angle < 0.0:
00213 self.cmd_vel.linear.x = 0.0
00214 if abs(drive_speed) > 0.15:
00215 self.cmd_vel.angular.z = -self.max_rotation_speed
00216 else:
00217 self.cmd_vel.angular.z = 0
00218
00219 elif drive_angle > 0.0 and drive_angle < 0.7:
00220 self.cmd_vel.linear.x = 0.0
00221 if abs(drive_speed) > 0.15:
00222 self.cmd_vel.angular.z = self.max_rotation_speed
00223 else:
00224 self.cmd_vel.angular.z = 0
00225
00226 else:
00227 self.cmd_vel.linear.x = drive_speed
00228 self.cmd_vel.angular.z = 0.0
00229
00230 def confident(self, joints):
00231 try:
00232 for joint in joints:
00233 if self.skeleton['confidence'][joint] < 0.5:
00234 return False
00235 return True
00236 except:
00237 return False
00238
00239 def set_command_callback(self, req):
00240 self.tracker_command = req.command
00241 return SetCommandResponse()
00242
00243 def skeleton_handler(self, msg):
00244 for joint in msg.name:
00245 self.skeleton['confidence'][joint] = msg.confidence[msg.name.index(joint)]
00246 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)
00247 self.skeleton['orientation'][joint] = KDL.Rotation.Quaternion(msg.orientation[msg.name.index(joint)].x, msg.orientation[msg.name.index(joint)].y, msg.orientation[msg.name.index(joint)].z, msg.orientation[msg.name.index(joint)].w)
00248
00249 def shutdown(self):
00250 rospy.loginfo('Shutting down Tracker Base Controller Node.')
00251
00252 if __name__ == '__main__':
00253 try:
00254 TrackerBaseController()
00255 except rospy.ROSInterruptException:
00256 pass