Skip to content

Commit 3128685

Browse files
Tinker-TwinsTinker-Twins
committed
Add MATLAB API for Hunter SE
1 parent abcff5e commit 3128685

File tree

2 files changed

+209
-0
lines changed

2 files changed

+209
-0
lines changed

‎autodrive_matlab_api/autodrive_hunter.m‎

Lines changed: 136 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,136 @@
1+
classdef autodrive_hunter < handle
2+
% AutoDRIVE Hunter SE API
3+
% Attributes and methods to store and parse Hunter SE data and commands
4+
5+
properties
6+
% Hunter SE data
7+
id;
8+
throttle;
9+
steering;
10+
encoder_ticks;
11+
encoder_angles;
12+
position;
13+
orientation_quaternion;
14+
orientation_euler_angles;
15+
angular_velocity;
16+
linear_acceleration;
17+
lidar_pointcloud;
18+
front_camera_image;
19+
rear_camera_image;
20+
% Hunter SE commands
21+
cosim_mode;
22+
posX_command;
23+
posY_command;
24+
posZ_command;
25+
rotX_command;
26+
rotY_command;
27+
rotZ_command;
28+
rotW_command;
29+
throttle_command;
30+
steering_command;
31+
end
32+
33+
methods
34+
function parse_data(obj,data,frontcamera_ax,rearcamera_ax,pointcloud_ax,verbose)
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,'%d'));
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+
obj.lidar_pointcloud = matlab.net.base64decode(data.V1LIDARPointcloud);
50+
% Cameras
51+
obj.front_camera_image = matlab.net.base64decode(data.V1FrontCameraImage);
52+
obj.front_camera_image = javax.imageio.ImageIO.read(java.io.ByteArrayInputStream(obj.front_camera_image));
53+
obj.front_camera_image = reshape(typecast(obj.front_camera_image.getData.getDataStorage, 'uint8'), [3,1280,720]);
54+
obj.front_camera_image = cat(3,transpose(reshape(obj.front_camera_image(3,:,:), [1280,720])), ...
55+
transpose(reshape(obj.front_camera_image(2,:,:), [1280,720])), ...
56+
transpose(reshape(obj.front_camera_image(1,:,:), [1280,720])));
57+
obj.rear_camera_image = matlab.net.base64decode(data.V1RearCameraImage);
58+
obj.rear_camera_image = javax.imageio.ImageIO.read(java.io.ByteArrayInputStream(obj.rear_camera_image));
59+
obj.rear_camera_image = reshape(typecast(obj.rear_camera_image.getData.getDataStorage, 'uint8'), [3,1280,720]);
60+
obj.rear_camera_image = cat(3,transpose(reshape(obj.rear_camera_image(3,:,:), [1280,720])), ...
61+
transpose(reshape(obj.rear_camera_image(2,:,:), [1280,720])), ...
62+
transpose(reshape(obj.rear_camera_image(1,:,:), [1280,720])));
63+
if verbose
64+
fprintf('\n--------------------------------\n')
65+
fprintf('Receive Data from Hunter SE:\n')
66+
fprintf('--------------------------------\n\n')
67+
% Monitor Hunter SE data
68+
fprintf('Throttle: %f\n',obj.throttle)
69+
fprintf('Steering: %f\n',obj.steering)
70+
fprintf('Encoder Ticks: [%d %d]\n',obj.encoder_ticks(1),obj.encoder_ticks(2))
71+
fprintf('Encoder Angles: [%d %d]\n',obj.encoder_angles(1),obj.encoder_angles(2))
72+
fprintf('Position: [%f %f %f]\n',obj.position(1),obj.position(2),obj.position(3))
73+
fprintf('Orientation [Quaternion]: [%f %f %f %f]\n',obj.orientation_quaternion(1),obj.orientation_quaternion(2),obj.orientation_quaternion(3),obj.orientation_quaternion(4))
74+
fprintf('Orientation [Euler Angles]: [%f %f %f]\n',obj.orientation_euler_angles(1),obj.orientation_euler_angles(2),obj.orientation_euler_angles(3))
75+
fprintf('Angular Velocity: [%f %f %f]\n',obj.angular_velocity(1),obj.angular_velocity(2),obj.angular_velocity(3))
76+
fprintf('Linear Acceleration: [%f %f %f]\n',obj.linear_acceleration(1),obj.linear_acceleration(2),obj.linear_acceleration(3))
77+
% Visualize camera frames
78+
% Front camera
79+
imshow(imresize(obj.front_camera_image, 0.5),'Parent',frontcamera_ax);
80+
drawnow;
81+
% Rear camera
82+
imshow(imresize(obj.rear_camera_image, 0.5),'Parent',rearcamera_ax);
83+
drawnow;
84+
% Visualize LIDAR pointcloud
85+
% Convert the byte array to numeric values
86+
numericValues = typecast(uint8(obj.lidar_pointcloud), 'single');
87+
% Reshape the numeric values into a 3-column matrix (assuming each point has x, y, and z coordinates)
88+
numPoints = numel(numericValues) / 3;
89+
pointCloud = reshape(numericValues, 3, numPoints)';
90+
% Extract x, y, and z coordinates
91+
x = pointCloud(:, 1);
92+
y = pointCloud(:, 2);
93+
z = pointCloud(:, 3);
94+
% Plot pointcloud
95+
colormap jet
96+
scatter3(x, y, z, 1, z, 'filled','Parent',pointcloud_ax);
97+
xlabel('X');
98+
ylabel('Y');
99+
zlabel('Z');
100+
title('LIDAR Pointcloud');
101+
colorbar;
102+
axis equal;
103+
drawnow;
104+
end
105+
end
106+
107+
function data = generate_commands(obj,verbose)
108+
if verbose
109+
fprintf('\n--------------------------------\n')
110+
fprintf('Transmit Data to Hunter SE:\n')
111+
fprintf('--------------------------------\n\n')
112+
% Monitor Hunter SE control commands
113+
if obj.cosim_mode == 0
114+
cosim_mode_str = 'False';
115+
else
116+
cosim_mode_str = 'True';
117+
end
118+
fprintf('Co-Simulation Mode: %s\n',cosim_mode_str)
119+
fprintf('Position Command: [%d %d %d]\n',obj.posX_command, obj.posY_command, obj.posZ_command)
120+
fprintf('Rotation Command: [%d %d %d %d]\n',obj.rotX_command, obj.rotY_command, obj.rotZ_command, obj.rotW_command)
121+
fprintf('Throttle Command: %d\n',obj.throttle_command)
122+
fprintf('Steering Command: %d\n',obj.steering_command)
123+
end
124+
data = strcat('42["Bridge",{"V1 CoSim":"',num2str(obj.cosim_mode), ...
125+
'","V1 PosX":"',num2str(obj.posX_command), ...
126+
'","V1 PosY":"',num2str(obj.posY_command), ...
127+
'","V1 PosZ":"',num2str(obj.posZ_command), ...
128+
'","V1 RotX":"',num2str(obj.rotX_command), ...
129+
'","V1 RotY":"',num2str(obj.rotY_command), ...
130+
'","V1 RotZ":"',num2str(obj.rotZ_command), ...
131+
'","V1 RotW":"',num2str(obj.rotW_command), ...
132+
'","V1 Throttle":"',num2str(obj.throttle_command), ...
133+
'","V1 Steering":"',num2str(obj.steering_command),'"}]');
134+
end
135+
end
136+
end

