Skip to content

Commit a0eb805

Browse files
Tinker-TwinsTinker-Twins
committed
Add MATLAB API for Polaris RZR and Environment
1 parent 945fe4a commit a0eb805

File tree

3 files changed

+314
-0
lines changed

3 files changed

+314
-0
lines changed

‎autodrive_matlab_api/autodrive_environment.m‎

Lines changed: 71 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,71 @@
1+
classdef autodrive_environment < handle
2+
% AutoDRIVE Environment API
3+
% Attributes and methods to store and parse environment commands
4+
5+
properties
6+
% Environmental conditions
7+
auto_time
8+
auto_time_str
9+
time_scale
10+
time_of_day
11+
weather_id
12+
cloud_intensity
13+
fog_intensity
14+
rain_intensity
15+
snow_intensity
16+
end
17+
18+
methods
19+
function data = generate_commands(obj,verbose)
20+
if(obj.auto_time == boolean(0))
21+
obj.auto_time_str = 'False';
22+
else
23+
obj.auto_time_str = 'True';
24+
end
25+
if verbose
26+
fprintf('\n--------------------------------\n')
27+
fprintf('Set Environmental Conditions:\n')
28+
fprintf('--------------------------------\n\n')
29+
% Monitor environmental conditions
30+
hours = floor(obj.time_of_day / 60);
31+
minutes = floor(mod(obj.time_of_day, 60));
32+
seconds = floor(mod(obj.time_of_day, 1) * 60);
33+
fprintf('Time: %d:%d:%.2f\n', hours, minutes, seconds)
34+
if obj.weather_id == 0
35+
weather_str = sprintf('Custom | Clouds: %.2f%%\tFog: %.2f%%\tRain: %.2f%%\tSnow: %.2f%%', ...
36+
round(obj.cloud_intensity * 100, 2), ...
37+
round(obj.fog_intensity * 100, 2), ...
38+
round(obj.rain_intensity * 100, 2), ...
39+
round(obj.snow_intensity * 100, 2));
40+
elseif obj.weather_id == 1
41+
weather_str = 'Sunny';
42+
elseif obj.weather_id == 2
43+
weather_str = 'Cloudy';
44+
elseif obj.weather_id == 3
45+
weather_str = 'Light Fog';
46+
elseif obj.weather_id == 4
47+
weather_str = 'Heavy Fog';
48+
elseif obj.weather_id == 5
49+
weather_str = 'Light Rain';
50+
elseif obj.weather_id == 6
51+
weather_str = 'Heavy Rain';
52+
elseif obj.weather_id == 7
53+
weather_str = 'Light Snow';
54+
elseif obj.weather_id == 8
55+
weather_str = 'Heavy Snow';
56+
else
57+
weather_str = 'Invalid';
58+
end
59+
fprintf('Weather: %s\n', weather_str)
60+
end
61+
data = strcat('","Auto Time":"',obj.auto_time_str, ...
62+
'","Time Scale":"',num2str(obj.time_scale), ...
63+
'","Time":"',num2str(obj.time_of_day), ...
64+
'","Weather":"',num2str(obj.weather_id), ...
65+
'","Clouds":"',num2str(obj.cloud_intensity), ...
66+
'","Fog":"',num2str(obj.fog_intensity), ...
67+
'","Rain":"',num2str(obj.rain_intensity), ...
68+
'","Snow":"',num2str(obj.snow_intensity));
69+
end
70+
end
71+
end

‎autodrive_matlab_api/autodrive_rzr.m‎

