Skip to content

Commit 7d959ad

Browse files
Tinker-TwinsTinker-Twins
committed
Add Simulink API for Hunter SE
1 parent 2e4f84e commit 7d959ad

File tree

4 files changed

+414
-0
lines changed

4 files changed

+414
-0
lines changed

‎autodrive_simulink_api/autodrive_hunter.m‎

Lines changed: 283 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,283 @@
1+
function autodrive_hunter(block)
2+
% AutoDRIVE Hunter SE Simulink API
3+
% This is a Level-2 MATLAB S-Function wrapper for AutoDRIVE Hunter SE API
4+
5+
% Copyright 2024 AutoDRIVE Ecosystem
6+
7+
%% The setup method is used to set up the basic attributes of the
8+
%% S-function such as ports, parameters, etc. Do not add any other
9+
%% calls to the main body of the function.
10+
11+
setup(block);
12+
13+
%endfunction
14+
15+
%% Function: setup
16+
%% Abstract:
17+
%% Set up the basic characteristics of the S-function block such as:
18+
%% - Input ports
19+
%% - Output ports
20+
%% - Dialog parameters
21+
%% - Options
22+
%%
23+
%% Required: Yes
24+
%% C MEX counterpart: mdlInitializeSizes
25+
26+
function setup(block)
27+
28+
% Allow more than 2D signals
29+
block.AllowSignalsWithMoreThan2D = 1;
30+
31+
% Register number of ports
32+
block.NumInputPorts = 10;
33+
block.NumOutputPorts = 13;
34+
35+
% Setup port properties to be inherited or dynamic
36+
block.SetPreCompInpPortInfoToDynamic;
37+
block.SetPreCompOutPortInfoToDynamic;
38+
39+
% Override input port properties
40+
% Co-Simulation Mode
41+
block.InputPort(1).Dimensions = 1;
42+
block.InputPort(1).DatatypeID = 3; % uint8
43+
block.InputPort(1).Complexity = 'Real';
44+
block.InputPort(1).DirectFeedthrough = true;
45+
% Position X Component
46+
block.InputPort(2).Dimensions = 1;
47+
block.InputPort(2).DatatypeID = 0; % double
48+
block.InputPort(2).Complexity = 'Real';
49+
block.InputPort(2).DirectFeedthrough = true;
50+
% Position Y Component
51+
block.InputPort(3).Dimensions = 1;
52+
block.InputPort(3).DatatypeID = 0; % double
53+
block.InputPort(3).Complexity = 'Real';
54+
block.InputPort(3).DirectFeedthrough = true;
55+
% Position Z Component
56+
block.InputPort(4).Dimensions = 1;
57+
block.InputPort(4).DatatypeID = 0; % double
58+
block.InputPort(4).Complexity = 'Real';
59+
block.InputPort(4).DirectFeedthrough = true;
60+
% Orientation Quaternion X Component
61+
block.InputPort(5).Dimensions = 1;
62+
block.InputPort(5).DatatypeID = 0; % double
63+
block.InputPort(5).Complexity = 'Real';
64+
block.InputPort(5).DirectFeedthrough = true;
65+
% Orientation Quaternion Y Component
66+
block.InputPort(6).Dimensions = 1;
67+
block.InputPort(6).DatatypeID = 0; % double
68+
block.InputPort(6).Complexity = 'Real';
69+
block.InputPort(6).DirectFeedthrough = true;
70+
% Orientation Quaternion Z Component
71+
block.InputPort(7).Dimensions = 1;
72+
block.InputPort(7).DatatypeID = 0; % double
73+
block.InputPort(7).Complexity = 'Real';
74+
block.InputPort(7).DirectFeedthrough = true;
75+
% Orientation Quaternion W Component
76+
block.InputPort(8).Dimensions = 1;
77+
block.InputPort(8).DatatypeID = 0; % double
78+
block.InputPort(8).Complexity = 'Real';
79+
block.InputPort(8).DirectFeedthrough = true;
80+
% Throttle Command
81+
block.InputPort(9).Dimensions = 1;
82+
block.InputPort(9).DatatypeID = 0; % double
83+
block.InputPort(9).Complexity = 'Real';
84+
block.InputPort(9).DirectFeedthrough = true;
85+
% Steering Command
86+
block.InputPort(10).Dimensions = 1;
87+
block.InputPort(10).DatatypeID = 0; % double
88+
block.InputPort(10).Complexity = 'Real';
89+
block.InputPort(10).DirectFeedthrough = true;
90+
91+
% Override output port properties
92+
% Vehicle ID
93+
block.OutputPort(1).Dimensions = 1;
94+
block.OutputPort(1).DatatypeID = 5; % uint16
95+
block.OutputPort(1).Complexity = 'Real';
96+
% Throttle Feedback
97+
block.OutputPort(2).Dimensions = 1;
98+
block.OutputPort(2).DatatypeID = 0; % double
99+
block.OutputPort(2).Complexity = 'Real';
100+
% Steering Feedback
101+
block.OutputPort(3).Dimensions = 1;
102+
block.OutputPort(3).DatatypeID = 0; % double
103+
block.OutputPort(3).Complexity = 'Real';
104+
% Encoder Ticks
105+
block.OutputPort(4).Dimensions = [2 1];
106+
block.OutputPort(4).DatatypeID = 6; % int32
107+
block.OutputPort(4).Complexity = 'Real';
108+
% Encoder Angles
109+
block.OutputPort(5).Dimensions = [2 1];
110+
block.OutputPort(5).DatatypeID = 0; % double
111+
block.OutputPort(5).Complexity = 'Real';
112+
% Position
113+
block.OutputPort(6).Dimensions = [3 1];
114+
block.OutputPort(6).DatatypeID = 0; % double
115+
block.OutputPort(6).Complexity = 'Real';
116+
% Orientation (Quaternion)
117+
block.OutputPort(7).Dimensions = [4 1];
118+
block.OutputPort(7).DatatypeID = 0; % double
119+
block.OutputPort(7).Complexity = 'Real';
120+
% Orientation (Euler Angles)
121+
block.OutputPort(8).Dimensions = [3 1];
122+
block.OutputPort(8).DatatypeID = 0; % double
123+
block.OutputPort(8).Complexity = 'Real';
124+
% Angular Velocity
125+
block.OutputPort(9).Dimensions = [3 1];
126+
block.OutputPort(9).DatatypeID = 0; % double
127+
block.OutputPort(9).Complexity = 'Real';
128+
% Linear Acceleration
129+
block.OutputPort(10).Dimensions = [3 1];
130+
block.OutputPort(10).DatatypeID = 0; % double
131+
block.OutputPort(10).Complexity = 'Real';
132+
% LIDAR Pointcloud
133+
block.OutputPort(11).DimensionsMode = 'Variable';
134+
block.OutputPort(11).Dimensions = [57600,3]; % max. size
135+
block.OutputPort(11).DatatypeID = 1; % single
136+
block.OutputPort(11).Complexity = 'Real';
137+
% Camera 01
138+
block.OutputPort(12).Dimensions = [720,1280,3];
139+
block.OutputPort(12).DatatypeID = 3; % uint8
140+
block.OutputPort(12).Complexity = 'Real';
141+
% Camera 02
142+
block.OutputPort(13).Dimensions = [720,1280,3];
143+
block.OutputPort(13).DatatypeID = 3; % uint8
144+
block.OutputPort(13).Complexity = 'Real';
145+
146+
% Register parameters
147+
block.NumDialogPrms = 1;
148+
149+
% Register sample times
150+
% [0 offset] : Continuous sample time
151+
% [positive_num offset] : Discrete sample time
152+
% [-1, 0] : Inherited sample time
153+
% [-2, 0] : Variable sample time
154+
block.SampleTimes = [1 0];
155+
156+
% Specify the block simStateCompliance. The allowed values are:
157+
% 'UnknownSimState', < The default setting; warn and assume DefaultSimState
158+
% 'DefaultSimState', < Same sim state as a built-in block
159+
% 'HasNoSimState', < No sim state
160+
% 'CustomSimState', < Has GetSimState and SetSimState methods
161+
% 'DisallowSimState' < Error out when saving or restoring the model sim state
162+
block.SimStateCompliance = 'DefaultSimState';
163+
164+
%% The MATLAB S-function uses an internal registry for all
165+
%% block methods. You should register all relevant methods
166+
%% (optional and required) as illustrated below. You may choose
167+
%% any suitable name for the methods and implement these methods
168+
%% as local functions within the same file. See comments
169+
%% provided for each function for more information.
170+
171+
% block.RegBlockMethod('PostPropagationSetup', @DoPostPropSetup);
172+
block.RegBlockMethod('InitializeConditions', @InitializeConditions);
173+
% block.RegBlockMethod('Start', @Start);
174+
block.RegBlockMethod('Outputs', @Outputs); % Required
175+
% block.RegBlockMethod('Update', @Update);
176+
% block.RegBlockMethod('Derivatives', @Derivatives);
177+
block.RegBlockMethod('Terminate', @Terminate); % Required
178+
179+
%end setup
180+
181+
%% PostPropagationSetup:
182+
%% Functionality: Setup work areas and state variables. Can
183+
%% also register run-time methods here
184+
%% Required: No
185+
%% C MEX counterpart: mdlSetWorkWidths
186+
187+
% function DoPostPropSetup(block)
188+
189+
%end DoPostPropSetup(block)
190+
191+
%% InitializeConditions:
192+
%% Functionality: Called at the start of simulation and if it is
193+
%% present in an enabled subsystem configured to reset
194+
%% states, it will be called when the enabled subsystem
195+
%% restarts execution to reset the states.
196+
%% Required: No
197+
%% C MEX counterpart: mdlInitializeConditions
198+
199+
function InitializeConditions(block)
200+
global autodrive
201+
autodrive = server_hunter(block.DialogPrm(1).Data);
202+
203+
%end InitializeConditions
204+
205+
%% Start:
206+
%% Functionality: Called once at start of model execution. If you
207+
%% have states that should be initialized once, this
208+
%% is the place to do it.
209+
%% Required: No
210+
%% C MEX counterpart: mdlStart
211+
212+
% function Start(block)
213+
214+
%end Start
215+
216+
%% Outputs:
217+
%% Functionality: Called to generate block outputs in
218+
%% simulation step
219+
%% Required: Yes
220+
%% C MEX counterpart: mdlOutputs
221+
222+
function Outputs(block)
223+
global autodrive
224+
% Parse inputs
225+
autodrive.hunter_1.cosim_mode = block.InputPort(1).Data;
226+
autodrive.hunter_1.posX_command = block.InputPort(2).Data;
227+
autodrive.hunter_1.posY_command = block.InputPort(3).Data;
228+
autodrive.hunter_1.posZ_command = block.InputPort(4).Data;
229+
autodrive.hunter_1.rotX_command = block.InputPort(5).Data;
230+
autodrive.hunter_1.rotY_command = block.InputPort(6).Data;
231+
autodrive.hunter_1.rotZ_command = block.InputPort(7).Data;
232+
autodrive.hunter_1.rotW_command = block.InputPort(8).Data;
233+
autodrive.hunter_1.throttle_command = block.InputPort(9).Data;
234+
autodrive.hunter_1.steering_command = block.InputPort(10).Data;
235+
% Configure outputs
236+
block.OutputPort(1).Data = autodrive.hunter_1.id;
237+
block.OutputPort(2).Data = autodrive.hunter_1.throttle;
238+
block.OutputPort(3).Data = autodrive.hunter_1.steering;
239+
block.OutputPort(4).Data = autodrive.hunter_1.encoder_ticks;
240+
block.OutputPort(5).Data = autodrive.hunter_1.encoder_angles;
241+
block.OutputPort(6).Data = autodrive.hunter_1.position;
242+
block.OutputPort(7).Data = autodrive.hunter_1.orientation_quaternion;
243+
block.OutputPort(8).Data = autodrive.hunter_1.orientation_euler_angles;
244+
block.OutputPort(9).Data = autodrive.hunter_1.angular_velocity;
245+
block.OutputPort(10).Data = autodrive.hunter_1.linear_acceleration;
246+
block.OutputPort(11).CurrentDimensions = [size(autodrive.hunter_1.lidar_pointcloud,1),3];
247+
block.OutputPort(11).Data = autodrive.hunter_1.lidar_pointcloud;
248+
block.OutputPort(12).Data = autodrive.hunter_1.front_camera_image;
249+
block.OutputPort(13).Data = autodrive.hunter_1.rear_camera_image;
250+
251+
%end Outputs
252+
253+
%% Update:
254+
%% Functionality: Called to update discrete states
255+
%% during simulation step
256+
%% Required: No
257+
%% C MEX counterpart: mdlUpdate
258+
259+
% function Update(block)
260+
261+
%end Update
262+
263+
%% Derivatives:
264+
%% Functionality: Called to update derivatives of
265+
%% continuous states during simulation step
266+
%% Required: No
267+
%% C MEX counterpart: mdlDerivatives
268+
269+
% function Derivatives(block)
270+
271+
%end Derivatives
272+
273+
%% Terminate:
274+
%% Functionality: Called at the end of simulation for cleanup
275+
%% Required: Yes
276+
%% C MEX counterpart: mdlTerminate
277+
278+
function Terminate(block)
279+
global autodrive
280+
autodrive.stop;
281+
autodrive.delete;
282+
283+
%end Terminate

