def initialize_actions(self, action):
"""
Creates action objects and initializes their state
"""
self.actions = {1: self.mime_go,
2: self.crane_go}
if action==1:
print " Action chosen: Mime\n"
self.mime = Mime()
self.action = 'mime_go'
self.mime_count = 0
self.action_chosen = True
# Screen images
self.img_switch.change_mode('mime',3)
elif action == 2:
print " Action chosen: Crane\n"
self.crane = Crane()
self.crane_count = 0
self.action_chosen = True
# Screen images
self.img_switch.change_mode('crane',3)
class Baxter_Controller:
"""
Baxter Controller
This class functions as the macro controller of Baxter's actions during
Natural Interaction with people. A primer user is chosen, the user chooses
an action by gesturing, and can command Baxter to switch to a different
application at any time. While not completing any action or tracking
people, Baxter performs an idle function.
In conjunction to running the Baxter Controller, the NU SkeletonTracker
and the Trajectory Controller must also be running. To do so:
Different terminal:
roslaunch skeletontracker_nu nu_skeletontracker.launch
Another different terminal:
rosrun baxter_interface trajectory_controller.py
Should be handled in launch file, no?
"""
def __init__(self):
"""
Enables robot, initializes booleans, subscribes to skeletons
"""
# print "Getting robot state..."
self.rs = baxter_interface.RobotEnable() #RS is a wrapper for the robot state
# print "Enabling robot... "
self.rs.enable()
self.left_arm = baxter_interface.limb.Limb('left')
self.right_arm = baxter_interface.limb.Limb('right')
self.mime_l_angles = {'left_s0': 0.35, 'left_s1': 0.00, 'left_e0': 0.00, 'left_e1': 0.00, 'left_w0': 0.00, 'left_w1': 0.00, 'left_w2': 0.00}
self.mime_r_angles = {'right_s0': -0.25, 'right_s1': 0.00, 'right_e0': 0.00, 'right_e1': 0.00, 'right_w0': 0.00, 'right_w1': 0.00, 'right_w2': 0.00}
self.crane_l_angles = {'left_s0': 0.35, 'left_s1': 0.00, 'left_e0': 0.00, 'left_e1': 1.57, 'left_w0': 0.00, 'left_w1': 0.00, 'left_w2': 0.00}
self.crane_r_angles = {'right_s0': -0.25, 'right_s1': 0.00, 'right_e0': 0.00, 'right_e1': 0.00, 'right_w0': 0.00, 'right_w1': 0.00, 'right_w2': 0.00}
# Booleans used throughout controller
self.userid_almost_chosen = False
self.userid_chosen = False
self.user_positioned = False
self.action_chosen = False
self.action_id = 0
self.display_top = True
self.display_mime_prep = True
self.display_crane_prep = True
self.display_mime = True
self.display_crane = True
#Pull the required filename from the parameter server
try:
img_files_filename = rospy.get_param('img_files_filename')
except KeyError:
sys.exit("img_files_filename not set in ROS parameter server")
# Set up our ImageSwitcher object to do our first set of images
# Note for the top mode there is only one image (for now) so we
# don't need to set a period other than 0 which is a one-shot
self.img_switch = imgswitch.ImageSwitcher(img_files_filename, mode='top',
image_period=0)
# skeletonCallback called whenever skeleton received
rospy.Subscriber("skeletons", Skeletons, self.skeletonCallback)
# instantiate a skeleton filter
self.skel_filt = sf.SkeletonFilter(sf.joints)
self.first_filt_flag = True
def choose_user(self, skeletons):
"""
Selects primary user to avoid ambiguity
"""
for skeleton in skeletons:
lh = skeleton.left_hand.transform.translation.y
rh = skeleton.right_hand.transform.translation.y
h = skeleton.head.transform.translation.y
if h - lh > 0.11 or h - rh > 0.11: # What units are these?
self.userid_almost_chosen = True
self.main_userid = skeleton.userid
print "\n\nMain user chosen.\nUser %s, please proceed.\n" % str(self.main_userid)
self.user_starting_position = skeleton.torso.transform.translation
if h - lh > 0.11:
self.img_switch.change_mode('crane_prep',3)
self.left_arm.move_to_joint_positions(self.crane_l_angles)
self.right_arm.move_to_joint_positions(self.crane_r_angles)
self.userid_chosen = True
return 'left'
elif h - rh > 0.11:
self.img_switch.change_mode('mime_prep', 3)
#Let's try and speed this stuff up
self.left_arm.move_to_joint_positions(self.mime_l_angles)
self.right_arm.move_to_joint_positions(self.mime_r_angles)
self.userid_chosen = True
return 'right'
def position_user(self, skeleton, choice):
lh_y = skeleton.left_hand.transform.translation.y
lh_x = skeleton.left_hand.transform.translation.x
rh_y = skeleton.right_hand.transform.translation.y
rh_x = skeleton.right_hand.transform.translation.y
tor_y = skeleton.torso.transform.translation.y
tor_x = skeleton.torso.transform.translation.x
if choice == 'left':
dy = math.fabs(tor_y - lh_y)
dx = math.fabs(lh_x - tor_x)
#These tolerances have been causing problems
if dy < 0.08 and dx > 0.4:
self.img_switch.change_mode('positioned',0)
rospy.sleep(0.5) # try a 1/2 second delay
return True
elif (choice == 'right'):
dy = math.fabs(tor_y - lh_y) + math.fabs(tor_y - rh_y)
dx = math.fabs(lh_x - tor_x) + math.fabs(rh_x - tor_x)
if dy < 0.20 and dx > 0.8:
self.img_switch.change_mode('positioned',0)
rospy.sleep(0.5) # try 1/2 second delay
return True
return False
def choose_action(self, skeleton, choice):
"""
Determines which action for Baxter to perform based on gestures
"""
# Action not chosen yet
if self.action_id == 0:
# MIME
if choice == 'right':
self.action_id = 1
action = 'Mime'
elif choice == 'left':
self.action_id = 2
action = 'Crane'
# ACTION CHOSEN
print "Action chosen: %s\nProceed?\n" % action
self.initialize_actions(self.action_id)
def initialize_actions(self, action):
"""
Creates action objects and initializes their state
"""
self.actions = {1: self.mime_go,
2: self.crane_go}
if action==1:
print " Action chosen: Mime\n"
self.mime = Mime()
self.action = 'mime_go'
self.mime_count = 0
self.action_chosen = True
# Screen images
self.img_switch.change_mode('mime',3)
elif action == 2:
print " Action chosen: Crane\n"
self.crane = Crane()
self.crane_count = 0
self.action_chosen = True
# Screen images
self.img_switch.change_mode('crane',3)
def reset_booleans(self):
"""
Resets booleans when user is done with action
"""
print "\n**Booleans reset**\n"
self.img_switch.change_mode('bool_reset',3)
#Why do we disable, reset, and enable when resetting user stuff?
self.rs.disable()
self.rs.reset()
self.rs.enable()
self.userid_almost_chosen = False
self.userid_chosen = False
self.user_positioned = False
self.action_chosen = False
self.action_id = 0
self.display_top = True
self.display_mime_prep = True
self.display_crane_prep = True
self.display_mime = True
self.display_crane = True
rospy.sleep(2.0)
# Screen images
self.img_switch.change_mode('top',3)
#=========================================================#
# ACTIONS: #
#=========================================================#
def mime_go(self, skeleton):
"""
Progresses mime when skeletons are passed into controller
"""
# For passing certain percentage of skeletons to mime
self.mime_count+=1
# Skeleton values
l_sh = skeleton.left_shoulder.transform.translation
l_el = skeleton.left_elbow.transform.translation
l_ha = skeleton.left_hand.transform.translation
r_sh = skeleton.right_shoulder.transform.translation
r_el = skeleton.right_elbow.transform.translation
r_ha = skeleton.right_hand.transform.translation
if self.mime_count % DOWN_SAMPLE == 0:
self.mime.move(l_sh, l_el, l_ha, r_sh, r_el, r_ha)
def crane_go(self, skeleton):
"""
Progresses crane when skeletons are passed into controller
"""
# For passing certain percentage of skeletons to crane
self.crane_count+=1
# Skeleton values
l_sh = skeleton.left_shoulder.transform.translation
l_el = skeleton.left_elbow.transform.translation
l_ha = skeleton.left_hand.transform.translation
r_sh = skeleton.right_shoulder.transform.translation
r_el = skeleton.right_elbow.transform.translation
r_ha = skeleton.right_hand.transform.translation
if self.crane_count % DOWN_SAMPLE == 0:
self.crane.move(l_sh, l_el, l_ha, r_sh, r_el, r_ha)
#######################
# SUBSCRIBER CALLBACK #
#######################
def skeletonCallback(self, data):
"""
Runs every time a skeleton is received from the tracker
This seems to do everything
This function is somewhat confusing, I'll go through later and document it better
"""
# Chooses correct user
if self.userid_chosen == True:
found = False
for skeleton in data.skeletons:
if skeleton.userid == self.main_userid:
skel_raw = skeleton
if self.first_filt_flag:
self.skel_filt.reset_filters(skel_raw)
skel = skel_raw
self.first_filt_flag = False
else:
skel = self.skel_filt.filter_skeletons(skel_raw)
found = True
# Chooses and sticks to one main user throughout
if self.userid_chosen == False:
self.choice = self.choose_user(data.skeletons)
self.first_filt_flag = True
elif self.user_positioned == False and found:
p1_x = self.user_starting_position.x
p1_z = self.user_starting_position.z
p2_x = skel.torso.transform.translation.x
p2_z = skel.torso.transform.translation.z
y_LH = skel.left_hand.transform.translation.y
y_RH = skel.right_hand.transform.translation.y
y_torso = skel.torso.transform.translation.y
left_ratio = (y_LH - y_torso) / y_torso
right_ratio = (y_RH - y_torso) / y_torso
dx = p2_x - p1_x
dz = p2_z - p1_z
if not (math.fabs(dx) > 0.10 and math.fabs(dz) > 0.10 and left_ratio > 0.5 and right_ratio > 0.5):
self.user_positioned = self.position_user(skel, self.choice)
else: self.reset_booleans()
# Chooses action to complete
elif self.action_chosen == False and found:
self.choose_action(skel, self.choice)
# If user doesn't leave, completes action
elif found:
p1_x = self.user_starting_position.x
p1_z = self.user_starting_position.z
p2_x = skel.torso.transform.translation.x
p2_z = skel.torso.transform.translation.z
y_LH = skel.left_hand.transform.translation.y
y_RH = skel.right_hand.transform.translation.y
y_torso = skel.torso.transform.translation.y
left_ratio = (y_LH - y_torso) / y_torso
right_ratio = (y_RH - y_torso) / y_torso
dx = p2_x - p1_x
dz = p2_z - p1_z
if not (math.fabs(dx) > 0.10 and math.fabs(dz) > 0.10 and left_ratio > 0.5 and right_ratio > 0.5):
self.actions[self.action_id](skel)
else: self.reset_booleans()
elif not found:
self.reset_booleans()
