Skip to content

Commit

Permalink
adding the LiDAR and the new track
Browse files Browse the repository at this point in the history
  • Loading branch information
@huckl3b3rry87
huckl3b3rry87 committed Apr 4, 2017
1 parent a32fc54 commit b94d693
Show file tree
Hide file tree
Showing 2 changed files with 81 additions and 30 deletions.
7 changes: 4 additions & 3 deletions src/NLOptControl_plots.jl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@ function allPlots(n::NLOpt,r::Result,s::Settings,idx::Int64)
end

"""
stp=statePlot(n,r,s,r.eval_num,7);
stp=statePlot(n,r,s,idx,st);
stp=statePlot(n,r,s,idx,st;(:legend=>"test1"));
stp=statePlot(n,r,s,idx,st,stp;(:append=>true));
Expand Down Expand Up @@ -58,7 +59,7 @@ function statePlot(n::NLOpt,r::Result,s::Settings,idx::Int64,st::Int64,args...;k
if r.dfs[idx]!=nothing
t_vec=linspace(0.0,max(5,round(r.dfs[end][:t][end]/5)*5),s.L);
else
t_vec=linspace(r.dfs_plant[1][:t][1],max(5,round(r.dfs_plant[end][:t][end]/5)*5),s.L);
t_vec=linspace(0.0,max(5,round(r.dfs_plant[end][:t][end]/5)*5),s.L);
end

if lims
Expand All @@ -77,7 +78,7 @@ function statePlot(n::NLOpt,r::Result,s::Settings,idx::Int64,st::Int64,args...;k
end
end

# plot the values
# plot the values TODO if there are no lims then you cannot really see the signal
if r.dfs[idx]!=nothing
plot!(r.dfs[idx][:t],r.dfs[idx][n.state.name[st]],w=s.lw1,label=string(legend_string,"mpc"));
end
Expand Down Expand Up @@ -195,7 +196,7 @@ function controlPlot(n::NLOpt,r::Result,s::Settings,idx::Int64,ctr::Int64,args..
if r.dfs[idx]!=nothing
t_vec=linspace(0.0,max(5,round(r.dfs[end][:t][end]/5)*5),s.L);
else
t_vec=linspace(r.dfs_plant[1][:t][1],max(5,round(r.dfs_plant[end][:t][end]/5)*5),s.L);
t_vec=linspace(0.0,max(5,round(r.dfs_plant[end][:t][end]/5)*5),s.L);
end

# plot the limits
Expand Down
104 changes: 77 additions & 27 deletions src/VehicleModels_plots.jl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,11 +17,12 @@ export
pSimPath

"""
pp=obstaclePlot(n,r,s,c,1);
pp=obstaclePlot(n,r,s,c,idx);
pp=obstaclePlot(n,r,s,c,idx,pp;(:append=>true));
--------------------------------------------------------------------------------------\n
Author: Huckleberry Febbo, Graduate Student, University of Michigan
Date Create: 3/11/2017, Last Modified: 3/29/2017 \n
Date Create: 3/11/2017, Last Modified: 4/3/2017 \n
--------------------------------------------------------------------------------------\n
"""
function obstaclePlot(n,r,s,c,idx,args...;kwargs...)
Expand Down Expand Up @@ -52,8 +53,14 @@ function obstaclePlot(n,r,s,c,idx,args...;kwargs...)
x += c.o.X0[i] + c.o.s_x[i]*r.dfs_plant[idx][:t][end];
y = c.o.B[i]/c.o.A[i]*y + c.o.Y0[i] + c.o.s_y[i]*r.dfs_plant[idx][:t][end];
else
x += c.o.X0[i] + c.o.s_x[i]*r.dfs[idx][:t][end];
y = c.o.B[i]/c.o.A[i]*y + c.o.Y0[i] + c.o.s_y[i]*r.dfs[idx][:t][end];
if r.dfs[idx]!=nothing
tc=r.dfs[idx][:t][end];
else
tc=0;
warn("\n Obstacles set to inital condition for current frame!! \n")
end
x += c.o.X0[i] + c.o.s_x[i]*tc;
y = c.o.B[i]/c.o.A[i]*y + c.o.Y0[i] + c.o.s_y[i]*tc;
end
pts = collect(zip(x, y))
if i==1
Expand Down Expand Up @@ -132,9 +139,9 @@ function vtPlot(n::NLOpt,r::Result,s::Settings,pa::VehicleModels.Vpara,c,idx::In
@unpack_Vpara pa

if !s.MPC && r.dfs[idx]!=nothing
t_vec=linspace(r.dfs[1][:t][1],round(r.dfs[end][:t][end]/10)*10,s.L);
t_vec=linspace(0,round(r.dfs[end][:t][end]/10)*10,s.L);
else
t_vec=linspace(r.dfs_plant[1][:t][1],max(5,round(r.dfs_plant[end][:t][end]/5)*5),s.L);
t_vec=linspace(0,max(5,round(r.dfs_plant[end][:t][end]/5)*5),s.L);
end

vt=plot(t_vec,Fz_min*ones(s.L,1),line=(s.lw2),label="min")
Expand Down Expand Up @@ -204,9 +211,9 @@ function axLimsPlot(n::NLOpt,r::Result,s::Settings,pa::VehicleModels.Vpara,idx::
@unpack_Vpara pa

if !s.MPC && r.dfs[idx]!=nothing
t_vec=linspace(r.dfs[1][:t][1],max(5,round(r.dfs[end][:t][end]/5)*5),s.L);
t_vec=linspace(0,max(5,round(r.dfs[end][:t][end]/5)*5),s.L);
else
t_vec=linspace(r.dfs_plant[1][:t][1],max(5,round(r.dfs_plant[end][:t][end]/5)*5),s.L);
t_vec=linspace(0,max(5,round(r.dfs_plant[end][:t][end]/5)*5),s.L);
end

if r.dfs[idx]!=nothing
Expand Down Expand Up @@ -247,7 +254,7 @@ function mainSim(n,r,s,c,pa,idx)
pp=statePlot(n,r,s,idx,1,2;(:lims=>false));
pp=obstaclePlot(n,r,s,c,idx,pp;(:append=>true)); # add obstacles
pp=vehiclePlot(n,r,s,c,idx,pp;(:append=>true)); # add the vehicle
tp=tPlot(n,r,s,idx)
if s.MPC; tp=tPlot(n,r,s,idx); else; tp=plot(0,leg=:false); end
vt=vtPlot(n,r,s,pa,c,idx)
l = @layout [a{0.3w} [grid(2,2)
b{0.2h}]]
Expand All @@ -257,6 +264,7 @@ end


"""
mainS=mainSimPath(n,r,s,c,pa,r.eval_num)
mainS=mainSimPath(n,r,s,c,pa,idx)
--------------------------------------------------------------------------------------\n
Author: Huckleberry Febbo, Graduate Student, University of Michigan
Expand All @@ -273,7 +281,7 @@ function mainSimPath(n,r,s,c,pa,idx)
rp=statePlot(n,r,s,idx,4)
vt=vtPlot(n,r,s,pa,c,idx)
pp=pSimPath(n,r,s,c,idx)
tp=tPlot(n,r,s,idx)
if s.MPC; tp=tPlot(n,r,s,idx); else; tp=plot(0,leg=:false); end
l = @layout [a{0.3w} [grid(2,2)
b{0.2h}]]
mainS=plot(pp,sap,vt,rp,vp,tp,layout=l,size=(1800,1200));
Expand All @@ -282,31 +290,49 @@ function mainSimPath(n,r,s,c,pa,idx)
end

