Make sure that you have set up your Matlab installation properly to support using our FastDDS Mex bindings.
Also, some more useful information regarding Matlab usage in the Lab in general can be found here for communication with the Middleware (ignore the RTI DDS part, but e.g. setting the middleware domain id or varargin are necessary) and here for the recommended script setup, and how to call your program from the LCC is explained here.
Matlab will not be able to execute your MEX-Files without preparation!
export LD_PRELOAD="/usr/lib/x86_64-linux-gnu/libstdc++.so.6:/home/leon/dev/software/cpm_lib/build/libcpm.so:/usr/local/lib/libfastcdr.so::/usr/local/lib/libfastrtps.so" |
setenv("LD_LIBRARY_PATH", [getenv('LD_LIBRARY_PATH'), ':/home/[username]/dev/software/cpm_lib/build/', ':/usr/local/lib/']); |
At this point, you should already be able to execute the MEX files directly. You may wonder why libstdc++ is included as well. Matlab comes with its own C++ Compiler / Environment, which is usually outdated, and depending on your system setup, MEX may build files that are incompatible with Matlab's C++ version when executing these files in Matlab, resulting in an error. Thus, you either need to set LD_PRELOAD globally in your system or replace Matlabs own C++ version with a link to your system's version. Furthermore, you need to link libraries that you want to use in your MEX files before Matlab is started, else it seems that Matlab is not able to find them.
If you made any mistake: Make sure that all readers and writers are cleared, and do that manually (shown at the end of this page) if necessary. Else, you may be stuck with an error-throwing version (even when it comes to loading the libraries) even after you have fixed the bugs that caused that error.
You should now be ready to go. The next section explains how the readers and writers are supposed to be used
Note: This guide is not final, as it is, in its current version, not fully compatible with remote deployment!
Make sure to import this folder in your matlab file. An example can be seen below:
% Initialize data readers/writers...
common_cpm_functions_path = fullfile( ...
script_directoy, '../../../../cpm_lib/matlab_MEX_bindings/' ...
);
assert(isfolder(common_cpm_functions_path), 'Missing folder "%s".', common_cpm_functions_path);
addpath(common_cpm_functions_path); |
All provided readers and writers allow you to pass an optional parameter, dds_domain, with which you can specifiy a DDS Domain to use to communicate with the Middleware via shared memory. The default is 1. The following guide shows you at which position this parameter can be set for each reader / writer.
The DDS Domain can only be set once. The parameter is ignored after the first time you called a reader or writer, and if you did not specify an ID in the first call, the default is chosen. You do not want to permanently recreate reader or writer performance-wise, and there should be no need to change the ID once it has been set. Just keep in mind to set it correctly the first time you call each of the provided eProsima functions. It needs to be set for each reader / writer individually. |
Both readers should not be used directly, unless you want them to only receive data while their MEX-function is called and before it returns. You very likely don't want that to happen and want the readers to receive data while your code is doing other things as well.
For this purpose, the wrappers systemTriggerReader and vehicleStateListReader have been created. They let the readers run in a new thread.
These messages inform the HLC if it should start (check after initialization) or stop (check in every timestep) its computation. You do not need to interpret the value of the signal - the Middleware takes care of these timing-issues for you. The only thing you need to consider is the value of the stop symbol, which you can define as:
stop_symbol = uint64((0xffffffffffffffffu64));
The MEX-file returns the received system trigger in form of a struct. It contains:
If no message was received, is_valid is false.
The file also takes two optional input parameters:
If you pass true, this means that the reader will wait for up to max_unsigned_int milliseconds before it returns. The parameter dds_domain sets the DDS Domain as described before.
Calling the reader thus may look like this:
system_trigger = systemTriggerReader(uint32(1), true);
Here is a full example:
disp('Waiting for start or stop signal');
stop_symbol = uint64((0xffffffffffffffffu64));
got_stop = false;
got_start = false;
while (~got_stop && ~got_start)
dds_domain = uint32(1);
system_trigger = systemTriggerReader(dds_domain, true);
if system_trigger.is_valid
if system_trigger.next_start == stop_symbol
got_stop = true;
else
got_start = true;
end
end
end
disp('Done'); |
The basic idea is similar to systemTrigger.
Input:
sample = vehicleStateListReader(uint32(1), uint32(5000));
The MEX-file returns the received VehicleStateList in form of a struct. It contains:
Again, the received data is only valid if is_valid is true, else no message was actually received and the rest of the struct should either be empty or default to zero.
Using the writers is just as simple. To write a message, you first need to create a class object of the message you want to write, ReadyStatus or VehicleCommandTrajectory, then fill it with the information you want to send and finally write it with the correct writer.
You just need to pass the correct object (type) to the MEX-writer to send it.
After the message object, you can again specify (optionally):
A full example of creating the ReadyStatus message object, filling it with data and sending it is shown below:
ready_msg = ReadyStatus;
ready_msg.source_id = strcat('hlc_', num2str(vehicle_id));
ready_msg.next_start_stamp = uint64(0);
dds_domain = uint32(1);
ready_status_writer(ready_msg, dds_domain); |
As before, here is a full example with setting up and sending the message.
% Some code was left out
% Set up the trajectory points
trajectory_points = [];
the_trajectory_point = TrajectoryPoint;
the_trajectory_point.t = ...;
the_trajectory_point.px = ...;
the_trajectory_point.py = ...;
the_trajectory_point.vx = ...;
the_trajectory_point.vy = ...;
trajectory_points = [trajectory_points the_trajectory_point];
% Set up the vehicle command trajectory message
max_delay_time_nanos = 200e6;
vehicle_command_trajectory = VehicleCommandTrajectory;
vehicle_command_trajectory.vehicle_id = uint8(vehicle_id);
vehicle_command_trajectory.trajectory_points = trajectory_points;
vehicle_command_trajectory.create_stamp = ...
uint64(sample.t_now);
vehicle_command_trajectory.valid_after_stamp = ...
uint64(sample.t_now + max_delay_time_nanos);
% Send the message using the MEX DDS writer
dds_domain = uint32(1);
vehicle_command_trajectory_writer(vehicle_command_trajectory, dds_domain); |
Analogous to VehicleCommandTrajectory, but to send VehicleCommandSpeedCurvature messages. Instead of trajectory_points, you can set speed and curvature as scalar doubles here.
Analogous to VehicleCommandTrajectory, but to send VehicleCommandDirect messages. Instead of trajectory_points, you can set motor_throttle and steering_servo as scalar doubles here.
Again analogous to VehicleCommandTrajectory. Another example for its usage is given below.
% Some code was left out
% Set up the path tracking part
path_points = [];
for i = 1:numel(s)
path_point = PathPoint;
path_point.x = x(i);
path_point.y = y(i);
path_point.yaw = yaw(i);
path_point.s = s(i);
path_points = [path_points [path_point]];
end
% Some code was left out
vehicle_command_path_tracking = VehicleCommandPathTracking;
vehicle_command_path_tracking.vehicle_id = uint8(vehicle_id);
vehicle_command_path_tracking.path = path_points;
vehicle_command_path_tracking.speed = 1.0;
vehicle_command_path_tracking.create_stamp = ...
uint64(sample(end).t_now);
vehicle_command_path_tracking.valid_after_stamp = ...
uint64(sample(end).t_now + dt_valid_after);
vehicle_command_path_tracking_writer(vehicle_command_path_tracking, matlabDomainId);
|
clear vehicle_command_trajectory_writer clear ready_status_writer clear systemTriggerReader.m clear vehicleStateListReader.m |
If you do not do this, old readers and writers will stay in memory. They may e.g. have old stop messages buffered and will thus let your program end immediately if you start it the next time.