def publish_state_and_config(self, data, Xfilt, Xref): xmsg = PlanarSystemState() qmsg = PlanarSystemConfig() xmsg.header = data.header qmsg.header = data.header tools.array_to_state(self.system, Xfilt, xmsg) tools.array_to_config(self.system, Xfilt[0:self.system.nQ], qmsg) self.filt_state_pub.publish(xmsg) self.filt_pub.publish(qmsg) # publish ref data: xmsg = PlanarSystemState() qmsg = PlanarSystemConfig() xmsg.header = data.header qmsg.header = data.header tools.array_to_state(self.system, Xref, xmsg) tools.array_to_config(self.system, Xref[0:self.system.nQ], qmsg) self.ref_state_pub.publish(xmsg) self.ref_pub.publish(qmsg) return