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
