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