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
