def createGroup(allTracks, tracksInGroup): global pub_groups global group_id global groups group = TrackedGroup() group.group_id = group_id group.age = rospy.Duration.from_sec(10) x = 0 y = 0 nagents = 0 for tracked_person in allTracks: if(tracked_person.track_id in tracksInGroup): quat = (tracked_person.pose.pose.orientation.x, tracked_person.pose.pose.orientation.y, tracked_person.pose.pose.orientation.z, tracked_person.pose.pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quat) tracked_person_theta = euler[2] x = x + tracked_person.pose.pose.position.x y = y + tracked_person.pose.pose.position.y nagents = nagents + 1 group.track_ids.append(tracked_person.track_id) group.centerOfGravity.pose.position.x = x / nagents group.centerOfGravity.pose.position.y = y / nagents groups.groups.append(group) group_id += 1
def trackGroups(groups, trackedPersons): # Initialize variables currentTime = trackedPersons.header.stamp publishSinglePersonGroups = rospy.get_param('~publish_single_person_groups', False) trackedGroups = [] assignedGroupIds = [] # Sort groups by smallest track ID per group to ensure reproducible group ID assignments sortedGroups = sorted(groups.iteritems(), key=lambda (clusterId, track_ids) : sorted(track_ids)[0]) # Used to calculate group centroids trackPositionsById = dict() for track in trackedPersons.tracks: pos = track.pose.pose.position trackPositionsById[track.track_id] = numpy.array([ pos.x, pos.y ]) # Create a TrackedGroup for each group, and assign a unique ID for clusterId, track_ids in sortedGroups: # Check if we encountered this combination of track IDs, or a superset thereof, before bestGroupIdAssignment = None trackIdSet = set(track_ids) for groupIdAssignment in trackGroups.groupIdAssignmentMemory: if groupIdAssignment.trackIds.issuperset(trackIdSet) or groupIdAssignment.trackIds.issubset(trackIdSet): trackCount = len(groupIdAssignment.trackIds) bestTrackCount = None if bestGroupIdAssignment is None else len(bestGroupIdAssignment.trackIds) if bestGroupIdAssignment is None or trackCount > bestTrackCount or (trackCount == bestTrackCount and groupIdAssignment.createdAt < bestGroupIdAssignment.createdAt): if groupIdAssignment.groupId not in assignedGroupIds: bestGroupIdAssignment = groupIdAssignment groupId = None if bestGroupIdAssignment is not None: groupId = bestGroupIdAssignment.groupId if groupId == None or groupId in assignedGroupIds: groupId = trackGroups.largestGroupId + 1 # just generate a new ID # Remember that this group ID has been used in this cycle assignedGroupIds.append(groupId) # Remember which group ID we assigned to this combination of track IDs groupIdAssignmentsToRemove = [] groupExistsSince = trackedPersons.header.stamp.to_sec() for groupIdAssignment in trackGroups.groupIdAssignmentMemory: if set(track_ids) == groupIdAssignment.trackIds: groupExistsSince = min(groupIdAssignment.createdAt, groupExistsSince) groupIdAssignmentsToRemove.append(groupIdAssignment) # remove any old entries with same track IDs for groupIdAssignment in groupIdAssignmentsToRemove: trackGroups.groupIdAssignmentMemory.remove(groupIdAssignment) trackGroups.groupIdAssignmentMemory.append( GroupIdAssignment(track_ids, groupId, groupExistsSince) ) #rospy.loginfo(str([str(x) for x in trackGroups.groupIdAssignmentMemory])) # Remember largest group ID used so far if(groupId > trackGroups.largestGroupId): trackGroups.largestGroupId = groupId; # Do not publish single-person groups if not desired if publishSinglePersonGroups or len(track_ids) > 1: accumulatedPosition = numpy.array([0.0, 0.0]) activeTrackCount = 0 for track_id in track_ids: if track_id in trackPositionsById: accumulatedPosition += trackPositionsById[track_id] activeTrackCount += 1 trackedGroup = TrackedGroup() trackedGroup.age = currentTime - rospy.Duration(groupExistsSince) trackedGroup.group_id = remapGroupId(groupId) # make sure published IDs are consecutive even if internally, they are not trackedGroup.track_ids = track_ids trackedGroup.centerOfGravity.pose.position.x = accumulatedPosition[0] / float(activeTrackCount) trackedGroup.centerOfGravity.pose.position.y = accumulatedPosition[1] / float(activeTrackCount) trackedGroups.append(trackedGroup) # Return currently tracked groups return trackedGroups
def trackGroups(groups, trackedPersons): # Initialize variables currentTime = trackedPersons.header.stamp publishSinglePersonGroups = rospy.get_param( '~publish_single_person_groups', False) trackedGroups = [] assignedGroupIds = [] # Sort groups by smallest track ID per group to ensure reproducible group ID assignments sortedGroups = sorted(groups.iteritems(), key=lambda (clusterId, track_ids): sorted(track_ids)[0]) # Used to calculate group centroids trackPositionsById = dict() for track in trackedPersons.tracks: pos = track.pose.pose.position trackPositionsById[track.track_id] = numpy.array([pos.x, pos.y]) # Create a TrackedGroup for each group, and assign a unique ID for clusterId, track_ids in sortedGroups: # Check if we encountered this combination of track IDs, or a superset thereof, before bestGroupIdAssignment = None trackIdSet = set(track_ids) for groupIdAssignment in trackGroups.groupIdAssignmentMemory: if groupIdAssignment.trackIds.issuperset( trackIdSet) or groupIdAssignment.trackIds.issubset( trackIdSet): trackCount = len(groupIdAssignment.trackIds) bestTrackCount = None if bestGroupIdAssignment is None else len( bestGroupIdAssignment.trackIds) if bestGroupIdAssignment is None or trackCount > bestTrackCount or ( trackCount == bestTrackCount and groupIdAssignment.createdAt < bestGroupIdAssignment.createdAt): if groupIdAssignment.groupId not in assignedGroupIds: bestGroupIdAssignment = groupIdAssignment groupId = None if bestGroupIdAssignment is not None: groupId = bestGroupIdAssignment.groupId if groupId == None or groupId in assignedGroupIds: groupId = trackGroups.largestGroupId + 1 # just generate a new ID # Remember that this group ID has been used in this cycle assignedGroupIds.append(groupId) # Remember which group ID we assigned to this combination of track IDs groupIdAssignmentsToRemove = [] groupExistsSince = trackedPersons.header.stamp.to_sec() for groupIdAssignment in trackGroups.groupIdAssignmentMemory: if set(track_ids) == groupIdAssignment.trackIds: groupExistsSince = min(groupIdAssignment.createdAt, groupExistsSince) groupIdAssignmentsToRemove.append( groupIdAssignment ) # remove any old entries with same track IDs for groupIdAssignment in groupIdAssignmentsToRemove: trackGroups.groupIdAssignmentMemory.remove(groupIdAssignment) trackGroups.groupIdAssignmentMemory.append( GroupIdAssignment(track_ids, groupId, groupExistsSince)) #rospy.loginfo(str([str(x) for x in trackGroups.groupIdAssignmentMemory])) # Remember largest group ID used so far if (groupId > trackGroups.largestGroupId): trackGroups.largestGroupId = groupId # Do not publish single-person groups if not desired if publishSinglePersonGroups or len(track_ids) > 1: accumulatedPosition = numpy.array([0.0, 0.0]) activeTrackCount = 0 for track_id in track_ids: if track_id in trackPositionsById: accumulatedPosition += trackPositionsById[track_id] activeTrackCount += 1 trackedGroup = TrackedGroup() trackedGroup.age = rospy.Duration( max(0, currentTime.to_sec() - groupExistsSince)) trackedGroup.group_id = remapGroupId( groupId ) # make sure published IDs are consecutive even if internally, they are not trackedGroup.track_ids = track_ids trackedGroup.centerOfGravity.pose.position.x = accumulatedPosition[ 0] / float(activeTrackCount) trackedGroup.centerOfGravity.pose.position.y = accumulatedPosition[ 1] / float(activeTrackCount) trackedGroups.append(trackedGroup) # Return currently tracked groups return trackedGroups