def __init__(self): rospy.init_node('tracker_command') rospy.on_shutdown(self.shutdown) # How frequently do we publish self.rate = rospy.get_param("~command_rate", 1) rate = rospy.Rate(self.rate) # Subscribe to the skeleton topic. rospy.Subscriber('skeleton', Skeleton, self.skeleton_handler) # Store the current skeleton configuration in a local dictionary. self.skeleton = dict() self.skeleton['confidence'] = dict() self.skeleton['position'] = dict() self.skeleton['orientation'] = dict() """ Define a dictionary of gestures to look for. Gestures are defined by the corresponding function which generally tests for the relative position of various joints. """ self.gestures = { 'right_foot_up': self.right_foot_up, 'left_foot_up': self.left_foot_up, 'arms_crossed': self.arms_crossed } # Connect to the base controller set_command service #rospy.wait_for_service('tracker_base_controller/set_command') self.base_controller_proxy = rospy.ServiceProxy( 'tracker_base_controller/set_command', SetCommand, persistent=True) self.base_active = False # Connect to the joint controller set_command service #rospy.wait_for_service('tracker_joint_controller/set_command') self.joint_controller_proxy = rospy.ServiceProxy( 'tracker_joint_controller/set_command', SetCommand, persistent=True) self.joints_active = False # Initialize the robot in the stopped state. self.tracker_command = "STOP" rospy.loginfo("Initializing Tracker Command Node...") while not rospy.is_shutdown(): """ Get the scale of a body dimension, in this case the shoulder width, so that we can scale interjoint distances when computing gestures. """ self.shoulder_width = PTL.get_body_dimension( self.skeleton, 'left_shoulder', 'right_shoulder', 0.4) # Compute the tracker command from the user's gesture self.tracker_command = self.get_command(self.tracker_command) rate.sleep()
def __init__(self): rospy.init_node('tracker_command') rospy.on_shutdown(self.shutdown) # How frequently do we publish self.rate = rospy.get_param("~command_rate", 1) rate = rospy.Rate(self.rate) # Subscribe to the skeleton topic. rospy.Subscriber('skeleton', Skeleton, self.skeleton_handler) # Store the current skeleton configuration in a local dictionary. self.skeleton = dict() self.skeleton['confidence'] = dict() self.skeleton['position'] = dict() self.skeleton['orientation'] = dict() """ Define a dictionary of gestures to look for. Gestures are defined by the corresponding function which generally tests for the relative position of various joints. """ self.gestures = {'right_foot_up': self.right_foot_up, 'left_foot_up': self.left_foot_up, 'arms_crossed': self.arms_crossed } # Connect to the base controller set_command service #rospy.wait_for_service('tracker_base_controller/set_command') self.base_controller_proxy = rospy.ServiceProxy('tracker_base_controller/set_command', SetCommand, persistent=True) self.base_active = False # Connect to the joint controller set_command service #rospy.wait_for_service('tracker_joint_controller/set_command') self.joint_controller_proxy = rospy.ServiceProxy('tracker_joint_controller/set_command', SetCommand, persistent=True) self.joints_active = False # Initialize the robot in the stopped state. self.tracker_command = "STOP" rospy.loginfo("Initializing Tracker Command Node...") while not rospy.is_shutdown(): """ Get the scale of a body dimension, in this case the shoulder width, so that we can scale interjoint distances when computing gestures. """ self.shoulder_width = PTL.get_body_dimension(self.skeleton, 'left_shoulder', 'right_shoulder', 0.4) # Compute the tracker command from the user's gesture self.tracker_command = self.get_command(self.tracker_command) rate.sleep()
def __init__(self): rospy.init_node('tracker_base_controller') rospy.on_shutdown(self.shutdown) rospy.loginfo("Initializing Base Controller Node...") self.rate = rospy.get_param('~base_controller_rate', 1) rate = rospy.Rate(self.rate) self.max_drive_speed = rospy.get_param('~max_drive_speed', 0.3) self.max_rotation_speed = rospy.get_param('~max_rotation_speed', 0.5) self.base_control_side = rospy.get_param('~base_control_side', "right") self.holonomic = rospy.get_param('~holonomic', False) self.scale_drive_speed = rospy.get_param('~scale_drive_speed', 1.0) self.scale_rotation_speed = rospy.get_param('~scale_rotation_speed', 1.0) self.reverse_rotation = rospy.get_param('~reverse_rotation', False) self.HALF_PI = pi / 2.0 # Subscribe to the skeleton topic. rospy.Subscriber('skeleton', Skeleton, self.skeleton_handler) # Store the current skeleton configuration in a local dictionary. self.skeleton = dict() self.skeleton['confidence'] = dict() self.skeleton['position'] = dict() self.skeleton['orientation'] = dict() # Set up the base controller service. self.set_state = rospy.Service('~set_command', SetCommand, self.set_command_callback) # We will control the robot base with a Twist message. self.cmd_vel = Twist() # And publish the command on the /cmd_vel topic self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist) # Keep track of the average width of the shoulders to scale gestures to body size. self.shoulder_width = PTL.get_body_dimension(self.skeleton, 'left_shoulder', 'right_shoulder', 0.4) self.count = 1.0 drive_vector = dict() drive_side_test = dict() # Initialize the robot in the stopped state. self.tracker_command = "STOP" while not rospy.is_shutdown(): # If we receive the STOP command, or we lose sight of both hands then stop all motion. if self.tracker_command == 'STOP' or ( self.skeleton['confidence']['left_hand'] < 0.5 and self.skeleton['confidence']['right_hand'] < 0.5): self.cmd_vel.linear.x = 0.0 self.cmd_vel.linear.y = 0.0 self.cmd_vel.angular.z = 0.0 elif self.tracker_command == 'DRIVE_BASE_HANDS': self.drive_base_hands() elif self.tracker_command == 'DRIVE_BASE_FEET': self.drive_base_feet() else: pass # Some robots might require a scale factor on the speeds and a reversal of the rotation direction. self.cmd_vel.linear.x = self.cmd_vel.linear.x * self.scale_drive_speed self.cmd_vel.angular.z = self.cmd_vel.angular.z * self.scale_rotation_speed if self.reverse_rotation: self.cmd_vel.angular.z = -self.cmd_vel.angular.z #rospy.loginfo('DRIVE: ' + str(self.cmd_vel.linear.x) + ' ROTATE: ' + str(self.cmd_vel.angular.z)) # Publish the drive command on the /cmd_vel topic. #rospy.loginfo('X: ' + str(self.cmd_vel.linear.x) + ' Y: ' + str(self.cmd_vel.linear.y) + ' Z: ' + str(self.cmd_vel.angular.z)) self.cmd_vel_pub.publish(self.cmd_vel) rate.sleep()
def drive_base_hands(self): # Compute the drive command based on the position of the control hand relative to the shoulder. if self.base_control_side == "right": side = "right" else: side = "left" # Use the upper arm length to scale the gesture. self.upper_arm_length = PTL.get_body_dimension(self.skeleton, side + '_shoulder', side + '_elbow', 0.25) # Use the forearm length to compute the origin of the control plane. self.forearm_length = PTL.get_body_dimension(self.skeleton, side + '_elbow', side + '_hand', 0.3) if not self.confident([side + '_shoulder', side + '_hand']): return drive_vector = self.skeleton['position'][ side + '_shoulder'] - self.skeleton['position'][side + '_hand'] """ Split out the x, y motion components. The origin is set near the position of the elbow when the arm is held straigth out. """ self.cmd_vel.linear.x = self.max_drive_speed * ( drive_vector.z() - self.upper_arm_length) / self.upper_arm_length self.cmd_vel.linear.y = -self.max_drive_speed * drive_vector.x( ) / self.upper_arm_length # For holonomic control, use torso rotation to rotate in place. if self.holonomic: try: # Check for torso rotation to see if we want to rotate in place. try: torso_quaternion = self.skeleton['orientation']['torso'] torso_rpy = torso_quaternion.GetRPY() torso_angle = torso_rpy[1] except: torso_angle = 0 if abs(torso_angle) > 0.6: self.cmd_vel.angular.z = self.max_rotation_speed * torso_angle * 1.5 / self.HALF_PI self.cmd_vel.linear.x = 0.0 self.cmd_vel.linear.y = 0.0 else: self.cmd_vel.angular.z = 0.0 except: pass else: # For non-holonomic control, segment the control plane into forward/backward and rotation motions. try: drive_speed = self.cmd_vel.linear.x drive_angle = atan2(self.cmd_vel.linear.y, self.cmd_vel.linear.x) self.cmd_vel.linear.y = 0 if abs(drive_speed) < 0.07: self.cmd_vel.linear.x = 0.0 self.cmd_vel.angular.z = 0.0 return #rospy.loginfo("DRIVE SPEED: " + str(drive_speed) + " ANGLE: " + str(drive_angle)) if (drive_speed < 0 and abs(drive_angle) > 2.5) or (drive_angle > -0.7 and drive_angle < 1.5): self.cmd_vel.angular.z = 0.0 self.cmd_vel.linear.x = drive_speed elif drive_angle < -0.7: self.cmd_vel.angular.z = self.max_rotation_speed self.cmd_vel.linear.x = 0.0 elif drive_angle > 1.5: self.cmd_vel.angular.z = -self.max_rotation_speed self.cmd_vel.linear.x = 0.0 else: pass except: pass
def __init__(self): rospy.init_node('tracker_base_controller') rospy.on_shutdown(self.shutdown) rospy.loginfo("Initializing Base Controller Node...") self.rate = rospy.get_param('~base_controller_rate', 1) rate = rospy.Rate(self.rate) self.max_drive_speed = rospy.get_param('~max_drive_speed', 0.3) self.max_rotation_speed = rospy.get_param('~max_rotation_speed', 0.5) self.base_control_side = rospy.get_param('~base_control_side', "right") self.holonomic = rospy.get_param('~holonomic', False) self.scale_drive_speed = rospy.get_param('~scale_drive_speed', 1.0) self.scale_rotation_speed = rospy.get_param('~scale_rotation_speed', 1.0) self.reverse_rotation = rospy.get_param('~reverse_rotation', False) self.HALF_PI = pi / 2.0 # Subscribe to the skeleton topic. rospy.Subscriber('skeleton', Skeleton, self.skeleton_handler) # Store the current skeleton configuration in a local dictionary. self.skeleton = dict() self.skeleton['confidence'] = dict() self.skeleton['position'] = dict() self.skeleton['orientation'] = dict() # Set up the base controller service. self.set_state = rospy.Service('~set_command', SetCommand, self.set_command_callback) # We will control the robot base with a Twist message. self.cmd_vel = Twist() # And publish the command on the /cmd_vel topic self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist) # Keep track of the average width of the shoulders to scale gestures to body size. self.shoulder_width = PTL.get_body_dimension(self.skeleton, 'left_shoulder', 'right_shoulder', 0.4) self.count = 1.0 drive_vector = dict() drive_side_test = dict() # Initialize the robot in the stopped state. self.tracker_command = "STOP" while not rospy.is_shutdown(): # If we receive the STOP command, or we lose sight of both hands then stop all motion. if self.tracker_command == 'STOP' or (self.skeleton['confidence']['left_hand'] < 0.5 and self.skeleton['confidence']['right_hand'] < 0.5): self.cmd_vel.linear.x = 0.0 self.cmd_vel.linear.y = 0.0 self.cmd_vel.angular.z = 0.0 elif self.tracker_command == 'DRIVE_BASE_HANDS': self.drive_base_hands() elif self.tracker_command == 'DRIVE_BASE_FEET': self.drive_base_feet() else: pass # Some robots might require a scale factor on the speeds and a reversal of the rotation direction. self.cmd_vel.linear.x = self.cmd_vel.linear.x * self.scale_drive_speed self.cmd_vel.angular.z = self.cmd_vel.angular.z * self.scale_rotation_speed if self.reverse_rotation: self.cmd_vel.angular.z = -self.cmd_vel.angular.z #rospy.loginfo('DRIVE: ' + str(self.cmd_vel.linear.x) + ' ROTATE: ' + str(self.cmd_vel.angular.z)) # Publish the drive command on the /cmd_vel topic. #rospy.loginfo('X: ' + str(self.cmd_vel.linear.x) + ' Y: ' + str(self.cmd_vel.linear.y) + ' Z: ' + str(self.cmd_vel.angular.z)) self.cmd_vel_pub.publish(self.cmd_vel) rate.sleep()
def drive_base_hands(self): # Compute the drive command based on the position of the control hand relative to the shoulder. if self.base_control_side == "right": side = "right" else: side = "left" # Use the upper arm length to scale the gesture. self.upper_arm_length = PTL.get_body_dimension(self.skeleton, side + '_shoulder', side + '_elbow', 0.25) # Use the forearm length to compute the origin of the control plane. self.forearm_length = PTL.get_body_dimension(self.skeleton, side + '_elbow', side + '_hand', 0.3) if not self.confident([side + '_shoulder', side + '_hand']): return drive_vector = self.skeleton['position'][side + '_shoulder'] - self.skeleton['position'][side + '_hand'] """ Split out the x, y motion components. The origin is set near the position of the elbow when the arm is held straigth out. """ self.cmd_vel.linear.x = self.max_drive_speed * (drive_vector.z() - self.upper_arm_length ) / self.upper_arm_length self.cmd_vel.linear.y = -self.max_drive_speed * drive_vector.x() / self.upper_arm_length # For holonomic control, use torso rotation to rotate in place. if self.holonomic: try: # Check for torso rotation to see if we want to rotate in place. try: torso_quaternion = self.skeleton['orientation']['torso'] torso_rpy = torso_quaternion.GetRPY() torso_angle = torso_rpy[1] except: torso_angle = 0 if abs(torso_angle) > 0.6: self.cmd_vel.angular.z = self.max_rotation_speed * torso_angle * 1.5 / self.HALF_PI self.cmd_vel.linear.x = 0.0 self.cmd_vel.linear.y = 0.0 else: self.cmd_vel.angular.z = 0.0 except: pass else: # For non-holonomic control, segment the control plane into forward/backward and rotation motions. try: drive_speed = self.cmd_vel.linear.x drive_angle = atan2(self.cmd_vel.linear.y, self.cmd_vel.linear.x) self.cmd_vel.linear.y = 0 if abs(drive_speed) < 0.07: self.cmd_vel.linear.x = 0.0 self.cmd_vel.angular.z = 0.0 return #rospy.loginfo("DRIVE SPEED: " + str(drive_speed) + " ANGLE: " + str(drive_angle)) if (drive_speed < 0 and abs(drive_angle) > 2.5) or (drive_angle > -0.7 and drive_angle < 1.5): self.cmd_vel.angular.z = 0.0 self.cmd_vel.linear.x = drive_speed elif drive_angle < -0.7: self.cmd_vel.angular.z = self.max_rotation_speed self.cmd_vel.linear.x = 0.0 elif drive_angle > 1.5: self.cmd_vel.angular.z = -self.max_rotation_speed self.cmd_vel.linear.x = 0.0 else: pass except: pass