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


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