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