‎autodrive_simulink_api/example_hunter.slx‎

149 KB
Binary file not shown.

‎autodrive_simulink_api/hunter.m‎

Lines changed: 81 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,81 @@
1+
classdef hunter < handle
2+
% AutoDRIVE Hunter SE MATLAB API
3+
% Attributes and methods to store and parse Hunter SE data and commands
4+
5+
properties
6+
% Hunter SE data
7+
id = uint16(1);
8+
throttle = double(0.0);
9+
steering = double(0.0);
10+
encoder_ticks = int32([0; 0]);
11+
encoder_angles = double([0; 0]);
12+
position = double([0; 0; 0]);
13+
orientation_quaternion = double([0; 0; 0; 0]);
14+
orientation_euler_angles = double([0; 0; 0]);
15+
angular_velocity = double([0; 0; 0]);
16+
linear_acceleration = double([0; 0; 0]);
17+
lidar_pointcloud = single(zeros(57600,3));
18+
front_camera_image = uint8(zeros(720,1280,3));
19+
rear_camera_image = uint8(zeros(720,1280,3));
20+
% Hunter SE commands
21+
cosim_mode = uint8(0);
22+
posX_command = double(0);
23+
posY_command = double(0);
24+
posZ_command = double(0);
25+
rotX_command = double(0);
26+
rotY_command = double(0);
27+
rotZ_command = double(0);
28+
rotW_command = double(0);
29+
throttle_command = double(0);
30+
steering_command = double(0);
31+
end
32+
33+
methods
34+
function parse_data(obj,data)
35+
% Actuator feedbacks
36+
obj.throttle = str2double(data.V1Throttle);
37+
obj.steering = str2double(data.V1Steering);
38+
% Wheel encoders
39+
obj.encoder_ticks = cell2mat(textscan(data.V1EncoderTicks,'%d'));
40+
obj.encoder_angles = cell2mat(textscan(data.V1EncoderAngles,'%f'));
41+
% IPS
42+
obj.position = cell2mat(textscan(data.V1Position,'%f'));
43+
% IMU
44+
obj.orientation_quaternion = cell2mat(textscan(data.V1OrientationQuaternion,'%f'));
45+
obj.orientation_euler_angles = cell2mat(textscan(data.V1OrientationEulerAngles,'%f'));
46+
obj.angular_velocity = cell2mat(textscan(data.V1AngularVelocity,'%f'));
47+
obj.linear_acceleration = cell2mat(textscan(data.V1LinearAcceleration,'%f'));
48+
% LIDAR
49+
raw_lidar_pointcloud = matlab.net.base64decode(data.V1LIDARPointcloud); % Raw byte array
50+
numericValues = typecast(uint8(raw_lidar_pointcloud), 'single'); % Convert to numeric values
51+
numPoints = numel(numericValues) / 3; % Get number of points
52+
obj.lidar_pointcloud = reshape(numericValues, 3, numPoints)'; % Reshape into 3-column matrix
53+
% Cameras
54+
obj.front_camera_image = matlab.net.base64decode(data.V1FrontCameraImage);
55+
obj.front_camera_image = javax.imageio.ImageIO.read(java.io.ByteArrayInputStream(obj.front_camera_image));
56+
obj.front_camera_image = reshape(typecast(obj.front_camera_image.getData.getDataStorage, 'uint8'), [3,1280,720]);
57+
obj.front_camera_image = cat(3,transpose(reshape(obj.front_camera_image(3,:,:), [1280,720])), ...
58+
transpose(reshape(obj.front_camera_image(2,:,:), [1280,720])), ...
59+
transpose(reshape(obj.front_camera_image(1,:,:), [1280,720])));
60+
obj.rear_camera_image = matlab.net.base64decode(data.V1RearCameraImage);
61+
obj.rear_camera_image = javax.imageio.ImageIO.read(java.io.ByteArrayInputStream(obj.rear_camera_image));
62+
obj.rear_camera_image = reshape(typecast(obj.rear_camera_image.getData.getDataStorage, 'uint8'), [3,1280,720]);
63+
obj.rear_camera_image = cat(3,transpose(reshape(obj.rear_camera_image(3,:,:), [1280,720])), ...
64+
transpose(reshape(obj.rear_camera_image(2,:,:), [1280,720])), ...
65+
transpose(reshape(obj.rear_camera_image(1,:,:), [1280,720])));
66+
end
67+
68+
function data = generate_commands(obj)
69+
data = strcat('42["Bridge",{"V1 CoSim":"',num2str(obj.cosim_mode), ...
70+
'","V1 PosX":"',num2str(obj.posX_command), ...
71+
'","V1 PosY":"',num2str(obj.posY_command), ...
72+
'","V1 PosZ":"',num2str(obj.posZ_command), ...
73+
'","V1 RotX":"',num2str(obj.rotX_command), ...
74+
'","V1 RotY":"',num2str(obj.rotY_command), ...
75+
'","V1 RotZ":"',num2str(obj.rotZ_command), ...
76+
'","V1 RotW":"',num2str(obj.rotW_command), ...
77+
'","V1 Throttle":"',num2str(obj.throttle_command), ...
78+
'","V1 Steering":"',num2str(obj.steering_command),'"}]');
79+
end
80+
end
81+
end

0 commit comments

Comments
 (0)