Lines changed: 159 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,159 @@
1+
classdef autodrive_rzr < handle
2+
% AutoDRIVE RZR API
3+
% Attributes and methods to store and parse RZR data and commands
4+
5+
properties
6+
% RZR data
7+
id;
8+
collision_count;
9+
throttle;
10+
steering;
11+
brake;
12+
handbrake;
13+
encoder_ticks;
14+
encoder_angles;
15+
position;
16+
orientation_quaternion;
17+
orientation_euler_angles;
18+
angular_velocity;
19+
linear_acceleration;
20+
% lidar_pointcloud;
21+
left_camera_image;
22+
right_camera_image;
23+
% RZR commands
24+
cosim_mode;
25+
posX_command;
26+
posY_command;
27+
posZ_command;
28+
rotX_command;
29+
rotY_command;
30+
rotZ_command;
31+
rotW_command;
32+
throttle_command;
33+
steering_command;
34+
brake_command;
35+
handbrake_command;
36+
headlights_command;
37+
% Environment commands
38+
env = autodrive_environment;
39+
end
40+
41+
methods
42+
function parse_data(obj,data,leftcamera_ax,rightcamera_ax,verbose)
43+
% Collision count
44+
obj.collision_count = uint16(str2double(data.V1Collisions));
45+
% Actuator feedbacks
46+
obj.throttle = str2double(data.V1Throttle);
47+
obj.steering = str2double(data.V1Steering);
48+
obj.brake = str2double(data.V1Brake);
49+
obj.handbrake = str2double(data.V1Handbrake);
50+
% Wheel encoders
51+
obj.encoder_ticks = cell2mat(textscan(data.V1EncoderTicks,'%d'));
52+
obj.encoder_angles = cell2mat(textscan(data.V1EncoderAngles,'%f'));
53+
% IPS
54+
obj.position = cell2mat(textscan(data.V1Position,'%f'));
55+
% IMU
56+
obj.orientation_quaternion = cell2mat(textscan(data.V1OrientationQuaternion,'%f'));
57+
obj.orientation_euler_angles = cell2mat(textscan(data.V1OrientationEulerAngles,'%f'));
58+
obj.angular_velocity = cell2mat(textscan(data.V1AngularVelocity,'%f'));
59+
obj.linear_acceleration = cell2mat(textscan(data.V1LinearAcceleration,'%f'));
60+
% LIDAR
61+
% obj.lidar_pointcloud = matlab.net.base64decode(data.V1LIDARPointcloud);
62+
% Cameras
63+
obj.left_camera_image = matlab.net.base64decode(data.V1LeftCameraImage);
64+
obj.left_camera_image = javax.imageio.ImageIO.read(java.io.ByteArrayInputStream(obj.left_camera_image));
65+
obj.left_camera_image = reshape(typecast(obj.left_camera_image.getData.getDataStorage, 'uint8'), [3,1280,720]);
66+
obj.left_camera_image = cat(3,transpose(reshape(obj.left_camera_image(3,:,:), [1280,720])), ...
67+
transpose(reshape(obj.left_camera_image(2,:,:), [1280,720])), ...
68+
transpose(reshape(obj.left_camera_image(1,:,:), [1280,720])));
69+
obj.right_camera_image = matlab.net.base64decode(data.V1RightCameraImage);
70+
obj.right_camera_image = javax.imageio.ImageIO.read(java.io.ByteArrayInputStream(obj.right_camera_image));
71+
obj.right_camera_image = reshape(typecast(obj.right_camera_image.getData.getDataStorage, 'uint8'), [3,1280,720]);
72+
obj.right_camera_image = cat(3,transpose(reshape(obj.right_camera_image(3,:,:), [1280,720])), ...
73+
transpose(reshape(obj.right_camera_image(2,:,:), [1280,720])), ...
74+
transpose(reshape(obj.right_camera_image(1,:,:), [1280,720])));
75+
if verbose
76+
fprintf('\n--------------------------------\n')
77+
fprintf('Receive Data from RZR:\n')
78+
fprintf('--------------------------------\n\n')
79+
% Monitor RZR data
80+
fprintf('Collisions: %f\n',obj.collision_count)
81+
fprintf('Throttle: %f\n',obj.throttle)
82+
fprintf('Steering: %f\n',obj.steering)
83+
fprintf('Brake: %f\n',obj.brake)
84+
fprintf('Handbrake: %f\n',obj.handbrake)
85+
fprintf('Encoder Ticks: [%d %d]\n',obj.encoder_ticks(1),obj.encoder_ticks(2))
86+
fprintf('Encoder Angles: [%d %d]\n',obj.encoder_angles(1),obj.encoder_angles(2))
87+
fprintf('Position: [%f %f %f]\n',obj.position(1),obj.position(2),obj.position(3))
88+
fprintf('Orientation [Quaternion]: [%f %f %f %f]\n',obj.orientation_quaternion(1),obj.orientation_quaternion(2),obj.orientation_quaternion(3),obj.orientation_quaternion(4))
89+
fprintf('Orientation [Euler Angles]: [%f %f %f]\n',obj.orientation_euler_angles(1),obj.orientation_euler_angles(2),obj.orientation_euler_angles(3))
90+
fprintf('Angular Velocity: [%f %f %f]\n',obj.angular_velocity(1),obj.angular_velocity(2),obj.angular_velocity(3))
91+
fprintf('Linear Acceleration: [%f %f %f]\n',obj.linear_acceleration(1),obj.linear_acceleration(2),obj.linear_acceleration(3))
92+
% Visualize camera frames
93+
% Left camera
94+
imshow(imresize(obj.left_camera_image, 0.5),'Parent',leftcamera_ax);
95+
drawnow;
96+
% Right camera
97+
imshow(imresize(obj.right_camera_image, 0.5),'Parent',rightcamera_ax);
98+
drawnow;
99+
% Visualize LIDAR pointcloud
100+
% % Convert the byte array to numeric values
101+
% numericValues = typecast(uint8(obj.lidar_pointcloud), 'single');
102+
% % Reshape the numeric values into a 3-column matrix (assuming each point has x, y, and z coordinates)
103+
% numPoints = numel(numericValues) / 3;
104+
% pointCloud = reshape(numericValues, 3, numPoints)';
105+
% % Extract x, y, and z coordinates
106+
% x = pointCloud(:, 1);
107+
% y = pointCloud(:, 2);
108+
% z = pointCloud(:, 3);
109+
% % Plot pointcloud
110+
% colormap jet
111+
% scatter3(x, y, z, 1, z, 'filled','Parent',pointcloud_ax);
112+
% xlabel('X');
113+
% ylabel('Y');
114+
% zlabel('Z');
115+
% title('LIDAR Pointcloud');
116+
% colorbar;
117+
% axis equal;
118+
% drawnow;
119+
end
120+
end
121+
122+
function data = generate_commands(obj,verbose)
123+
if verbose
124+
fprintf('\n--------------------------------\n')
125+
fprintf('Transmit Data to RZR:\n')
126+
fprintf('--------------------------------\n\n')
127+
% Monitor RZR control commands
128+
if obj.cosim_mode == 0
129+
cosim_mode_str = 'False';
130+
else
131+
cosim_mode_str = 'True';
132+
end
133+
fprintf('Co-Simulation Mode: %s\n',cosim_mode_str)
134+
fprintf('Position Command: [%d %d %d]\n',obj.posX_command, obj.posY_command, obj.posZ_command)
135+
fprintf('Rotation Command: [%d %d %d %d]\n',obj.rotX_command, obj.rotY_command, obj.rotZ_command, obj.rotW_command)
136+
fprintf('Throttle Command: %d\n',obj.throttle_command)
137+
fprintf('Steering Command: %d\n',obj.steering_command)
138+
fprintf('Brake Command: %d\n',obj.brake_command)
139+
fprintf('Handbrake Command: %d\n',obj.handbrake_command)
140+
fprintf('Headlights Command: %d\n',obj.headlights_command)
141+
end
142+
data = obj.env.generate_commands(true);
143+
data = strcat('42["Bridge",{"V1 CoSim":"',num2str(obj.cosim_mode), ...
144+
'","V1 PosX":"',num2str(obj.posX_command), ...
145+
'","V1 PosY":"',num2str(obj.posY_command), ...
146+
'","V1 PosZ":"',num2str(obj.posZ_command), ...
147+
'","V1 RotX":"',num2str(obj.rotX_command), ...
148+
'","V1 RotY":"',num2str(obj.rotY_command), ...
149+
'","V1 RotZ":"',num2str(obj.rotZ_command), ...
150+
'","V1 RotW":"',num2str(obj.rotW_command), ...
151+
'","V1 Throttle":"',num2str(obj.throttle_command), ...
152+
'","V1 Steering":"',num2str(obj.steering_command), ...
153+
'","V1 Brake":"',num2str(obj.brake_command), ...
154+
'","V1 Handbrake":"',num2str(obj.handbrake_command), ...
155+
'","V1 Headlights":"',num2str(obj.headlights_command), ...
156+
data,'"}]');
157+
end
158+
end
159+
end