"""
pp=pSimPath(n,r,s,c,idx)
pp=trackPlot(r,s,c,idx);
pp=trackPlot(r,s,c,idx,pp;(:append=>true));
--------------------------------------------------------------------------------------\n
Author: Huckleberry Febbo, Graduate Student, University of Michigan
Date Create: 3/28/2017, Last Modified: 3/28/2017 \n
Date Create: 3/28/2017, Last Modified: 4/3/2017 \n
--------------------------------------------------------------------------------------\n
"""
function pSimPath(n,r,s,c,idx)
pp=trackPlot(r,s,c,idx)
pp=statePlot(n,r,s,idx,1,2,pp;(:lims=>false),(:append=>true));
pp=obstaclePlot(n,r,s,c,idx,pp;(:append=>true)); # obstacles
pp=vehiclePlot(n,r,s,c,idx,pp;(:append=>true)); # vehicle
if !s.simulate savefig(string(r.results_dir,"pp.",s.format)) end
function trackPlot(r,s,c,idx,args...;kwargs...)
kw = Dict(kwargs);

# check to see if user would like to add to an existing plot
if !haskey(kw,:append); kw_ = Dict(:append => false); append = get(kw_,:append,0);
else; append = get(kw,:append,0);
end
if !append; pp=plot(0,leg=:false); else pp=args[1]; end

