Initial commit R2024b

Author: smiller01985

.gitattributes

@@ -0,0 +1,28 @@
+* text=auto
+
+*.fig binary
+*.mat binary
+*.mdl binary diff merge=mlAutoMerge
+*.mdlp binary
+*.mex* binary
+*.mlapp binary
+*.mldatx binary
+*.mlproj binary
+*.mlx binary
+*.p binary
+*.sfx binary
+*.sldd binary
+*.slreqx binary merge=mlAutoMerge
+*.slmx binary merge=mlAutoMerge
+*.sltx binary
+*.slxc binary
+*.slx binary merge=mlAutoMerge
+*.slxp binary
+
+## Other common binary file types
+*.docx binary
+*.exe binary
+*.jpg binary
+*.pdf binary
+*.png binary
+*.xlsx binary

.gitignore

@@ -0,0 +1,32 @@
+# Autosave files
+*.asv
+*.m~
+*.autosave
+*.slx.r*
+*.mdl.r*
+
+# Derived content-obscured files
+
+# Compiled MEX files
+*.mex*
+
+# Packaged app and toolbox files
+*.mlappinstall
+*.mltbx
+
+# Deployable archives
+*.ctf
+
+# Generated helpsearch folders
+helpsearch*/
+
+# Code generation folders
+slprj/
+sccprj/
+codegen/
+
+# Cache files
+*.slxc
+
+# Cloud based storage dotfile
+.MATLABDriveTag

LICENSE.md

@@ -0,0 +1,11 @@
+Copyright (c) 2021-2022, The MathWorks, Inc.
+All rights reserved.
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
+3. In all cases, the software is, and all modifications and derivatives of the software shall be, licensed to you solely for use in conjunction with MathWorks products and service offerings. 
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+
+
+

Libraries/Event/Maneuver/Maneuver_Get_Pose.slx

@@ -0,0 +1,50 @@
+function scene_data = sm_car_scenedata_double_lane_change_iso3888
+%% Floor and Grid parameters
+% Copyright 2018-2024 The MathWorks, Inc.
+
+scene_data.Name = 'Double_Lane_Change_ISO3888';
+scene_data.Dashes.pitch = 7.5;      % m
+scene_data.Dashes.num = 58;         % m
+scene_data.Dashes.l = 3;            % m
+scene_data.Dashes.w = 0.15;         % m
+scene_data.Dashes.h = 0.01;        % m
+scene_data.Dashes.base_h = 0.025;   % m
+scene_data.Dashes.clr = [1 1 1];    % [R G B]
+scene_data.Dashes.opc = 1;          % (0-1)
+
+% Road length matches dash length
+scene_data.Road.l = scene_data.Dashes.pitch*scene_data.Dashes.num; % m
+scene_data.Road.w = 8;              % m
+scene_data.Road.h = 0.1;            % m
+scene_data.Road.clr = [1 1 1]*0.5;  % [R G B]
+scene_data.Road.opc = 1;            % (0-1)
+scene_data.Road.x     = 218.4;      % m
+scene_data.Road.y     = -1.2;       % m
+scene_data.Road.z     = 0;          % m
+scene_data.Road.roll  = 0*pi/180;   % rad
+scene_data.Road.pitch = 0*pi/180;   % rad
+scene_data.Road.yaw   = 0*pi/180;   % rad
+
+scene_data.Cones.lane_a_offset       = 175; % m
+scene_data.Cones.lane_width          = 3.3;  % m
+scene_data.Cones.lane_lateral_offset = 3.5;%4.1;  % m
+scene_data.Cones.lane_a_length       = 15; % m
+scene_data.Cones.gap_a_to_b          = 30; % m
+scene_data.Cones.lane_b_length       = 25; % m
+scene_data.Cones.gap_b_to_c          = 25; % m
+scene_data.Cones.lane_c_length       = 30; % m
+scene_data.Cones.height              = 0.6;  % m
+scene_data.Cones.base_thickness      = 0.04; % m
+scene_data.Cones.base_width          = 0.35;  % m
+scene_data.Cones.bottom_diameter     = 0.24; % m
+scene_data.Cones.top_diameter        = 0.06; % m
+scene_data.Cones.strip_height        = 0.3;  % m
+scene_data.Cones.clr                 = [248 78 25]/255;  % [RGB]
+scene_data.Cones.strip_clr           = [0.75 0.75 0.75]; % [RGB]
+
+
+
+
+
+
+

