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)
