def bayes2spencer(self, bayes_msg):
spencer_msg = TrackedPersons()
spencer_msg.header = bayes_msg.header
for i, pose in enumerate(bayes_msg.poses):
track = TrackedPerson()
track.track_id = self.string2uint64(bayes_msg.uuids[i])
# PoseWithCovariance
track.pose.pose.position = pose.position
track.pose.pose.orientation = pose.orientation
# TwistWithCovariance
track.twist.twist.linear = bayes_msg.velocities[i]
# Data not in bayes. Not sure about these ...
track.pose.covariance = (np.random.normal(0.3, 0.1) *
np.identity(6)).flatten().tolist()
track.twist.covariance = (np.random.normal(0.3, 0.1) *
np.identity(6)).flatten().tolist()
# we assume 0 here
# track.twist.twist.angular
track.is_occluded = False
track.is_matched = False
track.detection_id = self.string2uint64(bayes_msg.uuids[i])
track.age = 0
spencer_msg.tracks.append(track)
return spencer_msg
def detectedPersonsCallback(trackAssociation, detectedPersons,
syncronized_person_pub):
age = rospy.Time.now() - detectedPersons.header.stamp
output = "New detections with track association available (age of detections = %.2f sec)! Detection to track association: " % age.to_sec(
)
syncronizedPersons = TrackedPersons()
syncronizedPersons.header = detectedPersons.header
if detectedPersons.detections:
for detectedPerson in detectedPersons.detections:
# The TrackSynchronizer invoking this callback guarantees that the detectedPersons message is buffered until a
# track association is available for these detections (by comparing message timestamps of tracks and detections).
detectionId = detectedPerson.detection_id
trackId = trackAssociation.lookupTrackId(
detectionId) # <-- this is what this is all about
output += "\n[det %d --> track %s]" % (detectionId, str(trackId))
if trackId is not None:
syncronizedPerson = TrackedPerson()
syncronizedPerson.track_id = trackId
syncronizedPerson.detection_id = detectionId
syncronizedPersons.tracks.append(syncronizedPerson)
rospy.loginfo(output)
if len(syncronizedPersons.tracks) > 0:
syncronized_person_pub.publish(syncronizedPersons)
else:
output += "Empty set of detections!"
def publish(self, trackedPersons, humanAttributes):
humanAttributes.header = trackedPersons.header
self.trackedPersonPublisher.publish(trackedPersons)
self.humanAttributePublisher.publish(humanAttributes)
# Separately publish the currently active track in the editor, if any
if self.trackedPersonWithoutActivePublisher.get_num_connections(
) > 0 or self.activeTrackedPersonPublisher.get_num_connections() > 0:
activeTrackedPersons = TrackedPersons()
activeTrackedPersons.header = trackedPersons.header
trackedPersonsWithoutActive = TrackedPersons()
trackedPersonsWithoutActive.header = trackedPersons.header
for trackedPerson in trackedPersons.tracks:
if self.editor is None or trackedPerson.track_id != self.editor.activeTrackId:
trackedPersonsWithoutActive.tracks.append(trackedPerson)
else:
activeTrackedPersons.tracks.append(trackedPerson)
self.activeTrackedPersonPublisher.publish(activeTrackedPersons)
self.trackedPersonWithoutActivePublisher.publish(
trackedPersonsWithoutActive)