if c.t.func==:poly
f(y)=c.t.a[1] + c.t.a[2]*y + c.t.a[3]*y^2 + c.t.a[4]*y^3 + c.t.a[5]*y^4;
Y=c.t.Y;
X=f.(Y);
elseif c.t.func==:fourier
ff(x)=c.t.a[1]*sin(c.t.b[1]*x+c.t.c[1]) + c.t.a[2]*sin(c.t.b[2]*x+c.t.c[2]) + c.t.a[3]*sin(c.t.b[3]*x+c.t.c[3]) + c.t.a[4]*sin(c.t.b[4]*x+c.t.c[4]) + c.t.a[5]*sin(c.t.b[5]*x+c.t.c[5]) + c.t.a[6]*sin(c.t.b[6]*x+c.t.c[6]) + c.t.a[7]*sin(c.t.b[7]*x+c.t.c[7]) + c.t.a[8]*sin(c.t.b[8]*x+c.t.c[8])+c.t.y0;
#ff(x)=c.t.a[1]*sin(-c.t.b[1]*x+c.t.c[1]) + c.t.a[2]*sin(-c.t.b[2]*x+c.t.c[2]) + c.t.a[3]*sin(-c.t.b[3]*x+c.t.c[3]) + c.t.a[4]*sin(-c.t.b[4]*x+c.t.c[4]) + c.t.a[5]*sin(-c.t.b[5]*x+c.t.c[5]) + c.t.a[6]*sin(-c.t.b[6]*x+c.t.c[6]) + c.t.a[7]*sin(-c.t.b[7]*x+c.t.c[7]) + c.t.a[8]*sin(-c.t.b[8]*x+c.t.c[8])+c.t.y0;
X=c.t.X;
Y=ff.(X);
plot(X,Y)
end

plot!(X,Y,label="Road",line=(s.lw1*2,:solid,:black))
return pp
end

"""
pp=trackPlot(r,s,c,idx);
pp=trackPlot(r,s,c,idx,pp;(:append=>true));
pp=lidarPlot(r,s,c,idx);
pp=lidarPlot(r,s,c,idx,pp;(:append=>true));
--------------------------------------------------------------------------------------\n
Author: Huckleberry Febbo, Graduate Student, University of Michigan
Date Create: 3/28/2017, Last Modified: 3/28/2017 \n
Date Create: 4/3/2017, Last Modified: 4/3/2017 \n
--------------------------------------------------------------------------------------\n
"""
function trackPlot(r,s,c,idx,args...;kwargs...)
function lidarPlot(r,s,c,idx,args...;kwargs...)
kw = Dict(kwargs);

# check to see if user would like to add to an existing plot
Expand All @@ -315,16 +341,40 @@ function trackPlot(r,s,c,idx,args...;kwargs...)
end
if !append; pp=plot(0,leg=:false); else pp=args[1]; end

f(y)=c.t.a0 + c.t.a1*y + c.t.a2*y^2 + c.t.a3*y^3 + c.t.a4*y^4;

if s.MPC && r.eval_num > 2
Y= r.dfs_plant[1][:y][1]:0.1:r.dfs[r.eval_num][:y][end];
# plot the LiDAR
if s.MPC
X_v = r.dfs_plant[idx][:x][1] # using the begining of the simulated data from the vehicle model
Y_v = r.dfs_plant[idx][:y][1]
PSI_v = r.dfs_plant[idx][:psi][1]-pi/2
else
Y=r.X[:,2];
X_v = r.dfs[idx][:x][1]
Y_v = r.dfs[idx][:y][1]
PSI_v = r.dfs[idx][:psi][1]-pi/2
end
X=f.(Y);

plot!(X,Y,label="Road",line=(s.lw1*2,:solid,:black))
pts = Plots.partialcircle(PSI_v-pi,PSI_v+pi,50,c.m.Lr);
x, y = Plots.unzip(pts);
x += X_v; y += Y_v;
pts = collect(zip(x, y));
plot!(pts, line=(s.lw1,0.2,:solid,:yellow),fill = (0, 0.2, :yellow),leg=true,label="LiDAR Range")

return pp
end
"""
pp=pSimPath(n,r,s,c,idx)
--------------------------------------------------------------------------------------\n
Author: Huckleberry Febbo, Graduate Student, University of Michigan
Date Create: 3/28/2017, Last Modified: 4/3/2017 \n
--------------------------------------------------------------------------------------\n
"""
function pSimPath(n,r,s,c,idx)
pp=trackPlot(r,s,c,idx);
pp=lidarPlot(r,s,c,idx,pp;(:append=>true));
pp=statePlot(n,r,s,idx,1,2,pp;(:lims=>false),(:append=>true));
pp=obstaclePlot(n,r,s,c,idx,pp;(:append=>true)); # obstacles
pp=vehiclePlot(n,r,s,c,idx,pp;(:append=>true)); # vehicle
if !s.simulate savefig(string(r.results_dir,"pp.",s.format)) end
return pp
end

Expand Down

0 comments on commit b94d693

Please sign in to comment.