def cc_ctrl(self):
rospy.loginfo_throttle(1, "Running CC control")
if (self.state.vx > 0.0):
# get lhpt
lhdist = 7 # todo determine from velocity
s_lh = self.state.s + lhdist
d_lh = self.cc_dref
Xlh, Ylh = ptsFrenetToCartesian(np.array([s_lh]), \
np.array([d_lh]), \
np.array(self.pathlocal.X), \
np.array(self.pathlocal.Y), \
np.array(self.pathlocal.psi_c), \
np.array(self.pathlocal.s))
rho_pp = self.pp_curvature(self.state.X, self.state.Y,
self.state.psi, Xlh[0], Ylh[0])
delta_out = rho_pp * (self.lf + self.lr) # kinematic feed fwd
else:
Xlh = 0.0
Ylh = 0.0
delta_out = 0.0
self.vx_error = self.cc_vxref - self.state.vx
if (self.robot_name == "gotthard"):
k = 500
elif (self.robot_name == "rhino"):
k = 1500
else:
k = 0
print "ERROR: incorrect /robot_name"
dc_out = k * self.vx_error
return delta_out, dc_out, Xlh, Ylh
def pathLocalToPolArr(self):
pa = PolygonArray()
pa.header.stamp = rospy.Time.now()
pa.header.frame_id = "map"
for i in range(self.N-1):
spoly = np.array([self.pathlocal.s[i], self.pathlocal.s[i+1],self.pathlocal.s[i+1],self.pathlocal.s[i]])
dpoly = np.array([self.pathlocal.dub[i], self.pathlocal.dub[i+1],self.pathlocal.dlb[i+1],self.pathlocal.dlb[i]])
Xpoly, Ypoly = ptsFrenetToCartesian(spoly,dpoly,self.pathlocal.X,self.pathlocal.Y,self.pathlocal.psi_c,self.pathlocal.s)
p = PolygonStamped()
p.header.stamp = rospy.Time.now()
p.header.frame_id = "map"
p.polygon.points = [Point32(x=Xpoly[0], y=Ypoly[0], z=0.0),
Point32(x=Xpoly[1], y=Ypoly[1], z=0.0),
Point32(x=Xpoly[2], y=Ypoly[2], z=0.0),
Point32(x=Xpoly[3], y=Ypoly[3], z=0.0)]
pa.polygons.append(p)
#pa.Color.r = 1.0
#p.color.g = 0.0
#p.color.b = 0.0
# color for mu
pa.likelihood = self.pathlocal.mu[0:-1]*(0.2/np.max(self.pathlocal.mu)) # discarding final value
#pa.color.r = 1.0
#pa.color.g = 0.0
#pa.color.b = 0.0
return pa
# compute psic
dX = np.diff(X_cl)
dY = np.diff(Y_cl)
psic = np.arctan2(dY, dX)
psic_final = np.arctan2(Y_cl[0] - Y_cl[-1],
X_cl[0] - X_cl[-1]) # assuming closed track
psic = np.append(psic, psic_final)
## get left and right boundaries
#dlb = (-lanewidth/2.0)*np.ones(s.size)
#dub = (3.0*lanewidth/2.0)*np.ones(s.size)
dlb = -lanewidth * np.ones(s.size)
dub = lanewidth * np.ones(s.size)
X_ll, Y_ll = ptsFrenetToCartesian(s, dub, X_cl, Y_cl, psic, s)
X_rl, Y_rl = ptsFrenetToCartesian(s, dlb, X_cl, Y_cl, psic, s)
# downsample to get cone positions
threshold_dist_cones = 4
cones_left_X = [X_ll[0]]
cones_left_Y = [Y_ll[0]]
for i in range(X_ll.size - 1):
dist = np.sqrt((X_ll[i] - cones_left_X[-1])**2 +
(Y_ll[i] - cones_left_Y[-1])**2)
if (dist > threshold_dist_cones):
cones_left_X.append(X_ll[i])
cones_left_Y.append(Y_ll[i])
cones_left_X = np.array(cones_left_X)
cones_left_Y = np.array(cones_left_Y)
def __init__(self):
# timing params
self.dt_sim = 0.01 # timestep of the simulation
self.t_activate = rospy.get_param('/t_activate')
self.t_final = rospy.get_param('/t_final')
# pop-up scenario params
self.s_ego_at_popup = rospy.get_param('/s_ego_at_popup')
self.s_obs_at_popup = rospy.get_param('/s_obs_at_popup')
self.d_obs_at_popup = rospy.get_param('/d_obs_at_popup')
# track params
self.track_name = rospy.get_param('/track_name')
self.s_begin_mu_segments = rospy.get_param('/s_begin_mu_segments')
self.mu_segment_values = rospy.get_param('/mu_segment_values')
self.N_mu_segments = len(self.s_begin_mu_segments)
self.mu_segment_idx = 0
# vehicle params
self.vehicle_width = rospy.get_param('/car/kinematics/l_width')
# algo params (from acado)
N = 40
dt_algo = 0.1
# init node subs pubs
rospy.init_node('experiment_manager', anonymous=True)
#self.clockpub = rospy.Publisher('/clock', Clock, queue_size=10)
self.pathglobalsub = rospy.Subscriber("pathglobal", Path,
self.pathglobal_callback)
self.statesub = rospy.Subscriber("state", State, self.state_callback)
self.carinfosub = rospy.Subscriber("/fssim/car_info", CarInfo,
self.fssim_carinfo_callback)
self.trajstarsub = rospy.Subscriber("trajstar", Trajectory,
self.trajstar_callback)
self.obspub = rospy.Publisher('/obs', Obstacles, queue_size=1)
self.obsvispub = rospy.Publisher('/obs_vis', Marker, queue_size=1)
self.tireparampub = rospy.Publisher('/tire_params',
TireParams,
queue_size=1)
self.ctrl_mode_pub = rospy.Publisher('/ctrl_mode', Int16, queue_size=1)
self.statetextmarkerpub = rospy.Publisher('/state_text_marker',
Marker,
queue_size=1)
# init logging vars
self.explog_activated = False
self.explog_iterationcounter = 0
self.stored_pathglobal = False
self.stored_trajstar = False
self.stored_trajcl = False
self.explog_saved = False
# init misc internal variables
self.pathglobal = Path()
self.received_pathglobal = False
self.state = State()
self.received_state = False
self.trajstar = Trajectory()
self.received_trajstar = False
self.fssim_carinfo = CarInfo()
while (not self.received_pathglobal):
print("waiting for pathglobal")
time.sleep(self.dt_sim * 100)
# init experiment variables
self.scenario_id = rospy.get_param('/scenario_id')
self.tireparams = TireParams()
self.obs = Obstacles()
self.obs.s = [self.s_obs_at_popup]
self.obs.d = [self.d_obs_at_popup]
self.obs.R = [0.5]
wiggleroom = 0.5
self.obs.Rmgn = [
0.5 * self.obs.R[0] + 0.5 * self.vehicle_width + wiggleroom
]
Xobs, Yobs = ptsFrenetToCartesian(np.array(self.obs.s), \
np.array(self.obs.d), \
np.array(self.pathglobal.X), \
np.array(self.pathglobal.Y), \
np.array(self.pathglobal.psi_c), \
np.array(self.pathglobal.s))
self.ctrl_mode = 0 # # 0: stop, 1: cruise_ctrl, 2: tamp
# Main loop
#print 'running experiment: '
#print 'track: ', self.track_name
self.exptime = 0
while (not rospy.is_shutdown()) and self.exptime < self.t_final:
if (self.exptime >= self.t_activate):
rospy.loginfo_throttle(1, "Running experiment")
# HANDLE TRACTION IN SIMULATION
s_ego = self.state.s % self.s_lap
for i in range(self.N_mu_segments - 1):
if (self.s_begin_mu_segments[i] <= s_ego <=
self.s_begin_mu_segments[i + 1]):
self.mu_segment_idx = i
break
if (s_ego >= self.s_begin_mu_segments[-1]):
self.mu_segment_idx = self.N_mu_segments - 1
mu = self.mu_segment_values[self.mu_segment_idx]
#print "s_ego = ", s_ego
#print "state.s = ", self.state.s
#print "self.N_mu_segments = ", self.N_mu_segments
#print "mu_segment_idx = ", self.mu_segment_idx
#print "mu in this section = ", self.mu_segment_values[self.mu_segment_idx]
# set tire params of sim vehicle
if (0.0 <= mu < 0.3): # ice
B = 4.0
C = 2.0
D = mu
E = 1.0
elif (0.3 <= mu < 0.5): # snow
B = 5.0
C = 2.0
D = mu
E = 1.0
elif (0.5 <= mu < 0.9): # wet
B = 12.0
C = 2.3
D = mu
E = 1.0
elif (0.9 <= mu < 1.5): # dry
B = 10.0
C = 1.9
D = mu
E = 0.97
elif (1.5 <= mu <
2.5): # dry + racing tires (gotthard default)
B = 12.56
C = 1.38
D = mu
E = 1.0
else:
rospy.loginfo_throttle(1, "Faulty mu value in exp manager")
self.tireparams.tire_coefficient = 1.0
#self.tireparams.B = 10
#self.tireparams.C = 1.9
#self.tireparams.D = -mu
#self.tireparams.E = 1.0
self.tireparams.B = B
self.tireparams.C = C
self.tireparams.D = -D
self.tireparams.E = E
self.tireparams.header.stamp = rospy.Time.now()
self.tireparampub.publish(self.tireparams)
# POPUP SCENARIO
if (self.scenario_id == 1):
self.ctrl_mode = 1 # cruise control
m_obs = self.getobstaclemarker(Xobs, Yobs, self.obs.R[0])
m_obs.color.a = 0.3 # transparent before detect
if (self.state.s >= self.s_ego_at_popup):
self.obspub.publish(self.obs)
m_obs.color.a = 1.0 # non-transparent after detect
self.ctrl_mode = 2 # tamp
self.obsvispub.publish(m_obs)
# REDUCED MU TURN
elif (self.scenario_id == 2):
self.ctrl_mode = 1 # pp cruise control from standstill
if (self.state.vx > 5): # todo
self.ctrl_mode = 2 # tamp cruise control when we get up to speed
# RACING
else:
self.ctrl_mode = 2 # tamp
# SEND STOP IF EXIT TRACK
dlb = np.interp(self.state.s, self.pathglobal.s,
self.pathglobal.dlb)
dub = np.interp(self.state.s, self.pathglobal.s,
self.pathglobal.dub)
if (self.state.d < dlb or self.state.d > dub):
self.ctrl_mode = 0 # stop
self.ctrl_mode_pub.publish(self.ctrl_mode)
# publish text marker (state info)
state_text = "vx: " + "%.3f" % self.state.vx + "\n" \
"mu: " + "%.3f" % mu
self.statetextmarkerpub.publish(self.gettextmarker(state_text))
# save data for plot
filename = "/home/larsvens/ros/tamp__ws/src/saarti/common/logs/explog_latest.npy" # todo get from param
self.s_begin_log = 195 # 190 todo get from param
if (self.state.s >= self.s_begin_log
and not self.explog_activated):
print "STARTED EXPLOG"
t_start_explog = copy.deepcopy(self.exptime)
self.explog_activated = True
# store planned traj
self.trajstar_dict = {
"X": np.array(self.trajstar.X),
"Y": np.array(self.trajstar.Y),
"psi": np.array(self.trajstar.psi),
"s": np.array(self.trajstar.s),
"d": np.array(self.trajstar.d),
"deltapsi": np.array(self.trajstar.deltapsi),
"psidot": np.array(self.trajstar.psidot),
"vx": np.array(self.trajstar.vx),
"vy": np.array(self.trajstar.vy),
"ax": np.array(self.trajstar.ax),
"ay": np.array(self.trajstar.ay),
"Fyf": np.array(self.trajstar.Fyf),
"Fxf": np.array(self.trajstar.Fxf),
"Fyr": np.array(self.trajstar.Fyr),
"Fxr": np.array(self.trajstar.Fxr),
"Fzf": np.array(self.trajstar.Fzf),
"Fzr": np.array(self.trajstar.Fzr),
"kappac": np.array(self.trajstar.kappac),
"Cr": np.array(self.trajstar.Cr),
"t": np.arange(0, (N + 1) * dt_algo, dt_algo),
}
self.stored_trajstar = True
# initialize vars for storing CL traj
self.X_cl = []
self.Y_cl = []
self.psi_cl = []
self.s_cl = []
self.d_cl = []
self.deltapsi_cl = []
self.psidot_cl = []
self.vx_cl = []
self.vy_cl = []
self.ax_cl = []
self.ay_cl = []
self.t_cl = []
self.Fyf_cl = []
self.Fyr_cl = []
self.Fx_cl = []
if (self.state.s >= self.s_begin_log and self.explog_activated
and self.exptime >= t_start_explog +
self.explog_iterationcounter * dt_algo):
# build CL traj
self.X_cl.append(self.state.X)
self.Y_cl.append(self.state.Y)
self.psi_cl.append(self.state.psi)
self.s_cl.append(self.state.s)
self.d_cl.append(self.state.d)
self.deltapsi_cl.append(self.state.deltapsi)
self.psidot_cl.append(self.state.psidot)
self.vx_cl.append(self.state.vx)
self.vy_cl.append(self.state.vy)
self.ax_cl.append(self.state.ax)
self.ay_cl.append(self.state.ay)
self.t_cl.append(self.exptime)
self.Fyf_cl.append(self.fssim_carinfo.Fy_f_l +
self.fssim_carinfo.Fy_f_r)
self.Fyr_cl.append(self.fssim_carinfo.Fy_r)
self.Fx_cl.append(self.fssim_carinfo.Fx)
# store CL traj as dict
if (self.explog_iterationcounter == N
): # store N+1 values, same as trajstar
self.trajcl_dict = {
"X": np.array(self.X_cl),
"Y": np.array(self.Y_cl),
"psi": np.array(self.psi_cl),
"s": np.array(self.s_cl),
"d": np.array(self.d_cl),
"deltapsi": np.array(self.deltapsi_cl),
"psidot": np.array(self.psidot_cl),
"vx": np.array(self.vx_cl),
"vy": np.array(self.vy_cl),
"ax": np.array(self.ax_cl),
"ay": np.array(self.ay_cl),
"t": np.array(self.t_cl),
"Fyf": np.array(self.Fyf_cl),
"Fyr": np.array(self.Fyr_cl),
"Fx": np.array(self.Fx_cl),
}
self.stored_trajcl = True
self.explog_iterationcounter += 1
# save explog
# debug
#print "stored_pathglobal: ", self.stored_pathglobal
#print "stored_trajstar: ", self.stored_trajstar
#print "stored_trajcl: ", self.stored_trajcl
if (self.stored_pathglobal and self.stored_trajstar
and self.stored_trajcl and not self.explog_saved):
explog = {
"pathglobal": self.pathglobal_dict,
"trajstar": self.trajstar_dict,
"trajcl": self.trajcl_dict,
}
np.save(filename, explog)
self.explog_saved = True
print "SAVED EXPLOG"
else: # not reached activation time
rospy.loginfo_throttle(
1, "Experiment starting in %i seconds" %
(self.t_activate - self.exptime))
# handle exptime
self.exptime += self.dt_sim
msg = Clock()
t_rostime = rospy.Time(self.exptime)
msg.clock = t_rostime
#self.clockpub.publish(msg)
time.sleep(self.dt_sim)
print 'simulation finished'
# send shutdown signal
message = 'run finished, shutting down'
print message
rospy.signal_shutdown(message)
pathglobal = pathglobal_npy.item() start = 500 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
def liveplot(self):
tm = time.time()
# clear figure
self.a0.cla()
self.a1.cla()
N = len(self.trajstar.s)
# plot lane lines
nplot = int(1.0 * len(self.pathlocal.s))
Xc = np.array(self.pathlocal.X[0:nplot])
Yc = np.array(self.pathlocal.Y[0:nplot])
dllub = np.array(self.pathlocal.dub[0:nplot])
drlub = np.array(self.pathlocal.dlb[0:nplot])
psic = np.array(self.pathlocal.psi_c[0:nplot])
llX = Xc - dllub * np.sin(psic)
llY = Yc + dllub * np.cos(psic)
rlX = Xc - drlub * np.sin(psic)
rlY = Yc + drlub * np.cos(psic)
self.a0.plot(llX, llY, 'k')
self.a0.plot(rlX, rlY, 'k')
# plot obstacles
Nobs = len(self.obstacles.s)
for i in range(Nobs):
# transform to cartesian
#spt = self.obstacles.s[i]
#dpt = self.obstacles.d[i]
Xobs,Yobs = ptsFrenetToCartesian(self.obstacles.s[i], \
self.obstacles.d[i], \
self.pathlocal.X, \
self.pathlocal.Y, \
self.pathlocal.psi_c, \
self.pathlocal.s)
#Xcpt = np.interp(spt,self.pathlocal.s,self.pathlocal.X)
#Ycpt = np.interp(spt,self.pathlocal.s,self.pathlocal.Y)
#psicpt = np.interp(spt,self.pathlocal.s,self.pathlocal.psi_c)
#Xobs,Yobs = ptsFrenetToCartesian(Xcpt,Ycpt,psicpt,dpt)
# define obstacle circle
Robs = self.obstacles.R[i]
Rmgnobs = self.obstacles.Rmgn[i]
Xpts, Ypts = getCirclePts(Xobs, Yobs, Rmgnobs, 20)
self.a0.plot(Xpts, Ypts, 'r')
Xpts, Ypts = getCirclePts(Xobs, Yobs, Robs, 20)
self.a0.plot(Xpts, Ypts, 'r')
# plot trajhat
self.a0.plot(self.trajhat.X, self.trajhat.Y, '.b')
# plot state constraint
for k in range(N):
slb = self.trajhat.slb[k]
sub = self.trajhat.sub[k]
dlb = self.trajhat.dlb[k]
dub = self.trajhat.dub[k]
n_intp = 3
args = (np.linspace(sub, sub,
n_intp), np.linspace(sub, slb, n_intp),
np.linspace(slb, slb,
n_intp), np.linspace(slb, sub, n_intp))
spts = np.concatenate(args)
args = (np.linspace(dub, dlb,
n_intp), np.linspace(dlb, dlb, n_intp),
np.linspace(dlb, dub,
n_intp), np.linspace(dub, dub, n_intp))
dpts = np.concatenate(args)
Xpts,Ypts = ptsFrenetToCartesian(spts, \
dpts, \
self.pathlocal.X, \
self.pathlocal.Y, \
self.pathlocal.psi_c, \
self.pathlocal.s)
#Xcpts = np.interp(spts,self.pathlocal.s,self.pathlocal.X)
#Ycpts = np.interp(spts,self.pathlocal.s,self.pathlocal.Y)
#psicpts = np.interp(spts,self.pathlocal.s,self.pathlocal.psi_c)
#Xpts, Ypts = ptsFrenetToCartesian(Xcpts, Ycpts, psicpts,dpts)
self.a0.plot(Xpts, Ypts, 'm')
# plot trajstar
self.a0.plot(self.trajstar.X, self.trajstar.Y, '*m')
# plot ego vehicle pose
length_front = self.sp.lf + 0.75
length_rear = self.sp.lr + 0.75
width = self.sp.w
R = np.matrix([[np.cos(self.state.psi), np.sin(self.state.psi)], \
[-np.sin(self.state.psi), np.cos(self.state.psi)]])
corners = np.matrix([[length_front, width/2], \
[-length_rear, width/2], \
[-length_rear, -width/2], \
[length_front, -width/2], \
[length_front, width/2]])
corners = corners * R
corners[:, 0] = corners[:, 0] + self.state.X
corners[:, 1] = corners[:, 1] + self.state.Y
self.a0.plot(corners[:, 0], corners[:, 1], 'k')
# plot real friction circle
# plot algo friction circle
Fxfpts, Fyfpts = getCirclePts(0, 0, self.dp.mu_alg * self.dp.Fzf, 100)
self.a1.plot(Fxfpts, Fyfpts, 'k')
# plot trajhat forces
#print len(self.trajhat.Fxf)
#print len(self.trajhat.Fyf)
#self.a1.plot(self.trajhat.Fxf, self.trajhat.Fyf, 'xr')
# plot trajstar forces [x / myInt for x in myList]
self.a1.plot([x / 2.0 for x in self.trajstar.Fx], self.trajstar.Fyf,
'xb')
self.a1.set_xlabel('Fxf')
self.a1.set_ylabel('Fyf')
# redraw plot
plt.draw()
plt.pause(0.001)
# print plot time
elapsed = time.time() - tm
f0_ax7.set_title("Fx")
f0_ax8 = f0.add_subplot(gs[2, 2])
f0_ax8.set_title("Fxr (u3)")
f0_ax9 = f0.add_subplot(gs[3, 2])
f0_ax9.set_title("Ff")
f0_ax10 = f0.add_subplot(gs[4, 2])
f0_ax10.set_title("Fr")
#
# OVERHEAD VIEW
#
# global path
Xll,Yll = ptsFrenetToCartesian(np.array(pathglobal['s']), \
np.array(pathglobal['dub']), \
np.array(pathglobal['X']), \
np.array(pathglobal['Y']), \
np.array(pathglobal['psi_c']), \
np.array(pathglobal['s']))
Xrl,Yrl = ptsFrenetToCartesian(np.array(pathglobal['s']), \
np.array(pathglobal['dlb']), \
np.array(pathglobal['X']), \
np.array(pathglobal['Y']), \
np.array(pathglobal['psi_c']), \
np.array(pathglobal['s']))
f0_ax0.plot(Xll, Yll, 'k')
f0_ax0.plot(Xrl, Yrl, 'k')
# planned traj
#f0_ax0.plot(trajstar["X"],trajstar["Y"],'b',linewidth=3.0)
line_trajstar = f0_ax0.add_collection(
def __init__(self):
rospy.init_node('track_interface', anonymous=True)
self.pathglobalpub = rospy.Publisher('pathglobal', Path, queue_size=10)
self.pathglobalvispub = rospy.Publisher('pathglobal_vis', navPath, queue_size=10)
self.dubvispub = rospy.Publisher('dubglobal_vis', navPath, queue_size=10)
self.dlbvispub = rospy.Publisher('dlbglobal_vis', navPath, queue_size=10)
self.track_sub = rospy.Subscriber("/fssim/track", Track, self.track_callback)
self.track = Track()
self.pathglobal = Path()
self.received_track = False
self.rate = rospy.Rate(1)
# set params
ds = 1.0 # step size in s # 0.5 nominal
plot_track = False
plot_orientation = False
plot_dlbdub = False
plot_mu = False
self.s_begin_mu_segments = rospy.get_param('/s_begin_mu_segments')
self.mu_segment_values = rospy.get_param('/mu_segment_values')
self.N_mu_segments = len(self.s_begin_mu_segments)
# wait for track
while(not self.received_track):
print "waiting for track"
self.rate.sleep()
# get cone positions left and right as numpy arrays
cl_X = []
cl_Y = []
for i in range(len(self.track.cones_left)):
cl_X.append(self.track.cones_left[i].x)
cl_Y.append(self.track.cones_left[i].y)
cl_X = np.array(cl_X)
cl_Y = np.array(cl_Y)
print "nr of cones left: ", cl_X.size
cr_X = []
cr_Y = []
for i in range(len(self.track.cones_right)):
cr_X.append(self.track.cones_right[i].x)
cr_Y.append(self.track.cones_right[i].y)
cr_X = np.array(cr_X)
cr_Y = np.array(cr_Y)
print "nr of cones right: ", cr_X.size
# compute rough centerline
ccl_X = []
ccl_Y = []
for i in range(cl_X.size):
pt_left = {"X": cl_X[i], "Y": cl_Y[i]}
# find closest right cone
dist = np.sqrt((cr_X-pt_left["X"])**2 + (cr_Y-pt_left["Y"])**2)
idx = np.argmin(dist)
pt_right = {"X": cr_X[idx], "Y": cr_Y[idx]}
pt_mid = {"X": 0.5*(pt_left["X"]+pt_right["X"]), "Y": 0.5*(pt_left["Y"]+pt_right["Y"])}
ccl_X.append(pt_mid["X"])
ccl_Y.append(pt_mid["Y"])
ccl_X = np.array(ccl_X)
ccl_Y = np.array(ccl_Y)
# get approximate length of track
stot = 0
for i in range(ccl_X.size-1):
stot += np.sqrt((ccl_X[i+1]-ccl_X[i])**2 + (ccl_Y[i+1]-ccl_Y[i])**2)
stot = (stot//ds)*ds
print "length of track: stot = ", str(stot)
# set s
s = np.arange(0, stot, ds)
# parametric spline interpolation
print "spline interpolation of centerline"
N = int(stot/ds)
unew = np.arange(0, 1.0, 1.0/N) # N equidistant pts
# center
tck, u = interpolate.splprep([ccl_X, ccl_Y], s=0)
out = interpolate.splev(unew, tck)
fcl_X = out[0]
fcl_Y = out[1]
# left
tck, u = interpolate.splprep([cl_X, cl_Y], s=0)
out = interpolate.splev(unew, tck)
fll_X = out[0]
fll_Y = out[1]
# right
tck, u = interpolate.splprep([cr_X, cr_Y], s=0)
out = interpolate.splev(unew, tck)
frl_X = out[0]
frl_Y = out[1]
# set psic
print "computing psic"
dX = np.diff(fcl_X)
dY = np.diff(fcl_Y)
psic = np.arctan2(dY,dX)
psic_final = np.arctan2(fcl_Y[0]-fcl_Y[-1],fcl_X[0]-fcl_X[-1])
psic = np.append(psic,psic_final) # assuming closed track
# separate in pieces (for 2pi flips)
idx_low = 0
idx_high = 0
psic_piecewise = []
s_piecewise = []
for i in range(psic.size-1):
if(np.abs(psic[i+1] - psic[i]) > np.pi ):
# if single pt, remove
if(np.abs(psic[i+2] - psic[i]) < np.pi ):
print("removing single flip")
psic[i+1] = psic[i]
# otherwise make a piece
else:
print("making pieces (psic flips)")
idx_high = i+1
psic_piece = psic[idx_low:idx_high]
psic_piecewise.append(psic_piece)
s_piece = s[idx_low:idx_high]
s_piecewise.append(s_piece)
idx_low = i+1
# add final piece
psic_piece = psic[idx_low:psic.size]
psic_piecewise.append(psic_piece)
s_piece = s[idx_low:psic.size]
s_piecewise.append(s_piece)
# shift pieces to make continous psi_c
for j in range(len(psic_piecewise)-1):
while(psic_piecewise[j][-1] - psic_piecewise[j+1][0] > np.pi):
psic_piecewise[j+1] = psic_piecewise[j+1] + 2*np.pi
while(psic_piecewise[j][-1] - psic_piecewise[j+1][0] <= -np.pi):
psic_piecewise[j+1] = psic_piecewise[j+1] - 2*np.pi
# concatenate pieces
psic_cont = psic_piecewise[0]
for j in range(len(psic_piecewise)-1):
psic_cont = np.concatenate((psic_cont,psic_piecewise[j+1]))
# downsample for smoother curve
step = 5 #11
print "interpolating downsampled psic with step size ", str(step)
s_ds = s[0::step]
s_ds = np.append(s_ds,s[-1]) # append final value for closed circuit
psic_ds = psic_cont[0::step]
psic_ds = np.append(psic_ds,psic_cont[-1]) # append final value for closed circuit
# interpolate
t, c, k = interpolate.splrep(s_ds, psic_ds, s=0, k=4)
psic_spl = interpolate.BSpline(t, c, k, extrapolate=False)
# compute derrivatives (compare with naive numerical)
print "computing derivatives of psic"
kappac_spl = psic_spl.derivative(nu=1)
kappacprime_spl = psic_spl.derivative(nu=2)
# put psi_c back on interval [-pi,pi]
psic_out = psic_spl(s)
#plt.plot(psic_out)
psic_out = angleToInterval(psic_out)
# tmp test helpers
#plt.plot(psic_out,'*')
#psic_out = angleToContinous(psic_out)
#plt.plot(psic_out,'.')
#plt.show()
# set kappac
kappac_out = kappac_spl(s)
kappacprime_out = kappacprime_spl(s)
# set dlb and dub
print "setting dlb and dub"
dub = []
dlb = []
for i in range(N):
X = fcl_X[i]
Y = fcl_Y[i]
# left
dub_ele = np.amin(np.sqrt((fll_X-X)**2 + (fll_Y-Y)**2))
# right
dlb_ele = -np.amin(np.sqrt((frl_X-X)**2 + (frl_Y-Y)**2))
# correction if dlb or dub w.r.t zero division in dynamics
th = 0.3
while(abs(1.0-dub_ele*kappac_out[i]) < th):
dub_ele=dub_ele-0.01
print "adjusting dub"
while(abs(1.0-dlb_ele*kappac_out[i]) < th):
dlb_ele=dlb_ele+0.01
print "adjusting dub"
dub.append(dub_ele)
dlb.append(dlb_ele)
dub = np.array(dub)
dlb = np.array(dlb)
# set mu
mu = []
for i in range(N):
mu_ele = self.mu_segment_values[-1]
for j in range(self.N_mu_segments-1):
if(self.s_begin_mu_segments[j]-0.01 <= s[i] <= self.s_begin_mu_segments[j+1]):
mu_ele = self.mu_segment_values[j]
break
mu.append(mu_ele)
mu = np.array(mu)
# plot to see what we're doing
if plot_orientation:
fig, axs = plt.subplots(3,1)
axs[0].plot(s,psic,'k*')
axs[0].set_title('psic')
#for j in range(len(psic_piecewise)):
# ax.plot(s_piecewise[j],psic_piecewise[j],'.')
#ax.plot(s,psic_cont,'r')
axs[0].plot(s,psic_out,'m.')
axs[1].plot(s,kappac_out,'.m')
axs[1].set_title('kappac')
plt.show()
if plot_track:
fig, ax = plt.subplots()
ax.axis("equal")
ax.plot(fcl_X,fcl_Y, '.b') # fine centerline
ax.plot(fll_X,fll_Y, '.b') # fine left line
ax.plot(frl_X,frl_Y, '.b') # fine right line
ax.plot(cl_X,cl_Y, '*k') # cones left
ax.plot(cr_X,cr_Y, '*k') # cones right
ax.plot(ccl_X,ccl_Y, '*r') # coarse centerline
plt.show()
if plot_dlbdub:
fig, axs = plt.subplots(3,1)
axs[0].plot(s,dlb,'.b')
axs[1].plot(s,dub,'.b')
axs[2].plot(s,1.0/kappac_out,'r.')
#axs[2]([np.min(dlb),np.max(dub)])
plt.show()
if plot_mu:
fig, ax = plt.subplots()
ax.plot(s,mu,'.b')
plt.show()
# put all in message and publish
self.pathglobal.X = fcl_X
self.pathglobal.Y = fcl_Y
self.pathglobal.s = s
self.pathglobal.psi_c = psic_out
self.pathglobal.kappa_c = kappac_out
self.pathglobal.kappaprime_c = kappacprime_out
self.pathglobal.theta_c = np.zeros(N) # grade/bank implement later
self.pathglobal.mu = mu
self.pathglobal.dub = dub
self.pathglobal.dlb = dlb
print "publishing pathglobal"
self.pathglobalpub.publish(self.pathglobal)
# publish paths for rviz
pathglobalvis = navPath()
for i in range(N):
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "map"
pose.pose.position.x = fcl_X[i]
pose.pose.position.y = fcl_Y[i]
quaternion = tf.transformations.quaternion_from_euler(0, 0, psic[i])
pose.pose.orientation.x = quaternion[0]
pose.pose.orientation.y = quaternion[1]
pose.pose.orientation.z = quaternion[2]
pose.pose.orientation.w = quaternion[3]
pathglobalvis.poses.append(pose)
pathglobalvis.header.stamp = rospy.Time.now()
pathglobalvis.header.frame_id = "map"
print "publishing pathglobal visualization"
self.pathglobalvispub.publish(pathglobalvis)
# test correctness of dub and dlb in rviz
Xleft,Yleft = ptsFrenetToCartesian(s,dub,fcl_X,fcl_Y,psic,s)
pathleft = navPath()
pathleft.header.stamp = rospy.Time.now()
pathleft.header.frame_id = "map"
for i in range(N):
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "map"
pose.pose.position.x = Xleft[i]
pose.pose.position.y = Yleft[i]
pathleft.poses.append(pose)
self.dubvispub.publish(pathleft)
Xright,Yright = ptsFrenetToCartesian(s,dlb,fcl_X,fcl_Y,psic,s)
pathright = navPath()
pathright.header.stamp = rospy.Time.now()
pathright.header.frame_id = "map"
for i in range(N):
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "map"
pose.pose.position.x = Xright[i]
pose.pose.position.y = Yright[i]
pathright.poses.append(pose)
self.dlbvispub.publish(pathright)