def vehicle_out_callback(self, msg):
if (self.pathlocal_received and (self.ctrl_mode in [1, 2])):
self.state.X = msg.x
self.state.Y = msg.y
self.state.psi = msg.yaw
self.state.psidot = msg.r
self.state.vx = msg.vx
self.state.vy = msg.vy
self.state.s,self.state.d = ptsCartesianToFrenet(np.array(self.state.X), \
np.array(self.state.Y), \
np.array(self.pathlocal.X), \
np.array(self.pathlocal.Y), \
np.array(self.pathlocal.psi_c), \
np.array(self.pathlocal.s))
self.state_received = True
def updateState(self):
self.state_out.X = self.state_in.x
self.state_out.Y = self.state_in.y
self.state_out.psi = self.state_in.yaw
self.state_out.psidot = self.state_in.r
self.state_out.vx = self.state_in.vx
self.state_out.vy = self.state_in.vy
# get s, d and deltapsi
s,d = ptsCartesianToFrenet(np.array(self.state_out.X), \
np.array(self.state_out.Y), \
np.array(self.pathglobal.X), \
np.array(self.pathglobal.Y), \
np.array(self.pathglobal.psi_c), \
np.array(self.pathglobal.s))
s_this_lap = s[0]
# checks for lap counter
if (s_this_lap > 0.45 * self.s_lap and s_this_lap < 0.55 * self.s_lap
and not self.passed_halfway):
self.passed_halfway = True
print "state est: passed halfway mark"
if (s_this_lap > 0.0 and s_this_lap < 10.0 and self.passed_halfway):
self.lapcounter = self.lapcounter + 1
self.passed_halfway = False
print "state est: completed lap, lap count = ", self.lapcounter
# make sure s is >= 0 on first lap
if (self.lapcounter == 0 and s_this_lap > 0.75 * self.s_lap
and not self.passed_halfway):
s_this_lap = 0.0
self.state_out.s = s_this_lap + self.lapcounter * self.s_lap
self.state_out.d = d[0]
psi_c = np.interp(s, self.pathglobal.s, self.pathglobal_psic_cont)
angleToInterval(psi_c)
self.state_out.deltapsi = self.state_out.psi - psi_c
# correction of detapsi @ psi flips
self.state_out.deltapsi = angleToInterval(self.state_out.deltapsi)
self.state_out.deltapsi = self.state_out.deltapsi[0]
def updateState(self):
self.state.X = self.vehicle_out.X
self.state.Y = self.vehicle_out.Y
self.state.psi = self.vehicle_out.psi
self.state.psidot = self.vehicle_out.psidot
self.state.vx = self.vehicle_out.vx
self.state.vy = self.vehicle_out.vy
# get s, d and deltapsi
s,d = ptsCartesianToFrenet(np.array(self.state.X), \
np.array(self.state.Y), \
np.array(self.pathlocal.X), \
np.array(self.pathlocal.Y), \
np.array(self.pathlocal.psi_c), \
np.array(self.pathlocal.s))
self.state.s = s[0]
self.state.d = d[0]
psi_c = np.interp(s, self.pathlocal.s, self.pathlocal.psi_c)
self.state.deltapsi = self.state.psi - psi_c
self.state.ax = self.vehicle_out.ax
self.state.ay = self.vehicle_out.ay
stop = 1500 Xpath = pathglobal['X'][start:stop] Ypath = pathglobal['Y'][start:stop] psipath = pathglobal['psi_c'][start:stop] spath = pathglobal['s'][start:stop] # define pts in s and d s_in = 270 d_in = -15 # transform frenet --> cart X, Y = ptsFrenetToCartesian(s_in, d_in, Xpath, Ypath, psipath, spath) # transform cart --> frenet s, d = ptsCartesianToFrenet(X, Y, Xpath, Ypath, psipath, spath) # check results print print 's in ', s_in print 's out ', s print 's error ', s_in - s print print 'd in ', d_in print 'd out ', d print 'd error ', d_in - d f, ax = plt.subplots(1, 1) ax.plot(Xpath, Ypath, 'k') #ax.plot(Xc,Yc,'og')