def send_filt_and_ref(self, data):
# publish the estimated pose:
qest = PlanarSystemConfig()
qest.header.stamp = data.header.stamp
qest.header.frame_id = data.header.frame_id
qest.xm = self.Xest2[0]
qest.ym = self.Xest2[1]
qest.xr = self.Xest2[2]
qest.r = self.Xest2[3]
self.filt_pub.publish(qest)
# publish estimated state
xest = PlanarSystemState()
xest.header.stamp = data.header.stamp
xest.header.frame_id = data.header.frame_id
xest.xm = self.Xest2[0]
xest.ym = self.Xest2[1]
xest.xr = self.Xest2[2]
xest.r = self.Xest2[3]
xest.pxm = self.Xest2[4]
xest.pym = self.Xest2[5]
xest.vxr = self.Xest2[6]
xest.vr = self.Xest2[7]
self.filt_state_pub.publish(xest)
# publish the reference pose:
qest = PlanarSystemConfig()
qest.header.stamp = data.header.stamp
qest.header.frame_id = data.header.frame_id
qest.xm = self.Qref[self.k][0]
qest.ym = self.Qref[self.k][1]
qest.xr = self.Qref[self.k][2]
qest.r = self.Qref[self.k][3]
self.ref_pub.publish(qest)
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
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
