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)