Libraries/Event/Scene/Double_Lane_Change_ISO3888/Double_Lane_Change_ISO3888_trajectory_default.mat

@@ -0,0 +1,132 @@
+function sm_car_trajectory_double_lane_change_iso3888
+% Function to construct double-lane change maneuver according to ISO-3888
+% Copyright 2018-2024 The MathWorks, Inc.
+
+cd(fileparts(which(mfilename)))
+% Parameters for trajectory
+maneuver_lateral_offset    = -3.35; % m
+cone_set_lat_separation    = 4.1;%3.5;   % m
+
+% Longitudinal gate locations (m)
+gate0       = 0;     % Start 
+
+gate1       = 175;   % Entry 1st set of cones
+gate2       = gate1+15;   % Exit  1st set of cones
+
+gate3       = gate2+30;   % Entry 2nd set of cones
+gate4       = gate3+25;   % Exit  2nd set of cones
+
+gate5       = gate4+25;   % Entry 3rd set of cones
+gate6       = gate5+30;   % Exit 3rd set of cones
+
+gate7       = 700;   % End
+
+v_lc        = 22;     % Speed during lane change (m/s)
+x_testv     = 100;    % Distance when target speed is reached (m)
+
+% Assemble waypoints, foundation for interpolation
+x_waypoints =  [gate0 x_testv   gate1:1:gate2              gate3:1:gate4              gate5:1:gate6             ];
+y_waypoints =  [0     0         zeros(1,gate2-gate1+1)     ones(1,gate4-gate3+1)      zeros(1,gate6-gate5+1)    ]*cone_set_lat_separation+maneuver_lateral_offset;
+vx_waypoints = [1     v_lc      v_lc*ones(1,gate2-gate1+1) v_lc*ones(1,gate4-gate3+1) v_lc*ones(1,gate6-gate5+1)];
+
+% Construct query points for interpolation
+x_new  = [...
+    gate0:(gate1-gate0)/30: (gate1-(gate1-gate0)/30) ...
+    gate1:(gate2-gate1)/10: (gate2-(gate2-gate1)/10) ...
+    gate2:(gate3-gate2)/20: (gate3-(gate3-gate2)/20) ...
+    gate3:(gate4-gate3)/10: (gate4-(gate4-gate3)/10) ...
+    gate4:(gate5-gate4)/20: (gate5-(gate5-gate4)/20) ...
+    gate5:(gate6-gate5)/15: (gate6-(gate6-gate5)/15) ...
+    gate6:(gate7-gate6)/10: (gate7)];
+
+% Interpolate to get lateral distance and speed
+y_new  = interp1(x_waypoints, y_waypoints,  x_new, 'pchip');
+vx_new = interp1(x_waypoints, vx_waypoints, x_new, 'pchip');
+
+% Plot waypoints and interpolated trajectory
+fig_handle_name = 'h1_sm_car_double_lane_change';
+
+handle_var = evalin('base',['who(''' fig_handle_name ''')']);
+if(isempty(handle_var))
+    evalin('base',[fig_handle_name ' = figure(''Name'', ''' fig_handle_name ''');']);
+elseif ~isgraphics(evalin('base',handle_var{:}))
+    evalin('base',[fig_handle_name ' = figure(''Name'', ''' fig_handle_name ''');']);
+end
+figure(evalin('base',fig_handle_name))
+clf(evalin('base',fig_handle_name))
+
+plot(x_waypoints,y_waypoints,'bo');
+hold on
+plot(x_new,y_new,'r-x')
+hold off
+xlabel('Lateral Distance (m)')
+ylabel('Longitudinal Distance (m)');
+title('Double-Lane Change Trajectory, ISO 3888');
+legend({'Waypoints','Interpolated'},'Location','Best');
+axis equal
+
+% Calculate distance traveled
+xTrajectory_new = [0 cumsum(sqrt((diff(x_new)).^2+diff(y_new).^2))];
+
+fig_handle_name = 'h2_sm_car_double_lane_change';
+
+handle_var = evalin('base',['who(''' fig_handle_name ''')']);
+if(isempty(handle_var))
+    evalin('base',[fig_handle_name ' = figure(''Name'', ''' fig_handle_name ''');']);
+elseif ~isgraphics(evalin('base',handle_var{:}))
+    evalin('base',[fig_handle_name ' = figure(''Name'', ''' fig_handle_name ''');']);
+end
+figure(evalin('base',fig_handle_name))
+clf(evalin('base',fig_handle_name))
+subplot(211)
+plot(xTrajectory_new,vx_new)
+xlabel('Distance Traveled (m)');
+ylabel('Target Speed (m/s)');
+title('Target Speed Along Trajectory');
+
+% Calculate target yaw angle (rad)
+yaw_interval = 10; % 10
+aYaw_new = atan2(...
+    y_new(yaw_interval+1:end)-y_new(1:end-yaw_interval),...
+    x_new(yaw_interval+1:end)-x_new(1:end-yaw_interval));
+
+%aYaw_new = atan2(...
+%    y_new(yaw_interval+1:end)-y_new(1:end-yaw_interval),...
+%    x_new(yaw_interval+1:end)-x_new(1:end-yaw_interval));
+
+
+aYaw_new = [repmat(aYaw_new(1),1,yaw_interval) aYaw_new];
+
+subplot(212)
+plot(xTrajectory_new,aYaw_new,'-o')
+xlabel('Distance Traveled (m)');
+ylabel('Target Yaw Angle (rad)');
+title('Target Yaw Angle Along Trajectory');
+
+% Assign parameters for trajectory definition
+x.Value = x_new;
+x.Units = 'm';
+x.Comments = '';
+
+y.Value = y_new;
+y.Units = 'm';
+y.Comments = '';
+
+z.Value = zeros(size(x_new));
+z.Units = 'm';
+z.Comments = '';
+
+xTrajectory.Value = xTrajectory_new;
+xTrajectory.Units = 'm';
+xTrajectory.Comments = 'Distance traveled';
+
+vx.Value = vx_new;
+vx.Units = 'm/s';
+vx.Comments = 'Vehicle speed along direction of travel';
+
+aYaw.Value = aYaw_new;
+aYaw.Units = 'rad';
+aYaw.Comments = 'Yaw Angle, non-wrapping';
+
+save Double_Lane_Change_ISO3888_trajectory_default x y z vx aYaw xTrajectory 
+

Libraries/Event/Scene/Double_Lane_Change_ISO3888/sm_car_scene_double_lane_change_iso3888.slx

@@ -0,0 +1,133 @@
+function sm_car_crg_to_stl(crg_file,stl_file,varargin)
+%sm_car_crg_to_stl  Create STL file from CRG file
+%   sm_car_crg_to_stl(crg_file,stl_file)
+%   Use CRG functions to generate MATLAB surface, 
+%   and convert that surface to an STL.
+%
+% Copyright 2018-2024 The MathWorks, Inc.
+
+% Defaults
+x_extension = 0;
+showplot = 'n';
+
+% Check if extension specified
+if (nargin >= 2)
+    x_extension = varargin{1};
+end
+% Check if plot requested
+if (nargin >= 2)
+    showplot = varargin{2};
+end
+
+% Create STL from CRG
+% crg_read is a from OpenCRG
+crg_data = crg_read(crg_file);
+data = crg_data;
+[nu, nv] = size(data.z);
+
+iu =  [1 nu];
+iv =  [1 nv];
+
+%% Generate Auxiliary Data
+nuiu = iu(2) - iu(1) + 1;
+nviv = iv(2) - iv(1) + 1;
+
+u = zeros(1, nuiu);
+for i = 1:nuiu
+    u(i) = data.head.ubeg + (i-1+iu(1)-1)*data.head.uinc;
+end
+
+if isfield(data.head, 'vinc')
+    v = zeros(1, nviv);
+    for i=1:nviv
+        v(i) = data.head.vmin + (i-1+iv(1)-1)*data.head.vinc;
+    end
+else
+    v = double(data.v(iv(1):iv(2)));
+end
+
+% Grid Coordinates in Inertial System
+x = zeros(nuiu, nviv);
+y = zeros(nuiu, nviv);
+z = zeros(nuiu, nviv);
+puv = zeros(nviv, 2);
+for i = 1:nuiu
+    puv(:, 1) = u(i);
+    puv(:, 2) = v';
+    [pxy, data] = crg_eval_uv2xy(data, puv);
+    x(i, :) = pxy(: , 1);
+    y(i, :) = pxy(: , 2);
+    [pz, data] = crg_eval_uv2z(data, puv);
+    z(i, :) = pz;
+end
+
+% Add flat surface at end along +x axis if desired
+x1 = [x;x(end,:)+x_extension];
+y1 = [y;y(end,:)];
+z1 = [z;z(end,:)];
+
+% Write STLs for upper and lower surface temporarily
+surf2stl('temp_stl_upper.stl',flipud(x1),flipud(y1),flipud(z1),'ascii')
+surf2stl('temp_stl_lower.stl',x1,y1,z1,'ascii')
+
+% Merge upper and lower surfaces into a single STL file.
+upr_fid = fopen('temp_stl_upper.stl');
+low_fid = fopen('temp_stl_lower.stl');
+uprlow_fid = fopen(stl_file,'w');
+line_of_text = 'firstline';
+
+while(~contains(line_of_text,'endsolid'))
+    line_of_text = fgets(upr_fid);
+    if(~contains(line_of_text,'endsolid'))
+        fprintf(uprlow_fid,line_of_text);
+    end
+end
+
+skip_line = 1;
+while(~feof(low_fid))
+    line_of_text = fgets(low_fid);
+    if((skip_line==1) && contains(line_of_text,'facet normal'))
+        skip_line = 0;
+    end
+    if(skip_line==0)
+        fprintf(uprlow_fid,line_of_text);
+    end
+end
+
+% Close files
+fclose(upr_fid);
+fclose(low_fid);
+fclose(uprlow_fid);
+
+% Delete temporary STL files
+delete('temp_stl_lower.stl');
+delete('temp_stl_upper.stl');
+
+% Plot surface if desired
+if (strcmpi(showplot,'plot'))
+    
+    % Figure name
+    figString = ['h1_' mfilename];
+    % Only create a figure if no figure exists
+    figExist = 0;
+    fig_hExist = evalin('base',['exist(''' figString ''')']);
+    if (fig_hExist)
+        figExist = evalin('base',['ishandle(' figString ') && strcmp(get(' figString ', ''type''), ''figure'')']);
+    end
+    if ~figExist
+        fig_h = figure('Name',figString);
+        assignin('base',figString,fig_h);
+    else
+        fig_h = evalin('base',figString);
+    end
+    figure(fig_h)
+    clf(fig_h)
+    
+    % Plot surface
+    surf(x1',y1',z1','LineStyle','none')
+    title(strrep(stl_file,'_','\_'));
+    axis equal
+    box on
+end
+
+