‎autodrive_matlab_api/example_rzr.m‎

Lines changed: 84 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,84 @@
1+
classdef example_rzr < WebSocketServer
2+
% AutoDRIVE RZR Example
3+
% Implements a MATLAB WebSocket server to communicate with AutoDRIVE
4+
% Simulator
5+
6+
properties
7+
rzr_1 = autodrive_rzr;
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_rzr(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.rzr_1.parse_data(in_data,obj.frontcamera_ax,obj.rearcamera_ax,true);
37+
38+
% Prepare outgoing data
39+
obj.rzr_1.cosim_mode = 0;
40+
obj.rzr_1.posX_command = 0;
41+
obj.rzr_1.posY_command = 0;
42+
obj.rzr_1.posZ_command = 0;
43+
obj.rzr_1.rotX_command = 0;
44+
obj.rzr_1.rotY_command = 0;
45+
obj.rzr_1.rotZ_command = 0;
46+
obj.rzr_1.rotW_command = 0;
47+
obj.rzr_1.throttle_command = 0.5;
48+
obj.rzr_1.steering_command = 0;
49+
obj.rzr_1.brake_command = 0;
50+
obj.rzr_1.handbrake_command = 0;
51+
obj.rzr_1.headlights_command = 1;
52+
obj.rzr_1.env.auto_time = boolean(0);
53+
obj.rzr_1.env.time_scale = 60;
54+
obj.rzr_1.env.time_of_day = 560;
55+
obj.rzr_1.env.weather_id = uint8(3);
56+
obj.rzr_1.env.cloud_intensity = 0;
57+
obj.rzr_1.env.fog_intensity = 0;
58+
obj.rzr_1.env.rain_intensity = 0;
59+
obj.rzr_1.env.snow_intensity = 0;
60+
% Encode outgoing data as a JSON message
61+
out_data = obj.rzr_1.generate_commands(true);
62+
% Transmit outgoing JSON message
63+
conn.send(out_data);
64+
end
65+
66+
function onBinaryMessage(obj,conn,bytearray)
67+
% This function sends an echo back to the client
68+
conn.send(bytearray); % Echo
69+
end
70+
71+
function onError(obj,conn,message)
72+
fprintf('%s\n',message)
73+
end
74+
75+
function onClose(obj,conn,message)
76+
fprintf('%s\n',message)
77+
end
78+
end
79+
end
80+
81+
%autodrive = example_rzr(4567)
82+
%autodrive.stop
83+
%delete(autodrive)
84+
%clear autodrive

0 commit comments

Comments
 (0)