def check_users_exist(self):
gone = []
for uid, user in self.users_.items():
if user.exists_ == False:
gone.append(uid)
for g in gone:
if rospy.has_param("wallframe/user_data/" + str(g)):
rospy.delete_param("wallframe/user_data/" + str(g))
rospy.logwarn("User [wallframe UID: " + str(g) +
"] lost, removing from list")
self.toast_pub_.publish(String("User " + str(g) + " Lost"))
msg = user_event_msg()
msg.user_id = g
msg.event_id = 'workspace_event'
msg.message = 'user_left'
self.user_event_pub_.publish(msg)
del self.users_[g]
if len(self.users_) == 0:
if self.no_users_ == False:
# Send all users left message
msg = user_event_msg()
msg.user_id = g
msg.event_id = 'workspace_event'
msg.message = 'all_users_left'
self.user_event_pub_.publish(msg)
self.no_users_ = True
def check_users_exist(self):
gone = []
for uid, user in self.users_.items():
if user.exists_ == False:
gone.append(uid)
for g in gone:
if rospy.has_param("wallframe/user_data/"+str(g)):
rospy.delete_param("wallframe/user_data/"+str(g))
rospy.logwarn("User [wallframe UID: " + str(g) + "] lost, removing from list")
self.toast_pub_.publish(String("User "+str(g)+" Lost"))
msg = user_event_msg()
msg.user_id = g
msg.event_id = 'workspace_event'
msg.message = 'user_left'
self.user_event_pub_.publish(msg)
del self.users_[g]
if len(self.users_) == 0:
if self.no_users_ == False:
# Send all users left message
msg = user_event_msg()
msg.user_id = g
msg.event_id = 'workspace_event'
msg.message = 'all_users_left'
self.user_event_pub_.publish(msg)
self.no_users_ = True
def tracker_cb(self,tracker_msg):
for user_packet in tracker_msg.users:
# print user_packet.uid
add_new_user = False
# print "number of wallframe users: " + str(len(self.users_))
# Check and update active trackers
if user_packet.tracker_id not in self.active_trackers_:
rospy.logwarn("New tracker is broadcasting messages: [" + str(user_packet.tracker_id) + "]")
self.active_trackers_.append(user_packet.tracker_id)
if len(self.users_) == 0:
# rospy.logwarn("First user found: [tracker ID: " + str(user_packet.uid) + "]")
add_new_user = True
else:
update_target = None
for uid, user in self.users_.items():
# m = "checking packet "+str(user_packet.uid)+" against current users "+str(user.uid_)+" ... "
if user_packet.tracker_id in user.tracker_uids_.keys():
found_tracker = True
# m = m + "tracker matched ... "
if user_packet.uid in user.tracker_uids_.values():
# m = m + "uid matched ... "
update_target = uid
else:
# m = m + "uid did not match ... "
pass
# else:
# # user is not being tracked by this packet's tracker
# if check_com(user_packet,user) == True:
# # this is the same user in a different tracker
# rospy.logwarn("Found user [wallframe UID:" + str(user.uid_) + "] in new tracker [tracker " + str(user_packet.tracker_id) + ", tracker ID: " + str(user_packet.uid) + "]")
# user.tracker_packets_[user_packet.tracker_id] = user_packet
# user.tracker_uids_[user_packet.tracker_id] = user_packet.uid
# else:
# add_new_user = True
# print m
if update_target is not None:
# m = m + "updating ... "
self.users_[update_target].tracker_packets_[user_packet.tracker_id] = user_packet
elif update_target is None:
# m = m + "uid did not match ... adding"
add_new_user = True
# Add new user
if add_new_user == True:
if user_packet.center_of_mass == Vector3(0.0,0.0,0.0):
pass
else:
# Increment the user id to assign if that id is already taken
while self.current_uid_ in self.users_.keys():
self.current_uid_ += 1
if self.current_uid_ > 6:
self.current_uid_ = 0
u = User( self.current_uid_,
self.tf_listener_,
self.user_event_pub_,
self.filtering_,
self.hand_click_)
u.tracker_uids_[user_packet.tracker_id] = user_packet.uid
u.tracker_packets_[user_packet.tracker_id] = user_packet
u.exists_in_tracker[user_packet.tracker_id] = True
u.exists_ = True
msg = user_event_msg()
msg.user_id = self.current_uid_
msg.event_id = 'workspace_event'
msg.message = 'user_entered'
self.user_event_pub_.publish(msg)
rospy.logwarn("Adding user: [tracker ID: " + str(user_packet.uid) + "], [wallframe ID: " + str(self.current_uid_) + "]")
self.toast_pub_.publish(String("User "+str(self.current_uid_)+" Joined"))
self.users_[u.mid_] = u
rospy.set_param("wallframe/user_data/"+str(u.mid_),self.users_[u.mid_].get_info())
self.current_uid_ += 1
if self.current_uid_ > 6:
self.current_uid_ = 0
self.no_users_ = False
add_new_user = False
pass
def evaluate_state(self):
msg = user_event_msg()
msg.user_id = self.mid_
msg.message = 'none'
for case in switch(self.state__):
if case('IDLE'):
if self.current_state_msg.outside_workspace:
msg.event_id = 'workspace_event'
msg.message = 'outside_workspace'
self.state__ = 'OUTSIDE_WORKSPACE'
break
# Hand Events
msg.event_id = 'hand_event'
if self.current_state_msg.hands_on_head:
msg.message = 'hands_on_head'
self.state__ = 'HANDS_HEAD'
break
if self.current_state_msg.hands_together:
msg.message = 'hands_together'
self.state__ = 'HANDS_TOGETHER'
break
if self.current_state_msg.right_elbow_click:
msg.message = 'right_elbow_click'
self.state__ = 'RIGHT_ELBOW_CLICK'
break
if self.current_state_msg.left_elbow_click:
msg.message = 'left_elbow_click'
self.state__ = 'LEFT_ELBOW_CLICK'
break
break
if case('OUTSIDE_WORKSPACE'):
if not self.current_state_msg.outside_workspace:
self.state__ = 'IDLE'
break
if case('HANDS_TOGETHER'):
if not self.current_state_msg.hands_together:
self.state__ = 'IDLE'
break
if case('HANDS_HEAD'):
if not self.check_joint_dist(self.head_limit_,'left_hand','head'):
self.state__ = 'IDLE'
break
elif not self.check_joint_dist(self.head_limit_,'right_hand','head'):
self.state__ = 'IDLE'
break
break
if case('RIGHT_ELBOW_CLICK'):
if not self.current_state_msg.right_elbow_click:
self.state__ = 'IDLE'
break
if case('LEFT_ELBOW_CLICK'):
if not self.current_state_msg.left_elbow_click:
self.state__ = 'IDLE'
break
# Check for populated message and publish
if 'none' not in msg.message:
self.user_event_pub_.publish(msg)
pass
def evaluate_events(self):
# Hands Together
if self.check_joint_dist(self.hand_limit_,'left_hand','right_hand'):
self.current_state_msg.hands_together = True
else:
self.current_state_msg.hands_together = False
# Hands on Head
if all( [self.check_joint_dist(self.head_limit_,'right_hand','head'),
self.check_joint_dist(self.head_limit_,'left_hand','head')] ):
self.current_state_msg.hands_on_head = True
else:
self.current_state_msg.hands_on_head = False
# Right Elbow Click
if self.check_joint_dist(self.hand_limit_,'left_hand','right_elbow') and self.joint_body_pos('right_hand').y > self.joint_body_pos('torso').y:
self.current_state_msg.right_elbow_click = True
else:
self.current_state_msg.right_elbow_click = False
# Left Elbow Click
if self.check_joint_dist(self.hand_limit_,'right_hand','left_elbow') and self.joint_body_pos('left_hand').y > self.joint_body_pos('torso').y:
self.current_state_msg.left_elbow_click = True
else:
self.current_state_msg.left_elbow_click = False
# Right Hand Front
if self.joint_body_pos('right_hand').y > self.joint_body_pos('left_hand').y:
self.current_state_msg.right_in_front = True
self.current_state_msg.left_in_front = False
else:
self.current_state_msg.right_in_front = False
self.current_state_msg.left_in_front = True
# Workspace Limit
self.current_state_msg.outside_workspace = self.check_outside_workspace()
# Hand Click
if self.hand_click_ == True:
cur_vel_x = (self.joint_pos('left_hand').x - self.hand_x_)/2.0
cur_vel_y = (self.joint_pos('left_hand').y - self.hand_y_)/2.0
self.hand_x_ = self.joint_pos('left_hand').x
self.hand_y_ = self.joint_pos('left_hand').y
self.vpub_.publish(Float32(cur_vel_x))
if cur_vel_y > -5.0 and cur_vel_y < 5.0:
if cur_vel_x < -5.0:
self.cl_ = True
if self.cl_ == True:
self.up_cnt_ += 1
self.vels_.append(cur_vel_x)
if self.cl_ == True and self.up_cnt_ == 6:
v = False
p = ''
for vel in self.vels_:
p = p + ' ' + str(vel)
if vel > 4.0:
v = True
if v == True:
msg = user_event_msg()
msg.user_id = self.mid_
msg.event_id = 'hand_event'
msg.message = 'left_elbow_click'
self.user_event_pub_.publish(msg)
# Reset
self.cl_ = False
self.up_cnt_ = 0
self.vels_ = []
pass
def tracker_cb(self, tracker_msg):
for user_packet in tracker_msg.users:
# print user_packet.uid
add_new_user = False
# print "number of wallframe users: " + str(len(self.users_))
# Check and update active trackers
if user_packet.tracker_id not in self.active_trackers_:
rospy.logwarn("New tracker is broadcasting messages: [" +
str(user_packet.tracker_id) + "]")
self.active_trackers_.append(user_packet.tracker_id)
if len(self.users_) == 0:
# rospy.logwarn("First user found: [tracker ID: " + str(user_packet.uid) + "]")
add_new_user = True
else:
update_target = None
for uid, user in self.users_.items():
# m = "checking packet "+str(user_packet.uid)+" against current users "+str(user.uid_)+" ... "
if user_packet.tracker_id in user.tracker_uids_.keys():
found_tracker = True
# m = m + "tracker matched ... "
if user_packet.uid in user.tracker_uids_.values():
# m = m + "uid matched ... "
update_target = uid
else:
# m = m + "uid did not match ... "
pass
# else:
# # user is not being tracked by this packet's tracker
# if check_com(user_packet,user) == True:
# # this is the same user in a different tracker
# rospy.logwarn("Found user [wallframe UID:" + str(user.uid_) + "] in new tracker [tracker " + str(user_packet.tracker_id) + ", tracker ID: " + str(user_packet.uid) + "]")
# user.tracker_packets_[user_packet.tracker_id] = user_packet
# user.tracker_uids_[user_packet.tracker_id] = user_packet.uid
# else:
# add_new_user = True
# print m
if update_target is not None:
# m = m + "updating ... "
self.users_[update_target].tracker_packets_[
user_packet.tracker_id] = user_packet
elif update_target is None:
# m = m + "uid did not match ... adding"
add_new_user = True
# Add new user
if add_new_user == True:
if user_packet.center_of_mass == Vector3(0.0, 0.0, 0.0):
pass
else:
# Increment the user id to assign if that id is already taken
while self.current_uid_ in self.users_.keys():
self.current_uid_ += 1
if self.current_uid_ > 6:
self.current_uid_ = 0
u = User(self.current_uid_, self.tf_listener_,
self.user_event_pub_, self.filtering_,
self.hand_click_)
u.tracker_uids_[user_packet.tracker_id] = user_packet.uid
u.tracker_packets_[user_packet.tracker_id] = user_packet
u.exists_in_tracker[user_packet.tracker_id] = True
u.exists_ = True
msg = user_event_msg()
msg.user_id = self.current_uid_
msg.event_id = 'workspace_event'
msg.message = 'user_entered'
self.user_event_pub_.publish(msg)
rospy.logwarn("Adding user: [tracker ID: " +
str(user_packet.uid) + "], [wallframe ID: " +
str(self.current_uid_) + "]")
self.toast_pub_.publish(
String("User " + str(self.current_uid_) + " Joined"))
self.users_[u.mid_] = u
rospy.set_param("wallframe/user_data/" + str(u.mid_),
self.users_[u.mid_].get_info())
self.current_uid_ += 1
if self.current_uid_ > 6:
self.current_uid_ = 0
self.no_users_ = False
add_new_user = False
pass
def evaluate_state(self):
msg = user_event_msg()
msg.user_id = self.mid_
msg.message = 'none'
for case in switch(self.state__):
if case('IDLE'):
if self.current_state_msg.outside_workspace:
msg.event_id = 'workspace_event'
msg.message = 'outside_workspace'
self.state__ = 'OUTSIDE_WORKSPACE'
break
# Hand Events
msg.event_id = 'hand_event'
if self.current_state_msg.hands_on_head:
msg.message = 'hands_on_head'
self.state__ = 'HANDS_HEAD'
break
if self.current_state_msg.hands_together:
msg.message = 'hands_together'
self.state__ = 'HANDS_TOGETHER'
break
if self.current_state_msg.right_elbow_click:
msg.message = 'right_elbow_click'
self.state__ = 'RIGHT_ELBOW_CLICK'
break
if self.current_state_msg.left_elbow_click:
msg.message = 'left_elbow_click'
self.state__ = 'LEFT_ELBOW_CLICK'
break
break
if case('OUTSIDE_WORKSPACE'):
if not self.current_state_msg.outside_workspace:
self.state__ = 'IDLE'
break
if case('HANDS_TOGETHER'):
if not self.current_state_msg.hands_together:
self.state__ = 'IDLE'
break
if case('HANDS_HEAD'):
if not self.check_joint_dist(self.head_limit_, 'left_hand',
'head'):
self.state__ = 'IDLE'
break
elif not self.check_joint_dist(self.head_limit_, 'right_hand',
'head'):
self.state__ = 'IDLE'
break
break
if case('RIGHT_ELBOW_CLICK'):
if not self.current_state_msg.right_elbow_click:
self.state__ = 'IDLE'
break
if case('LEFT_ELBOW_CLICK'):
if not self.current_state_msg.left_elbow_click:
self.state__ = 'IDLE'
break
# Check for populated message and publish
if 'none' not in msg.message:
self.user_event_pub_.publish(msg)
pass
def evaluate_events(self):
# Hands Together
if self.check_joint_dist(self.hand_limit_, 'left_hand', 'right_hand'):
self.current_state_msg.hands_together = True
else:
self.current_state_msg.hands_together = False
# Hands on Head
if all([
self.check_joint_dist(self.head_limit_, 'right_hand', 'head'),
self.check_joint_dist(self.head_limit_, 'left_hand', 'head')
]):
self.current_state_msg.hands_on_head = True
else:
self.current_state_msg.hands_on_head = False
# Right Elbow Click
if self.check_joint_dist(self.hand_limit_, 'left_hand', 'right_elbow'):
self.current_state_msg.right_elbow_click = True
else:
self.current_state_msg.right_elbow_click = False
# Left Elbow Click
if self.check_joint_dist(self.hand_limit_, 'right_hand', 'left_elbow'):
self.current_state_msg.left_elbow_click = True
else:
self.current_state_msg.left_elbow_click = False
# Right Hand Front
if self.joint_body_pos('right_hand').y > self.joint_body_pos(
'left_hand').y:
self.current_state_msg.right_in_front = True
self.current_state_msg.left_in_front = False
else:
self.current_state_msg.right_in_front = False
self.current_state_msg.left_in_front = True
# Workspace Limit
self.current_state_msg.outside_workspace = self.check_outside_workspace(
)
# Hand Click
if self.hand_click_ == True:
cur_vel_x = (self.joint_pos('left_hand').x - self.hand_x_) / 2.0
cur_vel_y = (self.joint_pos('left_hand').y - self.hand_y_) / 2.0
self.hand_x_ = self.joint_pos('left_hand').x
self.hand_y_ = self.joint_pos('left_hand').y
self.vpub_.publish(Float32(cur_vel_x))
if cur_vel_y > -5.0 and cur_vel_y < 5.0:
if cur_vel_x < -5.0:
self.cl_ = True
if self.cl_ == True:
self.up_cnt_ += 1
self.vels_.append(cur_vel_x)
if self.cl_ == True and self.up_cnt_ == 6:
v = False
p = ''
for vel in self.vels_:
p = p + ' ' + str(vel)
if vel > 4.0:
v = True
if v == True:
msg = user_event_msg()
msg.user_id = self.mid_
msg.event_id = 'hand_event'
msg.message = 'left_elbow_click'
self.user_event_pub_.publish(msg)
# Reset
self.cl_ = False
self.up_cnt_ = 0
self.vels_ = []
pass
