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)
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)