Skip to content

Commit 793ce35

Browse files
Tinker-TwinsTinker-Twins
committed
Add MATLAB API for RoboRacer
1 parent 45339c2 commit 793ce35

File tree

2 files changed

+190
-0
lines changed

2 files changed

+190
-0
lines changed

‎autodrive_matlab_api/autodrive_roboracer.m‎

Lines changed: 126 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,126 @@
1+
classdef autodrive_roboracer < handle
2+
% AutoDRIVE RoboRacer API
3+
% Attributes and methods to store and parse RoboRacer data and commands
4+
5+
properties
6+
% RoboRacer data
7+
id;
8+
throttle;
9+
steering;
10+
speed;
11+
encoder_ticks;
12+
encoder_angles;
13+
position;
14+
orientation_quaternion;
15+
orientation_euler_angles;
16+
angular_velocity;
17+
linear_acceleration;
18+
lidar_scan_rate;
19+
lidar_range_array;
20+
lidar_intensity_array;
21+
front_camera_image;
22+
% Race data
23+
lap_count;
24+
lap_time;
25+
last_lap_time;
26+
best_lap_time;
27+
collision_count;
28+
% RoboRacer commands
29+
throttle_command;
30+
steering_command;
31+
% Simulation commands
32+
reset_command = "False";
33+
end
34+
35+
methods
36+
function parse_data(obj,data,frontcamera_ax,laserscan_ax,verbose)
37+
% Actuator feedbacks
38+
obj.throttle = str2double(data.V1Throttle);
39+
obj.steering = str2double(data.V1Steering);
40+
% Speed
41+
obj.speed = str2double(data.V1Speed);
42+
% Wheel encoders
43+
obj.encoder_ticks = cell2mat(textscan(data.V1EncoderTicks,'%d'));
44+
obj.encoder_angles = cell2mat(textscan(data.V1EncoderAngles,'%d'));
45+
% IPS
46+
obj.position = cell2mat(textscan(data.V1Position,'%f'));
47+
% IMU
48+
obj.orientation_quaternion = cell2mat(textscan(data.V1OrientationQuaternion,'%f'));
49+
obj.orientation_euler_angles = cell2mat(textscan(data.V1OrientationEulerAngles,'%f'));
50+
obj.angular_velocity = cell2mat(textscan(data.V1AngularVelocity,'%f'));
51+
obj.linear_acceleration = cell2mat(textscan(data.V1LinearAcceleration,'%f'));
52+
% LIDAR
53+
obj.lidar_scan_rate = str2double(data.V1LIDARScanRate);
54+
obj.lidar_intensity_array = [];
55+
decodedBytes = matlab.net.base64decode(data.V1LIDARRangeArray);
56+
byteStream = java.io.ByteArrayInputStream(decodedBytes);
57+
gzipStream = java.util.zip.GZIPInputStream(byteStream);
58+
inputStreamReader = java.io.InputStreamReader(gzipStream, 'UTF-8');
59+
bufferedReader = java.io.BufferedReader(inputStreamReader);
60+
str = "";
61+
line = bufferedReader.readLine();
62+
while ~isempty(line)
63+
str = str + string(line) + newline;
64+
line = bufferedReader.readLine();
65+
end
66+
obj.lidar_range_array = sscanf(str, '%f');
67+
% Cameras
68+
obj.front_camera_image = matlab.net.base64decode(data.V1FrontCameraImage);
69+
obj.front_camera_image = javax.imageio.ImageIO.read(java.io.ByteArrayInputStream(obj.front_camera_image));
70+
obj.front_camera_image = reshape(typecast(obj.front_camera_image.getData.getDataStorage, 'uint8'), [3,192,108]);
71+
obj.front_camera_image = cat(3,transpose(reshape(obj.front_camera_image(3,:,:), [192,108])), ...
72+
transpose(reshape(obj.front_camera_image(2,:,:), [192,108])), ...
73+
transpose(reshape(obj.front_camera_image(1,:,:), [192,108])));
74+
% Race data
75+
obj.lap_count = uint16(str2double(data.V1LapCount));
76+
obj.lap_time = str2double(data.V1LapTime);
77+
obj.last_lap_time = str2double(data.V1LastLapTime);
78+
obj.best_lap_time = str2double(data.V1BestLapTime);
79+
obj.collision_count = uint16(str2double(data.V1Collisions));
80+
if verbose
81+
fprintf('\n--------------------------------\n')
82+
fprintf('Receive Data from RoboRacer:\n')
83+
fprintf('--------------------------------\n\n')
84+
% Monitor RoboRacer data
85+
fprintf('Throttle: %f\n',obj.throttle)
86+
fprintf('Steering: %f\n',obj.steering)
87+
fprintf('Speed: %f\n',obj.speed)
88+
fprintf('Encoder Ticks: [%d %d]\n',obj.encoder_ticks(1),obj.encoder_ticks(2))
89+
fprintf('Encoder Angles: [%d %d]\n',obj.encoder_angles(1),obj.encoder_angles(2))
90+
fprintf('Position: [%f %f %f]\n',obj.position(1),obj.position(2),obj.position(3))
91+
fprintf('Orientation [Quaternion]: [%f %f %f %f]\n',obj.orientation_quaternion(1),obj.orientation_quaternion(2),obj.orientation_quaternion(3),obj.orientation_quaternion(4))
92+
fprintf('Orientation [Euler Angles]: [%f %f %f]\n',obj.orientation_euler_angles(1),obj.orientation_euler_angles(2),obj.orientation_euler_angles(3))
93+
fprintf('Angular Velocity: [%f %f %f]\n',obj.angular_velocity(1),obj.angular_velocity(2),obj.angular_velocity(3))
94+
fprintf('Linear Acceleration: [%f %f %f]\n',obj.linear_acceleration(1),obj.linear_acceleration(2),obj.linear_acceleration(3))
95+
fprintf('Lap Count: %f\n',obj.lap_count)
96+
fprintf('Lap Time: %f\n',obj.lap_time)
97+
fprintf('Last Lap Time: %f\n',obj.last_lap_time)
98+
fprintf('Best Lap Time: %f\n',obj.best_lap_time)
99+
fprintf('Collision Count: %d\n',obj.collision_count)
100+
% Visualize camera frame
101+
imshow(imresize(obj.front_camera_image, 0.5),'Parent',frontcamera_ax);
102+
drawnow;
103+
% Visualize LIDAR laser scan
104+
angles = deg2rad(-45:0.25:225);
105+
scatter(laserscan_ax, angles,obj.lidar_range_array,1,'filled','red');
106+
title('LIDAR Laser Scan');
107+
drawnow;
108+
end
109+
end
110+
111+
function data = generate_commands(obj,verbose)
112+
if verbose
113+
fprintf('\n--------------------------------\n')
114+
fprintf('Transmit Data to RoboRacer:\n')
115+
fprintf('--------------------------------\n\n')
116+
% Monitor RoboRacer control commands
117+
fprintf('Throttle Command: %d\n',obj.throttle_command)
118+
fprintf('Steering Command: %d\n',obj.steering_command)
119+
fprintf('Reset Command: %s\n',mat2str(obj.reset_command))
120+
end
121+
data = strcat('42["Bridge",{"V1 Throttle":"',num2str(obj.throttle_command), ...
122+
'","V1 Steering":"',num2str(obj.steering_command), ...
123+
'","Reset":"',num2str(obj.reset_command),'"}]');
124+
end
125+
end
126+
end

‎autodrive_matlab_api/example_roboracer.m‎

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

0 commit comments

Comments
 (0)