def ref_config_service_handler(self, req): if req.index is not 0: rospy.logerr("Current provider cannot handle index request") return None elif req.t < 0.0 or req.t > self.tf: rospy.logerr("Requested time %f is outside of valid horizon", req.t) return None # get X,U at requested time r = self.RM.calc_reference_traj(self.dsys, [req.t]) if r: X, U = r else: rospy.logerr("calc_reference_traj returned None!") return None # fill out message config = PlanarSystemConfig() config.xm = X[0, self.system.get_config('xm').index] config.ym = X[0, self.system.get_config('ym').index] config.xr = X[0, self.system.get_config('xr').index] config.r = X[0, self.system.get_config('r').index] config.header.frame_id = "/optimization_frame" config.header.stamp = rospy.Time.now() time = req.t length = len(self.tvec) xtmp = X[0] dt = self.dt return { 'config': config, 'state': xtmp, 'dt': dt, 'length': length, 'time': time, }
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