pi_tracker: pi_tracker: tracker_base_controller.py Source File

00001 #!/usr/bin/env python
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         # Subscribe to the skeleton topic.
00053         rospy.Subscriber('skeleton', Skeleton, self.skeleton_handler)
00054         
00055         # Store the current skeleton configuration in a local dictionary.
00056         self.skeleton = dict()
00057         self.skeleton['confidence'] = dict()
00058         self.skeleton['position'] = dict()
00059         self.skeleton['orientation'] = dict()
00060         
00061         # Set up the base controller service.
00062         self.set_state = rospy.Service('~set_command', SetCommand, self.set_command_callback)
00063         
00064         # We will control the robot base with a Twist message.
00065         self.cmd_vel = Twist()
00066         
00067         # And publish the command on the /cmd_vel topic
00068         self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist)
00069         
00070         # Keep track of the average width of the shoulders to scale gestures to body size.
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         # Initialize the robot in the stopped state.
00078         self.tracker_command = "STOP"
00079         
00080         while not rospy.is_shutdown():        
00081             # If we receive the STOP command, or we lose sight of both hands then stop all motion.
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             # Some robots might require a scale factor on the speeds and a reversal of the rotation direction.
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             #rospy.loginfo('DRIVE: ' + str(self.cmd_vel.linear.x) + ' ROTATE: ' + str(self.cmd_vel.angular.z))
00103 
00104             # Publish the drive command on the /cmd_vel topic.
00105             #rospy.loginfo('X: ' + str(self.cmd_vel.linear.x) + ' Y: ' + str(self.cmd_vel.linear.y) + ' Z: ' + str(self.cmd_vel.angular.z))               
00106 
00107             self.cmd_vel_pub.publish(self.cmd_vel)
00108             
00109             rate.sleep()
00110             
00111     def drive_base_hands(self):
00112         # Compute the drive command based on the position of the control hand relative to the shoulder.
00113         if self.base_control_side == "right":
00114             side = "right"
00115         else:
00116             side = "left"
00117         
00118         # Use the upper arm length to scale the gesture.
00119         self.upper_arm_length = PTL.get_body_dimension(self.skeleton, side + '_shoulder', side + '_elbow', 0.25)
00120         
00121         # Use the forearm length to compute the origin of the control plane.
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         # For holonomic control, use torso rotation to rotate in place.
00135         if self.holonomic:
00136             try:
00137                 # Check for torso rotation to see if we want to rotate in place.
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             # For non-holonomic control, segment the control plane into forward/backward and rotation motions.
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                 #rospy.loginfo("DRIVE SPEED: " + str(drive_speed) + " ANGLE: " + str(drive_angle))
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         # Compute the drive command based on the position of the legs relative to the torso.
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