Skip to content

Commit ab1ed68

Browse files
Tinker-TwinsTinker-Twins
committed
Add MATLAB API for Husky A200
1 parent 5ae9dce commit ab1ed68

File tree

2 files changed

+197
-0
lines changed

2 files changed

+197
-0
lines changed

‎autodrive_matlab_api/autodrive_husky.m‎

Lines changed: 126 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,126 @@
1+
classdef autodrive_husky < handle
2+
% AutoDRIVE Husky API
3+
% Attributes and methods to store and parse Husky data and commands
4+
5+
properties
6+
% Husky data
7+
id;
8+
throttle;
9+
differential;
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+
camera_image;
19+
% Husky commands
20+
cosim_mode;
21+
posX_command;
22+
posY_command;
23+
posZ_command;
24+
rotX_command;
25+
rotY_command;
26+
rotZ_command;
27+
rotW_command;
28+
lin_vel_command;
29+
ang_vel_command;
30+
end
31+
32+
methods
33+
function parse_data(obj,data,frontcamera_ax,pointcloud_ax,verbose)
34+
% Actuator feedbacks
35+
obj.throttle = str2double(data.V1Throttle);
36+
obj.differential = str2double(data.V1Steering);
37+
% Wheel encoders
38+
obj.encoder_ticks = cell2mat(textscan(data.V1EncoderTicks,'%d'));
39+
obj.encoder_angles = cell2mat(textscan(data.V1EncoderAngles,'%f'));
40+
% IPS
41+
obj.position = cell2mat(textscan(data.V1Position,'%f'));
42+
% IMU
43+
obj.orientation_quaternion = cell2mat(textscan(data.V1OrientationQuaternion,'%f'));
44+
obj.orientation_euler_angles = cell2mat(textscan(data.V1OrientationEulerAngles,'%f'));
45+
obj.angular_velocity = cell2mat(textscan(data.V1AngularVelocity,'%f'));
46+
obj.linear_acceleration = cell2mat(textscan(data.V1LinearAcceleration,'%f'));
47+
% LIDAR
48+
obj.lidar_pointcloud = matlab.net.base64decode(data.V1LIDARPointcloud);
49+
% Camera
50+
obj.camera_image = matlab.net.base64decode(data.V1FrontCameraImage);
51+
obj.camera_image = javax.imageio.ImageIO.read(java.io.ByteArrayInputStream(obj.camera_image));
52+
obj.camera_image = reshape(typecast(obj.camera_image.getData.getDataStorage, 'uint8'), [3,1280,720]);
53+
obj.camera_image = cat(3,transpose(reshape(obj.camera_image(3,:,:), [1280,720])), ...
54+
transpose(reshape(obj.camera_image(2,:,:), [1280,720])), ...
55+
transpose(reshape(obj.camera_image(1,:,:), [1280,720])));
56+
if verbose
57+
fprintf('\n--------------------------------\n')
58+
fprintf('Receive Data from Husky:\n')
59+
fprintf('--------------------------------\n\n')
60+
% Monitor Husky data
61+
fprintf('Throttle: %f\n',obj.throttle)
62+
fprintf('Differential: %f\n',obj.differential)
63+
fprintf('Encoder Ticks: [%d %d]\n',obj.encoder_ticks(1),obj.encoder_ticks(2))
64+
fprintf('Encoder Angles: [%d %d]\n',obj.encoder_angles(1),obj.encoder_angles(2))
65+
fprintf('Position: [%f %f %f]\n',obj.position(1),obj.position(2),obj.position(3))
66+
fprintf('Orientation [Quaternion]: [%f %f %f %f]\n',obj.orientation_quaternion(1),obj.orientation_quaternion(2),obj.orientation_quaternion(3),obj.orientation_quaternion(4))
67+
fprintf('Orientation [Euler Angles]: [%f %f %f]\n',obj.orientation_euler_angles(1),obj.orientation_euler_angles(2),obj.orientation_euler_angles(3))
68+
fprintf('Angular Velocity: [%f %f %f]\n',obj.angular_velocity(1),obj.angular_velocity(2),obj.angular_velocity(3))
69+
fprintf('Linear Acceleration: [%f %f %f]\n',obj.linear_acceleration(1),obj.linear_acceleration(2),obj.linear_acceleration(3))
70+
% Visualize camera frames
71+
% Camera
72+
imshow(imresize(obj.camera_image, 0.5),'Parent',frontcamera_ax);
73+
drawnow;
74+
% Visualize LIDAR pointcloud
75+
% Convert the byte array to numeric values
76+
numericValues = typecast(uint8(obj.lidar_pointcloud), 'single');
77+
% Reshape the numeric values into a 3-column matrix (assuming each point has x, y, and z coordinates)
78+
numPoints = numel(numericValues) / 3;
79+
pointCloud = reshape(numericValues, 3, numPoints)';
80+
% Extract x, y, and z coordinates
81+
x = pointCloud(:, 1);
82+
y = pointCloud(:, 2);
83+
z = pointCloud(:, 3);
84+
% Plot pointcloud
85+
colormap jet
86+
scatter3(x, y, z, 1, z, 'filled','Parent',pointcloud_ax);
87+
xlabel('X');
88+
ylabel('Y');
89+
zlabel('Z');
90+
title('LIDAR Pointcloud');
91+
colorbar;
92+
axis equal;
93+
drawnow;
94+
end
95+
end
96+
97+
function data = generate_commands(obj,verbose)
98+
if verbose
99+
fprintf('\n--------------------------------\n')
100+
fprintf('Transmit Data to Husky:\n')
101+
fprintf('--------------------------------\n\n')
102+
% Monitor Husky control commands
103+
if obj.cosim_mode == 0
104+
cosim_mode_str = 'False';
105+
else
106+
cosim_mode_str = 'True';
107+
end
108+
fprintf('Co-Simulation Mode: %s\n',cosim_mode_str)
109+
fprintf('Position Command: [%d %d %d]\n',obj.posX_command, obj.posY_command, obj.posZ_command)
110+
fprintf('Rotation Command: [%d %d %d %d]\n',obj.rotX_command, obj.rotY_command, obj.rotZ_command, obj.rotW_command)
111+
fprintf('Lin. Vel. Command: %d\n',obj.lin_vel_command)
112+
fprintf('Ang. Vel. Command: %d\n',obj.ang_vel_command)
113+
end
114+
data = strcat('42["Bridge",{"V1 CoSim":"',num2str(obj.cosim_mode), ...
115+
'","V1 PosX":"',num2str(obj.posX_command), ...
116+
'","V1 PosY":"',num2str(obj.posY_command), ...
117+
'","V1 PosZ":"',num2str(obj.posZ_command), ...
118+
'","V1 RotX":"',num2str(obj.rotX_command), ...
119+
'","V1 RotY":"',num2str(obj.rotY_command), ...
120+
'","V1 RotZ":"',num2str(obj.rotZ_command), ...
121+
'","V1 RotW":"',num2str(obj.rotW_command), ...
122+
'","V1 Linear Velocity":"',num2str(obj.lin_vel_command), ...
123+
'","V1 Angular Velocity":"',num2str(obj.ang_vel_command),'"}]');
124+
end
125+
end
126+
end

