def main():
homedir = os.path.expanduser("~")
file_data = open(homedir + '/barc_data/ClosedLoopDataLMPC.obj', 'rb')
ClosedLoopData = pickle.load(file_data)
LMPController = pickle.load(file_data)
LMPCOpenLoopData = pickle.load(file_data)
file_data.close()
map = Map("oval")
pdb.set_trace()
print "Track length is: ", map.TrackLength
plotOneStepPreditionError(LMPController, LMPCOpenLoopData)
plt.show()
plotClosedLoopLMPC(LMPController, map)
plt.show()
pdb.set_trace()
animation_states(map, LMPCOpenLoopData, LMPController, 10)
plt.show()
def main():
homedir = os.path.expanduser("~")
file_data = open(homedir + '/barc_data/ClosedLoopDataCircularTest.obj',
'rb')
ClosedLoopData = pickle.load(file_data)
file_data.close()
map = Map("circle")
plotTrajectory(map, ClosedLoopData)
plt.show()
def main():
rospy.init_node("realTimePlotting")
mode = rospy.get_param("/control/mode") # testing planner
plotGPS = rospy.get_param("/visualization/plotGPS")
data = Estimation_Mesures_Planning_Data(mode, plotGPS)
map = Map()
loop_rate = 30.0
rate = rospy.Rate(loop_rate)
vSwitch = 1.3
psiSwitch = 1.2
StateView = False
if StateView == True:
fig, linevx, linevy, linewz, lineepsi, lineey, line_tr, line_pred = _initializeFigure(
map)
else:
Planning_Track = rospy.get_param("/TrajectoryPlanner/Planning_Track")
# Planning_Track = 1
planning_path = '/home/euge/GitHub/barc/workspace/src/barc/src/data/Planner_Refs'
if Planning_Track == 1:
X_Planner_Pts = sio.loadmat(planning_path +
'/GOOD_PLAN_REFS_1_.mat')['pxp']
Y_Planner_Pts = sio.loadmat(planning_path +
'/GOOD_PLAN_REFS_1_.mat')['pyp']
elif Planning_Track == 2:
X_Planner_Pts = sio.loadmat(planning_path +
'/GOOD_PLAN_REFS_2_.mat')['pxp']
Y_Planner_Pts = sio.loadmat(planning_path +
'/GOOD_PLAN_REFS_2_.mat')['pyp']
elif Planning_Track == 3:
X_Planner_Pts = sio.loadmat(planning_path +
'/GOOD_PLAN_REFS_3_.mat')['pxp']
Y_Planner_Pts = sio.loadmat(planning_path +
'/GOOD_PLAN_REFS_3_.mat')['pyp']
elif Planning_Track == 4:
X_Planner_Pts = sio.loadmat(planning_path +
'/GOOD_PLAN_REFS_4_.mat')['pxp']
Y_Planner_Pts = sio.loadmat(planning_path +
'/GOOD_PLAN_REFS_4_.mat')['pyp']
(fig, axtr, line_planning, line_tr, line_pred, line_SS, line_cl,
line_gps_cl, rec, rec_sim,
rec_planning) = _initializeFigure_xy(map, mode, X_Planner_Pts,
Y_Planner_Pts)
ClosedLoopTraj_gps_x = []
ClosedLoopTraj_gps_y = []
ClosedLoopTraj_x = []
ClosedLoopTraj_y = []
flagHalfLap = False
while not rospy.is_shutdown():
estimatedStates = data.readEstimatedData()
s, ey, epsi, insideMap = map.getLocalPosition(estimatedStates[3],
estimatedStates[4],
estimatedStates[5])
if s > map.TrackLength / 2:
flagHalfLap = True
if (s < map.TrackLength / 4) and (flagHalfLap == True): # New lap
ClosedLoopTraj_gps_x = []
ClosedLoopTraj_gps_y = []
ClosedLoopTraj_x = []
ClosedLoopTraj_y = []
flagHalfLap = False
x = estimatedStates[3]
y = estimatedStates[4]
if plotGPS == True:
ClosedLoopTraj_gps_x.append(data.MeasuredData[0])
ClosedLoopTraj_gps_y.append(data.MeasuredData[1])
ClosedLoopTraj_x.append(x)
ClosedLoopTraj_y.append(y)
psi = estimatedStates[5]
l = 0.4
w = 0.2
line_cl.set_data(ClosedLoopTraj_x, ClosedLoopTraj_y)
if plotGPS == True:
line_gps_cl.set_data(ClosedLoopTraj_gps_x, ClosedLoopTraj_gps_y)
if StateView == True:
linevx.set_data(0.0, estimatedStates[0])
linevy.set_data(0.0, estimatedStates[1])
linewz.set_data(0.0, estimatedStates[2])
lineepsi.set_data(0.0, epsi)
lineey.set_data(0.0, ey)
# Plotting the estimated car:
line_tr.set_data(estimatedStates[3], estimatedStates[4])
car_x, car_y = getCarPosition(x, y, psi, w, l)
rec.set_xy(np.array([car_x, car_y]).T)
# Plotting the planner car and trajectory:
# line_planning.set_data(data.x_d[:], data.y_d[:])
# car_x, car_y = getCarPosition(data.x_d[0], data.y_d[0], data.psi_d[0], w, l)
# rec_planning.set_xy(np.array([car_x, car_y]).T)
if mode == "simulations":
x_sim = data.sim_x[-1]
y_sim = data.sim_y[-1]
psi_sim = data.sim_psi[-1]
car_sim_x, car_sim_y = getCarPosition(x_sim, y_sim, psi_sim, w, l)
rec_sim.set_xy(np.array([car_sim_x, car_sim_y]).T)
# StringValue = "Vx = {0:0.2f}".format(estimatedStates[0]), " Vy = {0:0.2f}".format(estimatedStates[1]), "psiDot = {0:0.2f}".format(estimatedStates[2])
StringValue = "Vx = {0:0.2f}".format(
estimatedStates[0]), " Vx_Ref = {0:0.2f}".format(data.vx_d[0])
axtr.set_title(StringValue)
if insideMap == 1:
fig.canvas.draw()
rate.sleep()
def main():
# node initialization
rospy.init_node("state_estimation")
a_delay = rospy.get_param("state_estimator/delay_a")
df_delay = rospy.get_param("state_estimator/delay_df")
loop_rate = 50.0
# Initialize Map
map = Map()
Q = eye(8)
Q[0,0] = rospy.get_param("state_estimator/Q_x")
Q[1,1] = rospy.get_param("state_estimator/Q_y")
Q[2,2] = rospy.get_param("state_estimator/Q_vx")
Q[3,3] = rospy.get_param("state_estimator/Q_vy")
Q[4,4] = rospy.get_param("state_estimator/Q_ax")
Q[5,5] = rospy.get_param("state_estimator/Q_ay")
Q[6,6] = rospy.get_param("state_estimator/Q_psi")
Q[7,7] = rospy.get_param("state_estimator/Q_psiDot")
R = eye(6)
R[0,0] = rospy.get_param("state_estimator/R_x")
R[1,1] = rospy.get_param("state_estimator/R_y")
R[2,2] = rospy.get_param("state_estimator/R_vx")
R[3,3] = rospy.get_param("state_estimator/R_ax")
R[4,4] = rospy.get_param("state_estimator/R_ay")
R[5,5] = rospy.get_param("state_estimator/R_psiDot")
# R[6,6] = rospy.get_param("state_estimator/R_vy")
t0 = rospy.get_rostime().to_sec()
imu = ImuClass(t0)
gps = GpsClass(t0)
enc = EncClass(t0)
ecu = EcuClass(t0)
est = Estimator(t0,loop_rate,a_delay,df_delay,Q,R)
estMsg = pos_info()
while not rospy.is_shutdown():
est.estimateState(imu,gps,enc,ecu,est.ekf)
estMsg.s, estMsg.ey, estMsg.epsi, _ = map.getLocalPosition(est.x_est, est.y_est, est.yaw_est)
estMsg.header.stamp = rospy.get_rostime()
estMsg.v = np.sqrt(est.vx_est**2 + est.vy_est**2)
estMsg.x = est.x_est
estMsg.y = est.y_est
estMsg.v_x = est.vx_est
estMsg.v_y = est.vy_est
estMsg.psi = est.yaw_est
estMsg.psiDot = est.psiDot_est
estMsg.a_x = est.ax_est
estMsg.a_y = est.ay_est
estMsg.u_a = ecu.a
estMsg.u_df = ecu.df
est.state_pub_pos.publish(estMsg)
est.saveHistory()
est.rate.sleep()
homedir = os.path.expanduser("~")
pathSave = os.path.join(homedir,"barc_debugging/estimator_output.npz")
np.savez(pathSave,yaw_est_his = est.yaw_est_his,
psiDot_est_his = est.psiDot_est_his,
x_est_his = est.x_est_his,
y_est_his = est.y_est_his,
vx_est_his = est.vx_est_his,
vy_est_his = est.vy_est_his,
ax_est_his = est.ax_est_his,
ay_est_his = est.ay_est_his,
estimator_time = est.time_his)
pathSave = os.path.join(homedir,"barc_debugging/estimator_imu.npz")
np.savez(pathSave,psiDot_his = imu.psiDot_his,
roll_his = imu.roll_his,
pitch_his = imu.pitch_his,
yaw_his = imu.yaw_his,
ax_his = imu.ax_his,
ay_his = imu.ay_his,
imu_time = imu.time_his)
pathSave = os.path.join(homedir,"barc_debugging/estimator_gps.npz")
np.savez(pathSave,x_his = gps.x_his,
y_his = gps.y_his,
gps_time = gps.time_his)
pathSave = os.path.join(homedir,"barc_debugging/estimator_enc.npz")
np.savez(pathSave,v_fl_his = enc.v_fl_his,
v_fr_his = enc.v_fr_his,
v_rl_his = enc.v_rl_his,
v_rr_his = enc.v_rr_his,
enc_time = enc.time_his)
pathSave = os.path.join(homedir,"barc_debugging/estimator_ecu.npz")
np.savez(pathSave,a_his = ecu.a_his,
df_his = ecu.df_his,
ecu_time = ecu.time_his)
print "Finishing saveing state estimation data"
def main():
rospy.init_node("LPV-MPC")
input_commands = rospy.Publisher('ecu', ECU, queue_size=1)
pred_treajecto = rospy.Publisher('OL_predictions',
prediction,
queue_size=1)
racing_info = rospy.Publisher('Racing_Info', Racing_Info, queue_size=1)
mode = rospy.get_param("/control/mode")
N = rospy.get_param("/control/N")
loop_rate = 30.0
dt = 1.0 / loop_rate
rate = rospy.Rate(loop_rate)
## TODO: change this in order to be taken from the launch file
Steering_Delay = 0 #3
Velocity_Delay = 0
#Steering_Delay = int(rospy.get_param("/simulator/delay_df")/dt)
NN_LPV_MPC = False
#########################################################
#########################################################
# OFFLINE planning from LPV-MPP racing:
planning_path = '/home/euge/GitHub/barc/workspace/src/barc/src/data/Planner_Refs'
## TRACK (OVAL, Racing Planner) : (30 July 19)
CURV_Planner = sio.loadmat(planning_path +
'/LPV_MPC_PLANNING_1_.mat')['pCURV']
VEL_Planner = sio.loadmat(planning_path +
'/LPV_MPC_PLANNING_1_.mat')['pnew_Vx']
X_Planner = sio.loadmat(planning_path + '/LPV_MPC_PLANNING_1_.mat')['pxp']
Y_Planner = sio.loadmat(planning_path + '/LPV_MPC_PLANNING_1_.mat')['pyp']
PSI_Planner = sio.loadmat(planning_path +
'/LPV_MPC_PLANNING_1_.mat')['pyaw']
#########################################################
#########################################################
# Objects initializations
OL_predictions = prediction()
cmd = ECU() # Command message
rac_info = Racing_Info()
cmd.servo = 0.0
cmd.motor = 0.0
ClosedLoopData = ClosedLoopDataObj(dt, 6000, 0) # Closed-Loop Data
estimatorData = EstimatorData()
map = Map() # Map
planning_data = PlanningData()
first_it = 1
NumberOfLaps = 10
# Initialize variables for main loop
GlobalState = np.zeros(6)
LocalState = np.zeros(6)
HalfTrack = 0
LapNumber = 0
RunController = 1
Counter = 0
CounterRacing = 0
uApplied = np.array([0.0, 0.0])
oldU = np.array([0.0, 0.0])
RMSE_ve = np.zeros(NumberOfLaps)
RMSE_ye = np.zeros(NumberOfLaps)
RMSE_thetae = np.zeros(NumberOfLaps)
RMSE_acc_y = np.zeros(NumberOfLaps)
RMSE_matrix = np.zeros(NumberOfLaps)
Norm_matrix = np.zeros((NumberOfLaps, 3))
ALL_LOCAL_DATA = np.zeros(
(2000, 8)) # [vx vy psidot thetae s ye vxaccel vyaccel udelta uaccel]
GLOBAL_DATA = np.zeros((2000, 3)) # [x y psi]
PREDICTED_DATA = np.zeros(
(2000, 120)) # [vx vy psidot thetae s ye] presicted to N steps
TLAPTIME = np.zeros((30, 1))
ELAPSD_TIME = np.zeros((2000, 1))
IDENT_DATA = np.zeros((2000, 5))
Data_for_RMSE = np.zeros((2000, 4))
References = np.array([0.0, 0.0, 0.0, 1.0])
# Loop running at loop rate
TimeCounter = 0
PlannerCounter = 0
count = True
index = 0
start_LapTimer = datetime.datetime.now()
insideTrack = 1
rospy.sleep(
1
) # Soluciona los problemas de inicializacion esperando a que el estimador se inicialice bien
vel_ref = 1
Cf_new = 60
SS = 0
###----------------------------------------------------------------###
### PATH TRACKING TUNING:
### 33 ms - 20 Hp
Q = np.diag([100.0, 1.0, 1.0, 20.0, 0.0, 900.0])
R = 0.5 * 0.5 * np.diag([1.0, 1.0]) # delta, a
dR = 1.5 * 25 * np.array([1.3, 1.0]) # Input rate cost u
Controller = PathFollowingLPV_MPC(Q, R, dR, N, vel_ref, dt, map, "OSQP",
Steering_Delay, Velocity_Delay)
# ### RACING TAJECTORY TRACKING TUNING:
# ### 33 ms - 20 Hp
Q = np.diag([400.0, 1.0, 1.0, 20.0, 0.0, 1100.0])
R = 0.0 * np.diag([1.0, 1.0]) # delta, a
dR = np.array([100.0, 45.0]) # Input rate cost u
# dR = np.array([ 10.0, 10.0 ]) # Input rate cost u
Controller_TT = PathFollowingLPV_MPC(Q, R, dR, N, vel_ref, dt, map, "OSQP",
Steering_Delay, Velocity_Delay)
### RACING TAJECTORY TRACKING TUNING:
### 33 ms - 20 Hp
# Q = np.diag([800.0, 1.0, 2.0, 10.0, 0.0, 1200.0])
# R = np.diag([0.0, 0.0]) # delta, a
# # R = np.array([146.0, 37.0]) # Input rate cost u
# dR = np.array([20, 30.0]) # Input rate cost u
# Controller_TT = PathFollowingLPV_MPC(Q, R, dR, N, vel_ref, dt, map, "OSQP", Steering_Delay, Velocity_Delay)
###----------------------------------------------------------------###
###----------------------------------------------------------------###
###----------------------------------------------------------------###
###----------------------------------------------------------------###
L_LPV_States_Prediction = np.zeros((N, 6))
LPV_States_Prediction = np.zeros((N, 6))
while (not rospy.is_shutdown()) and RunController == 1:
# Read Measurements
GlobalState[:] = estimatorData.CurrentState # The current estimated state vector [vx vy w x y psi]
LocalState[:] = estimatorData.CurrentState # [vx vy w x y psi]
if LocalState[0] < 0.01:
LocalState[0] = 0.01
if LapNumber == 0: # Path tracking:
# OUT: s, epsi, ey IN: x, y, psi
LocalState[4], LocalState[3], LocalState[
5], insideTrack = map.getLocalPosition(GlobalState[3],
GlobalState[4],
GlobalState[5])
# Check if the lap has finished
if LocalState[4] >= 3 * map.TrackLength / 4:
HalfTrack = 1
new_Ref = np.array([0.0, 0.0, 0.0, 1.0])
References = np.vstack(
(References,
new_Ref)) # Simplemente se usa para guardar datos en fichero
else: # Trajectory tracking:
GlobalState[5] = GlobalState[
5] - 2 * np.pi * LapNumber ## BODY FRAME ERROR COMPUTATION:
GlobalState[5] = wrap(GlobalState[5])
PSI_Planner[0, PlannerCounter] = wrap(PSI_Planner[0,
PlannerCounter])
# Offline planning data:
# OUT: s, ex, ey, epsi
# LocalState[4], Xerror, LocalState[3], LocalState[5] = Body_Frame_Errors(GlobalState[3],
# GlobalState[4], GlobalState[5], X_Planner[0,PlannerCounter], Y_Planner[0,PlannerCounter],
# PSI_Planner[0,PlannerCounter], SS, LocalState[0], LocalState[1], CURV_Planner[0, PlannerCounter], dt )
# print "Desired yaw = ", PSI_Planner[0,PlannerCounter], " and Real yaw = ", GlobalState[5]
# Online planning data:
new_Ref = np.array([
planning_data.x_d[0], planning_data.y_d[0],
planning_data.psi_d[0], planning_data.vx_d[0]
])
References = np.vstack((References, np.transpose(new_Ref)))
max_window = 0
if index <= max_window:
if index == 0:
X_REF_vector = planning_data.x_d[0:N + max_window]
Y_REF_vector = planning_data.y_d[0:N + max_window]
YAW_REF_vector = planning_data.psi_d[0:N + max_window]
VEL_REF_vector = planning_data.vx_d[0:N + max_window]
CURV_REF_vector = planning_data.curv_d[0:N + max_window]
x_ref = X_REF_vector[index:index + N]
y_ref = Y_REF_vector[index:index + N]
yaw_ref = YAW_REF_vector[index:index + N]
vel_ref = VEL_REF_vector[index:index + N]
curv_ref = CURV_REF_vector[index:index + N]
index += 1
else:
index = 0
# OUT: s, ex, ey, epsi
LocalState[4], Xerror, LocalState[5], LocalState[
3] = Body_Frame_Errors(GlobalState[3], GlobalState[4],
GlobalState[5], x_ref[0], y_ref[0],
yaw_ref[0], SS, LocalState[0],
LocalState[1], curv_ref[0], dt)
SS = LocalState[4]
### END OF THE LAP.
# PATH TRACKING:
if (HalfTrack == 1 and (LocalState[4] <= map.TrackLength / 4)):
HalfTrack = 0
LapNumber += 1
SS = 0
PlannerCounter = 0
TimeCounter = 0
TotalLapTime = datetime.datetime.now() - start_LapTimer
print 'Lap completed in', TotalLapTime, 'seconds. Starting lap:', LapNumber, '\n'
TLAPTIME[LapNumber - 1] = TotalLapTime.total_seconds()
start_LapTimer = datetime.datetime.now()
# RACING:
elif ((LapNumber >= 1) and (np.abs(GlobalState[3]) < 0.1)
and (LocalState[4] >= (map.TrackLength - map.TrackLength / 10))):
LapNumber += 1
SS = 0
# PlannerCounter = TrackCounter
TotalLapTime = datetime.datetime.now() - start_LapTimer
print 'Lap completed in', TotalLapTime, 'seconds. Starting lap:', LapNumber, '. PlannerCounter = ', PlannerCounter, '\n'
TLAPTIME[LapNumber - 1] = TotalLapTime.total_seconds()
start_LapTimer = datetime.datetime.now()
if LapNumber > NumberOfLaps:
print('RunController = 0')
RunController = 0
startTimer = datetime.datetime.now()
oldU = uApplied
uApplied = np.array([cmd.servo, cmd.motor])
if LapNumber == 0:
Controller.OldSteering.append(
cmd.servo) # meto al final del vector
Controller.OldAccelera.append(cmd.motor)
Controller.OldSteering.pop(0)
Controller.OldAccelera.pop(0)
else:
Controller_TT.OldSteering.append(
cmd.servo) # meto al final del vector
Controller_TT.OldAccelera.append(cmd.motor)
Controller_TT.OldSteering.pop(0)
Controller_TT.OldAccelera.pop(0)
### Publish input ###
input_commands.publish(cmd)
###################################################################################################
###################################################################################################
if first_it < 10:
# print('Iteration = ', first_it)
vel_ref = np.ones(N)
xx, uu = predicted_vectors_generation(N, LocalState, uApplied, dt)
Controller.solve(LocalState[0:6], xx, uu, NN_LPV_MPC, vel_ref, 0,
0, 0, first_it)
first_it = first_it + 1
# print('---> Controller.uPred = ', Controller.uPred)
Controller.OldPredicted = np.hstack(
(Controller.OldSteering[0:len(Controller.OldSteering) - 1],
Controller.uPred[Controller.steeringDelay:Controller.N, 0]))
Controller.OldPredicted = np.concatenate(
(np.matrix(Controller.OldPredicted).T,
np.matrix(Controller.uPred[:, 1]).T),
axis=1)
else:
NN_LPV_MPC = False
if LapNumber == 0: # PATH TRACKING : First lap at 1 m/s
vel_ref = np.ones(N + 1)
curv_ref = np.zeros(N)
LPV_States_Prediction, A_L, B_L, C_L = Controller.LPVPrediction(
LocalState[0:6], Controller.uPred, vel_ref, curv_ref,
Cf_new, LapNumber)
Controller.solve(LPV_States_Prediction[0, :],
LPV_States_Prediction, Controller.uPred,
NN_LPV_MPC, vel_ref, A_L, B_L, C_L, first_it)
# IDENT_DATA[Counter,:] = [LocalState[0], LocalState[1], LocalState[2], uApplied[0], uApplied[1]]
Controller_TT.uPred = Controller.uPred
else: # TRAJECTORY TRACKING
# Offline planning data:
# vel_ref = VEL_Planner[0, PlannerCounter:PlannerCounter+N+1]
# curv_ref = CURV_Planner[0, PlannerCounter:PlannerCounter+N]
# VEL_REF_vector = planning_data.vx_d[0:N+10]
# CURV_REF_vector = planning_data.curv_d[0:N+10]
# if index < 10:
# vel_ref = VEL_REF_vector[index:index+N]
# curv_ref = CURV_REF_vector[index:index+N]
# index += 1
# else:
# index = 0
# Online planning data:curv_ref
# vel_ref = planning_data.vx_d[0:N]
# curv_ref = planning_data.curv_d[0:N]
LPV_States_Prediction, A_L, B_L, C_L = Controller_TT.LPVPrediction(
LocalState[0:6], Controller_TT.uPred, vel_ref, curv_ref,
Cf_new, LapNumber)
Controller_TT.solve(LocalState[0:6], 0.0, Controller_TT.uPred,
NN_LPV_MPC, vel_ref, A_L, B_L, C_L,
first_it)
###################################################################################################
###################################################################################################
if first_it > 19:
new_LPV_States_Prediction = LPV_States_Prediction[0, :]
for i in range(1, N):
new_LPV_States_Prediction = np.hstack(
(new_LPV_States_Prediction, LPV_States_Prediction[i, :]))
PREDICTED_DATA[Counter, :] = new_LPV_States_Prediction
if LapNumber == 0:
cmd.servo = Controller.uPred[0 + Controller.steeringDelay, 0]
cmd.motor = Controller.uPred[0 + Controller.velocityDelay, 1]
else:
cmd.servo = Controller_TT.uPred[0 + Controller.steeringDelay, 0]
cmd.motor = Controller_TT.uPred[0 + Controller.velocityDelay, 1]
#print Controller.uPred[0, :]
endTimer = datetime.datetime.now()
deltaTimer = endTimer - startTimer
# ESTO HABRA QUE RESCATARLO PARA PRUEBAS FISICAS...
# else: # If car out of the track
# cmd.servo = 0
# cmd.motor = 0
# input_commands.publish(cmd)
## Record Prediction
if LapNumber < 1:
OL_predictions.s = Controller.xPred[:, 4]
OL_predictions.ex = []
OL_predictions.ey = Controller.xPred[:, 5]
OL_predictions.epsi = Controller.xPred[:, 3]
else:
OL_predictions.s = Controller_TT.xPred[:, 4]
OL_predictions.ex = []
OL_predictions.ey = Controller_TT.xPred[:, 5]
OL_predictions.epsi = Controller_TT.xPred[:, 3]
ALL_LOCAL_DATA[CounterRacing, :] = np.hstack(
(LocalState, uApplied))
GLOBAL_DATA[CounterRacing, :] = [
GlobalState[3], GlobalState[4], GlobalState[5]
]
CounterRacing += 1
pred_treajecto.publish(OL_predictions)
# ClosedLoopData.addMeasurement(GlobalState, LocalState, uApplied, Counter, deltaTimer.total_seconds())
# Data_for_RMSE[TimeCounter,:] = [ LocalState[0], LocalState[5], LocalState[3], LocalState[7]]
ELAPSD_TIME[Counter, :] = deltaTimer.total_seconds()
# Publishing important info about the racing:
rac_info.LapNumber = LapNumber
# rac_info.PlannerCounter = PlannerCounter
racing_info.publish(rac_info)
# Increase time counter and ROS sleep()
# print PlannerCounter
TimeCounter += 1
PlannerCounter += 1
# if PlannerCounter > 580: #offline racing planning
if PlannerCounter > 999:
cmd.servo = 0
cmd.motor = 0
input_commands.publish(cmd)
day = '31_7_19'
num_test = 'Test_1'
newpath = '/home/euge/GitHub/barc/results_simu_test/' + day + '/' + num_test + '/'
if not os.path.exists(newpath):
os.makedirs(newpath)
np.savetxt(newpath + '/ALL_LOCAL_DATA.dat',
ALL_LOCAL_DATA,
fmt='%.5e')
np.savetxt(newpath + '/PREDICTED_DATA.dat',
PREDICTED_DATA,
fmt='%.5e')
np.savetxt(newpath + '/GLOBAL_DATA.dat', GLOBAL_DATA, fmt='%.5e')
np.savetxt(newpath + '/References.dat', References, fmt='%.5e')
np.savetxt(newpath + '/TLAPTIME.dat', TLAPTIME, fmt='%.5e')
np.savetxt(newpath + '/ELAPSD_TIME.dat', ELAPSD_TIME, fmt='%.5e')
quit()
Counter += 1
rate.sleep()
# END WHILE
# Save Data
# file_data = open(sys.path[0]+'/data/'+mode+'/ClosedLoopData'+"LPV_MPC"+'.obj', 'wb')
# pickle.dump(ClosedLoopData, file_data)
# pickle.dump(Controller, file_data)
#############################################################
# file_data = open(newpath+'/ClosedLoopDataLPV_MPC.obj', 'wb')
# pickle.dump(ClosedLoopData, file_data)
# pickle.dump(Controller, file_data)
# file_data.close()
# INFO_data = open(newpath+'/INFO.txt', 'wb')
# INFO_data.write('Running in Track number ')
# INFO_data.write('%d \n' % Planning_Track)
# #INFO_data.write(' ')
# INFO_data.close()
#############################################################
# plotTrajectory(map, ClosedLoopData, Complete_Vel_Vect)
quit()
def main():
# Initializa ROS node
rospy.init_node("Trajectory_Planner")
planning_refs = rospy.Publisher('My_Planning', My_Planning, queue_size=1)
map = Map()
refs = My_Planning()
refs.x_d = []
refs.y_d = []
refs.psi_d = []
refs.vx_d = []
refs.curv_d = []
HW = rospy.get_param("/TrajectoryPlanner/halfWidth")
# HW = 0.35 # It is a bit larger than the configured in the launch with the aim of improving results
loop_rate = rospy.get_param("/TrajectoryPlanner/Frecuency") # 20 Hz (50 ms)
dt = 1.0/loop_rate
rate = rospy.Rate(loop_rate)
Testing = rospy.get_param("/TrajectoryPlanner/Testing")
if Testing == 0:
racing_info = RacingDataClass()
estimatorData = EstimatorData()
mode = rospy.get_param("/control/mode")
else: # Testing mode
mode = "simulations"
racing_info = RacingDataClass()
first_it = 1
Xlast = 0.0
Ylast = 0.0
Thetalast = 0.0
Counter = 0
ALL_LOCAL_DATA = np.zeros((1000,7)) # [vx vy psidot ey psi udelta uaccel]
References = np.zeros((1000,5))
ELAPSD_TIME = np.zeros((1000,1))
TimeCounter = 0
PlannerCounter = 0
start_LapTimer = datetime.datetime.now()
if rospy.get_param("/TrajectoryPlanner/Visualization") == 1:
fig, axtr, line_tr, line_pred, line_trs, line_cl, line_gps_cl, rec, rec_sim = InitFigure_XY( map, mode, HW )
#####################################################################
# states = [vx vy w ey epsi]
Q = - np.diag( [ -0.000000000000088, -9.703658572659423, -0.5, 0.000000000213635, -0.153591566469547] )
L = - np.array( [ 1.00702414775175, 0.187661946033823, -0.0, 0.0, -0.0329493219494661 ] )
R = np.diag([0.8, 0.0]) # Input variables weight [delta, a]. El peso de "a" ha de ser 0.
dR = np.array([6.0, 6.0]) # Input slew rate weight [d_delta, d_a]
N = rospy.get_param("/TrajectoryPlanner/N")
planner_dt = dt
# planner_dt = 0.05
Planner = LPV_MPC_Planner(Q, R, dR, L, N, planner_dt, map, "OSQP")
#####################################################################
# Filter to be applied.
b_filter, a_filter = signal.ellip(4, 0.01, 120, 0.125)
LPV_X_Pred = np.zeros((N,5))
Xref = np.zeros(N+1)
Yref = np.zeros(N+1)
Thetaref = np.zeros(N+1)
xp = np.zeros(N)
yp = np.zeros(N)
yaw = np.zeros(N)
SS = np.zeros(N+1,)
GlobalState = np.zeros(6)
LocalState = np.zeros(5)
while (not rospy.is_shutdown()):
# If inside the track publish input
if (racing_info.LapNumber >= 1 or Testing == 1):
startTimer = datetime.datetime.now()
###################################################################################################
# GETTING INITIAL STATE:
###################################################################################################
if Testing == 0:
# Read Measurements
GlobalState[:] = estimatorData.CurrentState # The current estimated state vector [vx vy w x y psi]
LocalState[:] = np.array([ GlobalState[0], GlobalState[1], GlobalState[2], 0.0, 0.0 ]) # [vx vy w ey epsi]
S_realDist, LocalState[4], LocalState[3], insideTrack = map.getLocalPosition(GlobalState[3], GlobalState[4], GlobalState[5])
else:
LocalState[:] = np.array([1.0, 0, 0, 0, 0])
S_realDist, LocalState[4], LocalState[3], insideTrack = map.getLocalPosition(xp[0], yp[0], yaw[0])
###################################################################################################
# OPTIMIZATION:
###################################################################################################
if first_it == 1:
# Resolvemos la primera OP con los datos del planner no-lineal:
# xx son los estados del modelo no lineal para la primera it.
# delta es el steering angle del modelo no lineal para la primera it.
x0 = LocalState[:] # Initial planning state
accel_rate = 0.2
xx, uu = predicted_vectors_generation(N, x0, accel_rate, planner_dt)
Planner.solve(x0, xx, uu, 0, 0, 0, first_it, HW)
first_it += 1
print "Dentro primera iter... = "
else:
# LPV_X_Pred, A_L, B_L, C_L = Planner.LPVPrediction( LocalState[:], SS[:], Planner.uPred )
# Planner.solve( LocalState[:], 0, 0, A_L, B_L, C_L, first_it, HW)
# Planner.solve(Planner.xPred[1,:], LPV_X_Pred, Planner.uPred, A_L, B_L, C_L, first_it)
LPV_X_Pred, A_L, B_L, C_L = Planner.LPVPrediction( Planner.xPred[1,:], SS[:], Planner.uPred )
Planner.solve(Planner.xPred[1,:], 0, 0, A_L, B_L, C_L, first_it, HW)
###################################################################################################
###################################################################################################
# print "Ey: ", Planner.xPred[:,3]
# Saving current control actions to perform then the slew rate:
Planner.OldSteering.append(Planner.uPred[0,0])
Planner.OldAccelera.append(Planner.uPred[0,1])
# pdb.set_trace()
#####################################
## Getting vehicle position:
#####################################
Xref[0] = Xlast
Yref[0] = Ylast
Thetaref[0] = Thetalast
# print "SS[0] = ", S_realDist
# SS[0] = S_realDist
for j in range( 0, N ):
PointAndTangent = map.PointAndTangent
curv = Curvature( SS[j], PointAndTangent )
SS[j+1] = ( SS[j] + ( ( Planner.xPred[j,0]* np.cos(Planner.xPred[j,4])
- Planner.xPred[j,1]*np.sin(Planner.xPred[j,4]) ) / ( 1-Planner.xPred[j,3]*curv ) ) * planner_dt )
Xref[j+1], Yref[j+1], Thetaref[j+1] = map.getGlobalPosition( SS[j+1], 0.0 )
SS[0] = SS[1]
Xlast = Xref[1]
Ylast = Yref[1]
Thetalast = Thetaref[1]
for i in range(0,N):
yaw[i] = Thetaref[i] + Planner.xPred[i,4]
xp[i] = Xref[i] - Planner.xPred[i,3]*np.sin(yaw[i])
yp[i] = Yref[i] + Planner.xPred[i,3]*np.cos(yaw[i])
vel = Planner.xPred[0:N,0]
curv = Planner.xPred[0:N,2] / Planner.xPred[0:N,0]
endTimer = datetime.datetime.now()
deltaTimer = endTimer - startTimer
ELAPSD_TIME[Counter,:] = deltaTimer.total_seconds()
#####################################
## Plotting vehicle position:
#####################################
if rospy.get_param("/TrajectoryPlanner/Visualization") == 1:
line_trs.set_data(xp[0:N/2], yp[0:N/2])
line_pred.set_data(xp[N/2:], yp[N/2:])
l = 0.4; w = 0.2
car_sim_x, car_sim_y = getCarPosition(xp[0], yp[0], yaw[0], w, l)
# car_sim_x, car_sim_y = getCarPosition(xp[N-1], yp[N-1], yaw[N-1], w, l)
rec_sim.set_xy(np.array([car_sim_x, car_sim_y]).T)
fig.canvas.draw()
StringValue = "vx = "+str(Planner.xPred[0,0])
axtr.set_title(StringValue)
#####################################
## Interpolating vehicle references:
#####################################
interp_dt = 0.033
time50ms = np.linspace(0, N*dt, num=N, endpoint=True)
time33ms = np.linspace(0, N*dt, num=np.around(N*dt/interp_dt), endpoint=True)
# X
f = interp1d(time50ms, xp, kind='cubic')
X_interp = f(time33ms)
# Y
f = interp1d(time50ms, yp, kind='cubic')
Y_interp = f(time33ms)
# Yaw
f = interp1d(time50ms, yaw, kind='cubic')
Yaw_interp = f(time33ms)
# Velocity (Vx)
f = interp1d(time50ms, vel, kind='cubic')
Vx_interp = f(time33ms)
# Curvature (K)
f = interp1d(time50ms, curv, kind='cubic')
Curv_interp = f(time33ms)
Curv_interp_filtered = signal.filtfilt(b_filter, a_filter, Curv_interp, padlen=50)
# plt.clf()
# plt.figure(2)
# plt.plot(Curv_interp, 'k-', label='input')
# plt.plot(Curv_interp_filtered, 'c-', linewidth=1.5, label='pad')
# plt.legend(loc='best')
# plt.show()
# plt.grid()
# pdb.set_trace()
#####################################
## Publishing vehicle references:
#####################################
# refs.x_d = xp
# refs.y_d = yp
# refs.psi_d = yaw
# refs.vx_d = vel
# refs.curv_d = curv
refs.x_d = X_interp
refs.y_d = Y_interp
refs.psi_d = Yaw_interp
refs.vx_d = Vx_interp
refs.curv_d = Curv_interp_filtered
planning_refs.publish(refs)
ALL_LOCAL_DATA[Counter,:] = np.hstack(( Planner.xPred[0,:], Planner.uPred[0,:] ))
References[Counter,:] = np.hstack(( refs.x_d[0], refs.y_d[0], refs.psi_d[0], refs.vx_d[0], refs.curv_d[0] ))
# Increase time counter and ROS sleep()
TimeCounter += 1
PlannerCounter += 1
Counter += 1
rate.sleep()
#############################################################
day = '30_7_19'
num_test = 'References'
newpath = '/home/euge/GitHub/barc/results_simu_test/Test_Planner/'+day+'/'+num_test+'/'
if not os.path.exists(newpath):
os.makedirs(newpath)
np.savetxt(newpath+'/References.dat', References, fmt='%.5e')
#############################################################
# day = '29_7_19'
# num_test = 'Test_1'
# newpath = '/home/euge/GitHub/barc/results_simu_test/Test_Planner/'+day+'/'+num_test+'/'
# if not os.path.exists(newpath):
# os.makedirs(newpath)
# np.savetxt(newpath+'/ALL_LOCAL_DATA.dat', ALL_LOCAL_DATA, fmt='%.5e')
# # np.savetxt(newpath+'/PREDICTED_DATA.dat', PREDICTED_DATA, fmt='%.5e')
# # np.savetxt(newpath+'/GLOBAL_DATA.dat', GLOBAL_DATA, fmt='%.5e')
# # np.savetxt(newpath+'/Complete_Vel_Vect.dat', Complete_Vel_Vect, fmt='%.5e')
# # np.savetxt(newpath+'/References.dat', References, fmt='%.5e')
# # np.savetxt(newpath+'/TLAPTIME.dat', TLAPTIME, fmt='%.5e')
# np.savetxt(newpath+'/ELAPSD_TIME.dat', ELAPSD_TIME, fmt='%.5e')
plt.close()
# time50ms = np.linspace(0, (Counter-1)*dt, num=Counter-1, endpoint=True)
# time33ms = np.linspace(0, (Counter-1)*dt, num=np.around((Counter-1)*dt/0.033), endpoint=True)
# f = interp1d(time50ms, References[0:Counter-1,3], kind='cubic')
plt.figure(2)
plt.subplot(211)
plt.plot(References[0:Counter-1,3], 'o')
plt.legend(['Velocity'], loc='best')
plt.grid()
plt.subplot(212)
# plt.plot(time33ms, f(time33ms), 'o')
# plt.legend(['Velocity interpolated'], loc='best')
plt.plot(References[0:Counter-1,4], '-')
plt.legend(['Curvature'], loc='best')
plt.show()
plt.grid()
# pdb.set_trace()
quit() # final del while
def main():
# Initializa ROS node
rospy.init_node("LMPC")
input_commands = rospy.Publisher('ecu', ECU, queue_size=1)
pred_treajecto = rospy.Publisher('OL_predictions',
prediction,
queue_size=1)
sel_safe_set = rospy.Publisher('SS', SafeSetGlob, queue_size=1)
mode = rospy.get_param("/control/mode")
loop_rate = 30.0
dt = 1.0 / loop_rate
rate = rospy.Rate(loop_rate)
Steering_Delay = 1
### BUILDING THE GRID FOR TUNNING:
Data_for_RMSE = np.zeros((2000, 3))
VQ_ve = np.array([100.0, 150.0, 200.0, 250.0])
VQ_ye = np.array([50.0, 100.0, 150.0, 200.0])
VQ_thetae = np.array([
150.0, 200.0, 250.0, 300.0, 350.0, 400.0, 450.0, 500.0, 550.0, 600.0,
650.0, 700.0, 750.0, 800.0
])
grid_length = len(VQ_ve) * len(VQ_ye) * len(VQ_thetae)
Q_matrix_grid = np.zeros((grid_length, 3))
counter = 0
for i in range(0, len(VQ_ve)):
Q_ve = VQ_ve[i]
for j in range(0, len(VQ_ye)):
Q_ye = VQ_ye[j]
for k in range(0, len(VQ_thetae)):
Q_thetae = VQ_thetae[k]
Q_matrix_grid[counter, :] = [Q_ve, Q_ye, Q_thetae]
counter += 1
#print Q_matrix_grid
# Objects initializations
SS_glob_sel = SafeSetGlob()
OL_predictions = prediction()
cmd = ECU() # Command message
cmd.servo = 0.0
cmd.motor = 0.0
ClosedLoopData = ClosedLoopDataObj(dt, 6000, 0) # Closed-Loop Data
estimatorData = EstimatorData()
map = Map() # Map
PickController = "LPV_MPC"
first_it = 1
NumberOfLaps = grid_length
vt = 1.0
PathFollowingLaps = 0 #EA: With this at 0 skips the first PID lap
# Initialize variables for main loop
GlobalState = np.zeros(6)
LocalState = np.zeros(6)
HalfTrack = 0
LapNumber = 0
RunController = 1
PlotingCounter = 0
uApplied = np.array([0.0, 0.0])
oldU = np.array([0.0, 0.0])
RMSE_ve = np.zeros(grid_length)
RMSE_ye = np.zeros(grid_length)
RMSE_thetae = np.zeros(grid_length)
RMSE_matrix = np.zeros(grid_length)
# Loop running at loop rate
TimeCounter = 0
KeyInput = raw_input("Press enter to start the controller... \n")
oneStepPrediction = np.zeros(6)
### CONTROLLER INITIALIZATION:
### 33 ms - 10 Hp
Q = np.diag([100.0, 1.0, 1.0, 50.0, 0.0, 1000.0])
R = 1 * np.diag([1.0, 0.5]) # delta, a
dR = 30 * np.array([2.0, 1.5]) # Input rate cost u
N = 14
Controller = PathFollowingLPV_MPC(Q, R, dR, N, vt, dt, map, "OSQP",
Steering_Delay)
print "Q matrix: \n", Q, "\n"
while (not rospy.is_shutdown()) and RunController == 1:
# Read Measurements
GlobalState[:] = estimatorData.CurrentState
LocalState[:] = estimatorData.CurrentState #The current estimated state vector
# EA: s, ey, epsi
LocalState[4], LocalState[5], LocalState[
3], insideTrack = map.getLocalPosition(GlobalState[4],
GlobalState[5],
GlobalState[3])
if LocalState[0] < 0.01:
LocalState[0] = 0.01
# Check if the lap has finished
if LocalState[4] >= 3 * map.TrackLength / 4:
HalfTrack = 1
### END OF THE LAP:
if HalfTrack == 1 and (LocalState[4] <= map.TrackLength / 4):
HalfTrack = 0
LapNumber += 1
print "Lap completed starting lap:", LapNumber, ". Lap time: ", float(
TimeCounter) / loop_rate
if LapNumber > 1:
print "Recording RMSE... \n"
PointAndTangent = map.PointAndTangent
cur, vel = Curvature(LocalState[4], PointAndTangent)
RMSE_ve[LapNumber - 2] = np.sqrt(
((vel - Data_for_RMSE[0:TimeCounter, 0])**2).mean())
RMSE_ye[LapNumber - 2] = np.sqrt((Data_for_RMSE[0:TimeCounter,
1]**2).mean())
RMSE_thetae[LapNumber - 2] = np.sqrt(
(Data_for_RMSE[0:TimeCounter, 2]**2).mean())
Data_for_RMSE = np.zeros((2000, 3))
TimeCounter = 0
first_it = 1
Q = np.diag([
Q_matrix_grid[LapNumber - 1,
0], 1.0, 1.0, Q_matrix_grid[LapNumber - 1, 1],
0.0, Q_matrix_grid[LapNumber - 1, 2]
])
R = 1 * np.diag([1.0, 0.5]) # delta, a
dR = 30 * np.array([2.0, 1.5]) # Input rate cost u
N = 14
Controller = PathFollowingLPV_MPC(Q, R, dR, N, vt, dt, map, "OSQP",
Steering_Delay)
print "Q matrix: \n", Q, "\n"
if LapNumber >= NumberOfLaps:
RunController = 0
# If inside the track publish input
if (insideTrack == 1):
startTimer = datetime.datetime.now()
oldU = uApplied
uApplied = np.array([cmd.servo, cmd.motor])
#Controller.OldInput = uApplied
#print "Insert steering in the last space of the vector...", "\n"
Controller.OldSteering.append(
cmd.servo) # meto al final del vector
Controller.OldAccelera.append(cmd.motor)
#print "1o-Controller.OldSteering: ",Controller.OldSteering, "\n"
#print "Remove the first element of the vector...", "\n"
Controller.OldSteering.pop(0)
Controller.OldAccelera.pop(0)
#print "2o-Controller.OldSteering: ",Controller.OldSteering, "\n"
### Publish input ###
input_commands.publish(cmd)
#print "Publishing: ", cmd.servo, "\n"
oneStepPredictionError = LocalState - oneStepPrediction # Subtract the local measurement to the previously predicted one step
uAppliedDelay = [
Controller.OldSteering[-1 - Controller.steeringDelay],
Controller.OldAccelera[-1]
]
#print "uAppliedDelay: ",Controller.OldSteering[-1 - Controller.steeringDelay], "\n"
oneStepPrediction, oneStepPredictionTime = Controller.oneStepPrediction(
LocalState, uAppliedDelay, 1)
if first_it < 10: # EA: Starting mode:
xx = np.array([[
LocalState[0] + 0.05, LocalState[1], 0., 0.000001,
LocalState[4], 0.0001
],
[
LocalState[0] + 0.2, LocalState[1], 0.,
0.000001, LocalState[4] + 0.01, 0.0001
],
[
LocalState[0] + 0.4, LocalState[1], 0.,
0.000001, LocalState[4] + 0.02, 0.0001
],
[
LocalState[0] + 0.6, LocalState[1], 0.,
0.000001, LocalState[4] + 0.04, 0.0001
],
[
LocalState[0] + 0.7, LocalState[1], 0.,
0.000001, LocalState[4] + 0.07, 0.0001
],
[
LocalState[0] + 0.8, LocalState[1], 0.,
0.000001, LocalState[4] + 0.1, 0.0001
],
[
LocalState[0] + 0.8, LocalState[1], 0.,
0.000001, LocalState[4] + 0.14, 0.0001
],
[
LocalState[0] + 1.0, LocalState[1], 0.,
0.000001, LocalState[4] + 0.18, 0.0001
],
[
LocalState[0] + 1.05, LocalState[1], 0.,
0.000001, LocalState[4] + 0.23, 0.0001
],
[
LocalState[0] + 1.05, LocalState[1], 0.,
0.000001, LocalState[4] + 0.55, 0.0001
],
[
LocalState[0] + 1.05, LocalState[1], 0.,
0.000001, LocalState[4] + 0.66, 0.0001
],
[
LocalState[0] + 1.05, LocalState[1], 0.,
0.000001, LocalState[4] + 0.77, 0.0001
],
[
LocalState[0] + 1.05, LocalState[1], 0.,
0.000001, LocalState[4] + 0.89, 0.0001
],
[
LocalState[0] + 1.0, LocalState[1], 0.,
0.000001, LocalState[4] + 0.999, 0.0001
]])
uu = np.array([[0., 0.], [0., 0.3], [0., 0.5], [0., 0.7],
[0., 0.9], [0., 1.0], [0., 1.0], [0., 1.0],
[0., 1.0], [0., 0.8], [0., 0.8], [0., 0.8],
[0., 0.8], [0., 0.8]])
Controller.solve(LocalState, xx, uu)
first_it = first_it + 1
#print "Resolving MPC...", "\n "
#print "Steering predicted: ",Controller.uPred[:,0] , "\n "
#Controller.OldPredicted = np.hstack((Controller.OldSteering, Controller.uPred[Controller.steeringDelay:Controller.N-1,0]))
#Controller.OldPredicted = np.concatenate((np.matrix(Controller.OldPredicted).T, np.matrix(Controller.uPred[:,1]).T), axis=1)
Controller.OldPredicted = np.hstack(
(Controller.OldSteering[0:len(Controller.OldSteering) - 1],
Controller.uPred[Controller.steeringDelay:Controller.N,
0]))
Controller.OldPredicted = np.concatenate(
(np.matrix(Controller.OldPredicted).T,
np.matrix(Controller.uPred[:, 1]).T),
axis=1)
#print "OldPredicted: ",Controller.OldPredicted[:,0], "\n"
# We store the predictions since we are going to rebuild the controller object:
U_pred = Controller.uPred
last_Hp = len(uu)
else:
# # Recomputamos el objeto controller con nuevo Hp:
# PointAndTangent = map.PointAndTangent
# cur, vel = Curvature(LocalState[4], PointAndTangent)
# print "Curvature: ",cur, "\n"
# if cur > 0.1:
# Q = np.diag([100.0, 1.0, 1.0, 20.0, 0.0, 700.0])
# R = 0 * np.diag([1.0, 1.0]) # delta, a
# dR = 40 * np.array([2.0, 1.5]) # Input rate cost u
# N = 10
# Controller = PathFollowingLPV_MPC(Q, R, dR, N, vt, dt, map, "OSQP", Steering_Delay)
# print "Hp = ", N, "\n"
# if last_Hp == 4:
# last_input = U_pred[3,:]
# for i in range(last_Hp, N):
# U_pred = np.vstack((U_pred, last_input))
# LPV_States_Prediction = Controller.LPVPrediction(LocalState, U_pred)
# Controller.solve(LPV_States_Prediction[0,:], LPV_States_Prediction, U_pred)
# U_pred = Controller.uPred
# #print U_pred
# last_Hp = N
# else:
# LPV_States_Prediction = Controller.LPVPrediction(LocalState, U_pred)
# Controller.solve(LPV_States_Prediction[0,:], LPV_States_Prediction, U_pred)
# U_pred = Controller.uPred
# #print U_pred
# last_Hp = N
# else:
# Q = np.diag([100.0, 1.0, 1.0, 20.0, 0.0, 700.0])
# R = 0 * np.diag([1.0, 1.0]) # delta, a
# dR = 40 * np.array([2.0, 1.5]) # Input rate cost u
# N = 4
# Controller = PathFollowingLPV_MPC(Q, R, dR, N, vt, dt, map, "OSQP", Steering_Delay)
# print "Hp = ", N, "\n"
# LPV_States_Prediction = Controller.LPVPrediction(LocalState, U_pred)
# Controller.solve(LPV_States_Prediction[0,:], LPV_States_Prediction, U_pred)
# U_pred = Controller.uPred
# #print "U predicted:",U_pred, "\n"
# last_Hp = N
# Old version:
#LPV_States_Prediction = Controller.LPVPrediction(LocalState, Controller.uPred)
#Controller.solve(LPV_States_Prediction[0,:], LPV_States_Prediction, Controller.uPred)
#print "Resolving MPC...", "\n "
LPV_States_Prediction = Controller.LPVPrediction(
LocalState, Controller.OldPredicted)
Controller.solve(LPV_States_Prediction[0, :],
LPV_States_Prediction,
Controller.OldPredicted)
###################################################################################################
###################################################################################################
cmd.servo = Controller.uPred[0 + Controller.steeringDelay, 0]
cmd.motor = Controller.uPred[0, 1]
#print "Steering que SE PUBLICARA teniendo en cuenta el delay: ","\n", "------>", cmd.servo, "\n \n \n \n \n"
##################################################################
# GETTING DATA FOR IDENTIFICATION:
# cmd.servo = 0.5*np.cos(50*TimeCounter)
# if LocalState[0]>1.6:
# cmd.motor = 0.0
# else:
# cmd.motor = 1.0
##################################################################
#print Controller.uPred[steps_Delay,0], Controller.uPred[0,0]
# if (Controller.solverTime.total_seconds() + Controller.linearizationTime.total_seconds() + oneStepPredictionTime.total_seconds() > dt):
# print "NOT REAL-TIME FEASIBLE!!"
# print "Solver time: ", Controller.solverTime.total_seconds(), " Linearization Time: ", Controller.linearizationTime.total_seconds() + oneStepPredictionTime.total_seconds()
endTimer = datetime.datetime.now()
deltaTimer = endTimer - startTimer
#print "Tot Solver Time: ", deltaTimer.total_seconds()
else: # If car out of the track
cmd.servo = 0
cmd.motor = 0
input_commands.publish(cmd)
print " Current Input: ", cmd.servo, cmd.motor
print " X, Y State: ", GlobalState
print " Current State: ", LocalState
# Record Prediction
OL_predictions.s = Controller.xPred[:, 4]
OL_predictions.ey = Controller.xPred[:, 5]
OL_predictions.epsi = Controller.xPred[:, 3]
pred_treajecto.publish(OL_predictions)
ClosedLoopData.addMeasurement(GlobalState, LocalState,
uApplied, PlotingCounter,
deltaTimer.total_seconds())
Data_for_RMSE[TimeCounter, :] = [
LocalState[0], LocalState[5], LocalState[3]
]
# Increase time counter and ROS sleep()
TimeCounter = TimeCounter + 1
PlotingCounter += 1
rate.sleep()
# Save Data
file_data = open(
sys.path[0] + '/data/' + mode + '/ClosedLoopData' + "LPV_MPC" + '.obj',
'wb')
pickle.dump(ClosedLoopData, file_data)
pickle.dump(Controller, file_data)
#pickle.dump(OpenLoopData, file_data)
print " \n \n \n Root Mean Squared Vel Error Normalised: \n", np.divide(
RMSE_ve[0:LapNumber - 1], np.amax(RMSE_ve[0:LapNumber - 1])), "\n"
print "Root Mean Squared Lateral Error Normalised: \n", np.divide(
RMSE_ye[0:LapNumber - 1], np.amax(RMSE_ye[0:LapNumber - 1])), "\n"
print "Root Mean Squared Theta Error Normalised: \n", np.divide(
RMSE_thetae[0:LapNumber - 1],
np.amax(RMSE_thetae[0:LapNumber - 1])), "\n"
print len(RMSE_thetae[0:LapNumber - 1])
for k in range(0, len(RMSE_thetae[0:LapNumber - 1]) - 1):
RMSE_matrix[k] = RMSE_ve[k] + RMSE_ye[k] + RMSE_thetae[k]
print "Best tunning: ", Q_matrix_grid[np.argmin(RMSE_matrix)], "\n \n \n"
#plotTrajectory(map, ClosedLoopData)
file_data.close()
def main():
rospy.init_node("realTimePlotting")
StateView = False
data = EstimationAndMesuredData()
map = Map()
loop_rate = 100
rate = rospy.Rate(loop_rate)
if StateView == True:
fig, linevx, linevy, linewz, lineepsi, lineey, line_tr, line_pred = _initializeFigure(map)
else:
fig, axtr, line_tr, line_pred, line_SS, line_cl, line_gps_cl, rec = _initializeFigure_xy(map)
ClosedLoopTraj_gps_x = []
ClosedLoopTraj_gps_y = []
ClosedLoopTraj_x = []
ClosedLoopTraj_y = []
maxVx = 0.0
flagHalfLap = False
while not rospy.is_shutdown():
estimatedStates = data.readEstimatedData()
s, ey, epsi, insideMap = map.getLocalPosition(estimatedStates[4], estimatedStates[5], estimatedStates[3])
if s > map.TrackLength / 2:
flagHalfLap = True
if (s < map.TrackLength / 4) and (flagHalfLap == True): # New lap
ClosedLoopTraj_gps_x = []
ClosedLoopTraj_gps_y = []
ClosedLoopTraj_x = []
ClosedLoopTraj_y = []
flagHalfLap = False
x = estimatedStates[4]
y = estimatedStates[5]
ClosedLoopTraj_gps_x.append(data.MeasuredData[0])
ClosedLoopTraj_gps_y.append(data.MeasuredData[1])
ClosedLoopTraj_x.append(x)
ClosedLoopTraj_y.append(y)
psi = estimatedStates[3]
l = 0.4; w = 0.2
car_x = [ x + l * np.cos(psi) - w * np.sin(psi), x + l*np.cos(psi) + w * np.sin(psi),
x - l * np.cos(psi) + w * np.sin(psi), x - l * np.cos(psi) - w * np.sin(psi)]
car_y = [ y + l * np.sin(psi) + w * np.cos(psi), y + l * np.sin(psi) - w * np.cos(psi),
y - l * np.sin(psi) - w * np.cos(psi), y - l * np.sin(psi) + w * np.cos(psi)]
if data.s != []:
xPredicted = np.zeros((len(data.s), 1))
yPredicted = np.zeros((len(data.s), 1))
for j in range(0, len(data.s)):
sPred = data.s[j]
eyPred = data.ey[j]
epsiPred = data.epsi[j]
xPredicted[j], yPredicted[j] = map.getGlobalPosition(sPred, eyPred)
# print "sPred is: ", sPred
# print "eyPred is: ", eyPred
line_pred.set_data(xPredicted, yPredicted)
if data.SSx != []:
line_SS.set_data(data.SSx, data.SSy)
# line_cl.set_data(ClosedLoopTraj_x, ClosedLoopTraj_y)
# line_gps_cl.set_data(ClosedLoopTraj_gps_x, ClosedLoopTraj_gps_y)
if StateView == True:
linevx.set_data(0.0, estimatedStates[0])
linevy.set_data(0.0, estimatedStates[1])
linewz.set_data(0.0, estimatedStates[2])
lineepsi.set_data(0.0, epsi)
lineey.set_data(0.0, ey)
else:
line_cl.set_data(ClosedLoopTraj_x, ClosedLoopTraj_y)
line_gps_cl.set_data(ClosedLoopTraj_gps_x, ClosedLoopTraj_gps_y)
line_tr.set_data(estimatedStates[4], estimatedStates[5])
if StateView == False:
rec.set_xy(np.array([car_x, car_y]).T)
maxVx = np.maximum(maxVx, estimatedStates[0])
StringValue = "vx: "+str(estimatedStates[0])+" max vx: "+str(maxVx)
if StateView == False:
axtr.set_title(StringValue)
if insideMap == 1:
fig.canvas.draw()
rate.sleep()
def main():
map = Map("oval")
# FIGURE 1 plotting of estimator output data
homedir = os.path.expanduser("~")
pathSave = os.path.join(homedir, "barc_data/estimator_output.npz")
npz_output = np.load(pathSave)
x_est_his = npz_output["x_est_his"]
y_est_his = npz_output["y_est_his"]
vx_est_his = npz_output["vx_est_his"]
vy_est_his = npz_output["vy_est_his"]
ax_est_his = npz_output["ax_est_his"]
ay_est_his = npz_output["ay_est_his"]
psiDot_est_his = npz_output["psiDot_est_his"]
yaw_est_his = npz_output["yaw_est_his"]
gps_time_his = npz_output["gps_time"]
imu_time_his = npz_output["imu_time"]
enc_time_his = npz_output["enc_time"]
inp_x_his = npz_output["inp_x_his"]
inp_y_his = npz_output["inp_y_his"]
inp_v_meas_his = npz_output["inp_v_meas_his"]
inp_ax_his = npz_output["inp_ax_his"]
inp_ay_his = npz_output["inp_ay_his"]
inp_psiDot_his = npz_output["inp_psiDot_his"]
inp_a_his = npz_output["inp_a_his"]
inp_df_his = npz_output["inp_df_his"]
pathSave = os.path.join(homedir, "barc_data/estimator_imu.npz")
npz_imu = np.load(pathSave)
psiDot_his = npz_imu["psiDot_his"]
roll_his = npz_imu["roll_his"]
pitch_his = npz_imu["pitch_his"]
yaw_his = npz_imu["yaw_his"]
ax_his = npz_imu["ax_his"]
ay_his = npz_imu["ay_his"]
imu_time = npz_imu["imu_time"]
pathSave = os.path.join(homedir, "barc_data/estimator_gps.npz")
npz_gps = np.load(pathSave)
x_his = npz_gps["x_his"]
y_his = npz_gps["y_his"]
gps_time = npz_gps["gps_time"]
pathSave = os.path.join(homedir, "barc_data/estimator_enc.npz")
npz_enc = np.load(pathSave)
v_fl_his = npz_enc["v_fl_his"]
v_fr_his = npz_enc["v_fr_his"]
v_rl_his = npz_enc["v_rl_his"]
v_rr_his = npz_enc["v_rr_his"]
enc_time = npz_enc["enc_time"]
pathSave = os.path.join(homedir, "barc_data/estimator_ecu.npz")
npz_ecu = np.load(pathSave)
a_his = npz_ecu["a_his"]
df_his = npz_ecu["df_his"]
ecu_time = npz_ecu["ecu_time"]
# Plot Trajectory
fig = plotTrack(map)
plt.axis("equal")
plt.plot(x_est_his, y_est_his, color="green")
plt.plot(x_his, y_his, color="blue")
plt.legend()
plt.show()
# Acc
plt.subplot(311)
plt.plot(range(0, len(inp_ax_his)), inp_ax_his, '-or')
plt.plot(range(0, len(ax_est_his)), ax_est_his, '-sb')
plt.subplot(312)
plt.plot(range(0, len(inp_ay_his)), inp_ay_his, '-or')
plt.plot(range(0, len(ay_est_his)), ay_est_his, '-sb')
plt.subplot(313)
plt.plot(range(0, len(inp_psiDot_his)), inp_psiDot_his, '-or')
plt.plot(range(0, len(psiDot_est_his)), psiDot_est_his, '-sb')
# Check Redundancy
plt.figure(10)
plt.plot(range(0,
len(gps_time_his) - 1),
gps_time_his[1:] - gps_time_his[:-1], '--sk')
plt.ylabel('delta gps time')
plt.figure(11)
plt.plot(range(0,
len(imu_time_his) - 1),
imu_time_his[1:] - imu_time_his[:-1], '--sk')
plt.ylabel('delta imu time')
plt.figure(12)
plt.plot(range(0,
len(enc_time_his) - 1),
enc_time_his[1:] - enc_time_his[:-1], '--sk')
plt.ylabel('delta enc time')
plt.show()
# Animation
fig, axtr, line_est, line_gps, rec = _initializeFigure_xy(map)
ClosedLoopTraj_gps_x = []
ClosedLoopTraj_gps_y = []
ClosedLoopTraj_est_x = []
ClosedLoopTraj_est_y = []
for i in range(len(x_est_his) - 1):
ClosedLoopTraj_gps_x.append(inp_x_his[i])
ClosedLoopTraj_gps_y.append(inp_y_his[i])
line_gps.set_data(ClosedLoopTraj_gps_x, ClosedLoopTraj_gps_y)
ClosedLoopTraj_est_x.append(x_est_his[i + 1])
ClosedLoopTraj_est_y.append(y_est_his[i + 1])
line_est.set_data(ClosedLoopTraj_est_x, ClosedLoopTraj_est_y)
psi = yaw_est_his[i + 1]
x = x_est_his[i + 1]
y = y_est_his[i + 1]
l = 0.4
w = 0.2
car_x, car_y = getCar_x_y(x, y, psi, l, w)
rec.set_xy(np.array([car_x, car_y]).T)
fig.canvas.draw()