def spin_once(self):
        img = np.ones([self.resolution[1], self.resolution[0], 3],
                      np.uint8) * 255
        t = self.time()
        if t == len(self.T) - 1:
            if self.terminate: return True
            #self.end = rospy.Time.now().to_sec()+self.duration
            self.time = self.prev
            self.rate.sleep()
            return False
        if t <= 0:
            #self.end = rospy.Time.now().to_sec()+self.duration
            self.time = self.next
            self.rate.sleep()
            return False

        for i, e in zip(self.ids, self.entities):
            i.update(e.update(self.T[t]).draw(img).ids)
        oid = ObjectIds()

        oid.header.stamp = rospy.Time.now()
        oid.ids = list(map(lambda i: i << 32, chain(*map(list, self.ids))))
        oid.offsets = [0]
        for i in range(len(self.ids)):
            oid.offsets.append(oid.offsets[i] + len(self.ids[i]))

        oid.labels = range(len(self.ids))
        self.pub_objects.publish(oid)

        self.msg.header.stamp = rospy.Time.now()
        self.msg.x, self.msg.y = map(
            list,
            np.vstack([e.tracks.T for e in self.entities]).T)
        self.msg.ids = list(chain(*[e.ids for e in self.entities]))
        img_msg = self.br.cv2_to_imgmsg(img, 'rgb8')
        img_msg.header = self.msg.header
        self.pub_image.publish(img_msg)
        self.rate.sleep()
        self.pub_points.publish(self.msg)
    def spin_once(self):
        img = np.ones([self.resolution[1],self.resolution[0],3],np.uint8)*255
        t = self.time()
        if t == len(self.T)-1:
            if self.terminate: return True
            #self.end = rospy.Time.now().to_sec()+self.duration
            self.time = self.prev
            self.rate.sleep()
            return False
        if t <= 0:
            #self.end = rospy.Time.now().to_sec()+self.duration
            self.time = self.next
            self.rate.sleep()
            return False

        for i, e in zip(self.ids,self.entities):
            i.update(e.update(self.T[t]).draw(img).ids)
        oid = ObjectIds()

        oid.header.stamp = rospy.Time.now()
        oid.ids = list(map(lambda i: i<<32,chain(*map(list,self.ids))))
        oid.offsets = [0]
        for i in range(len(self.ids)):
            oid.offsets.append(oid.offsets[i]+len(self.ids[i]))

        oid.labels = range(len(self.ids))
        self.pub_objects.publish(oid)
        
        self.msg.header.stamp = rospy.Time.now()
        self.msg.x,self.msg.y = map(list,np.vstack([e.tracks.T for e in self.entities]).T)
        self.msg.ids = list(chain(*[e.ids for e in self.entities]))
        img_msg = self.br.cv2_to_imgmsg(img,'rgb8')
        img_msg.header = self.msg.header
        self.pub_image.publish(img_msg)
        self.rate.sleep()
        self.pub_points.publish(self.msg)
 def publish_object_ids(self, ids, y):
     msg = ObjectIds()
     msg.ids = ids
     idx = np.argsort(y)
     msg.header = self.last_header
     msg.ids = np.array(ids)[idx]
     msg.offsets = [0]
     msg.labels = [y[idx[0]]]
     for i in range(1, len(y)):
         if (y[idx[i]] != y[idx[i - 1]]):
             msg.labels.append(y[idx[i]])
             msg.offsets.append(i)
     msg.offsets.append(len(y))
     self.pub_objs.publish(msg)