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