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