‎autodrive_matlab_api/example_husky.m‎

Lines changed: 71 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,71 @@
1+
classdef example_husky < WebSocketServer
2+
% AutoDRIVE Husky Example
3+
% Implements a MATLAB WebSocket server to communicate with AutoDRIVE
4+
% Simulator
5+
6+
properties
7+
husky_1 = autodrive_husky;
8+
frontcamera_fig = figure(1)
9+
frontcamera_ax = axes;
10+
pointcloud_fig = figure(2)
11+
pointcloud_ax = axes;
12+
end
13+
14+
methods
15+
function obj = example_husky(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.husky_1.parse_data(in_data,obj.frontcamera_ax,obj.pointcloud_ax,true);
35+
36+
% Prepare outgoing data
37+
obj.husky_1.cosim_mode = 0;
38+
obj.husky_1.posX_command = 0;
39+
obj.husky_1.posY_command = 0;
40+
obj.husky_1.posZ_command = 0;
41+
obj.husky_1.rotX_command = 0;
42+
obj.husky_1.rotY_command = 0;
43+
obj.husky_1.rotZ_command = 0;
44+
obj.husky_1.rotW_command = 0;
45+
obj.husky_1.lin_vel_command = 1;
46+
obj.husky_1.ang_vel_command = 0;
47+
% Encode outgoing data as a JSON message
48+
out_data = obj.husky_1.generate_commands(true);
49+
% Transmit outgoing JSON message
50+
conn.send(out_data);
51+
end
52+
53+
function onBinaryMessage(obj,conn,bytearray)
54+
% This function sends an echo back to the client
55+
conn.send(bytearray); % Echo
56+
end
57+
58+
function onError(obj,conn,message)
59+
fprintf('%s\n',message)
60+
end
61+
62+
function onClose(obj,conn,message)
63+
fprintf('%s\n',message)
64+
end
65+
end
66+
end
67+
68+
%autodrive = example_husky(4567)
69+
%autodrive.stop
70+
%delete(autodrive)
71+
%clear autodrive

0 commit comments

Comments
 (0)