‎autodrive_matlab_api/example_hunter.m‎

Lines changed: 73 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,73 @@
1+
classdef example_hunter < WebSocketServer
2+
% AutoDRIVE Hunter SE Example
3+
% Implements a MATLAB WebSocket server to communicate with AutoDRIVE
4+
% Simulator
5+
6+
properties
7+
hunter_1 = autodrive_hunter;
8+
frontcamera_fig = figure(1)
9+
frontcamera_ax = axes;
10+
rearcamera_fig = figure(2)
11+
rearcamera_ax = axes;
12+
pointcloud_fig = figure(3)
13+
pointcloud_ax = axes;
14+
end
15+
16+
methods
17+
function obj = example_hunter(varargin)
18+
% Constructor
19+
obj@WebSocketServer(varargin{:});
20+
end
21+
end
22+
23+
methods (Access = protected)
24+
function onOpen(obj,conn,message)
25+
fprintf('%s\n',message)
26+
end
27+
28+
function onTextMessage(obj,conn,message)
29+
% This function recieves the incoming data from WebSocket,
30+
% parses incoming data, prepares outgoing data, and transmits
31+
% the outgoing data over WebSocket
32+
33+
% Recieve and decode incoming JSON message
34+
in_data = jsondecode(message(13:end-1));
35+
% Parse incoming data
36+
obj.hunter_1.parse_data(in_data,obj.frontcamera_ax,obj.rearcamera_ax,obj.pointcloud_ax,true);
37+
38+
% Prepare outgoing data
39+
obj.hunter_1.cosim_mode = 0;
40+
obj.hunter_1.posX_command = 0;
41+
obj.hunter_1.posY_command = 0;
42+
obj.hunter_1.posZ_command = 0;
43+
obj.hunter_1.rotX_command = 0;
44+
obj.hunter_1.rotY_command = 0;
45+
obj.hunter_1.rotZ_command = 0;
46+
obj.hunter_1.rotW_command = 0;
47+
obj.hunter_1.throttle_command = 1;
48+
obj.hunter_1.steering_command = 1;
49+
% Encode outgoing data as a JSON message
50+
out_data = obj.hunter_1.generate_commands(true);
51+
% Transmit outgoing JSON message
52+
conn.send(out_data);
53+
end
54+
55+
function onBinaryMessage(obj,conn,bytearray)
56+
% This function sends an echo back to the client
57+
conn.send(bytearray); % Echo
58+
end
59+
60+
function onError(obj,conn,message)
61+
fprintf('%s\n',message)
62+
end
63+
64+
function onClose(obj,conn,message)
65+
fprintf('%s\n',message)
66+
end
67+
end
68+
end
69+
70+
%autodrive = example_hunter(4567)
71+
%autodrive.stop
72+
%delete(autodrive)
73+
%clear autodrive

0 commit comments

Comments
 (0)