Skip to end of metadata
Go to start of metadata

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.

How to Execute the MEX Files

Matlab will not be able to execute your MEX-Files without preparation!

  • You need to preload all required library files before you start Matlab. You may notice that libstdc++ is included here as well. I will explain that in step 3. DO NOT set this variable globally unless you want some of your programs to crash on start. (Adapt the folder names depending on your system setup):
 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" 
  • You also again need to include the files in your Matlab file (adapt the folder names depending on your system setup):
setenv("LD_LIBRARY_PATH", [getenv('LD_LIBRARY_PATH'), ':/home/[username]/dev/software/cpm_lib/build/', ':/usr/local/lib/']);

Some Notes

  • 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

How to use the provided readers and writers in the MEX Files

Note: This guide is not final, as it is, in its current version, not fully compatible with remote deployment!

Importing the readers and writers

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);

Setting a DDS Domain ID

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.

Using the Readers

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.

Receiving systemTrigger messages

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:

  • next_start as a uint64
  • is_valid as a boolean

If no message was received, is_valid is false.

The file also takes two optional input parameters:

  • dds_domain as uint32
  • wait_infinitely as a boolean

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');

Receiving vehicleStateList messages

The basic idea is similar to systemTrigger.

Input:

  • Optional dds_domain as uint32
  • Optional uint32, specifying the max. time to wait for a message in milliseconds
sample = vehicleStateListReader(uint32(1), uint32(5000));

The MEX-file returns the received VehicleStateList in form of a struct. It contains:

  • t_now as a uint64
  • period_ms as a uint64
  • state_list as a struct array. Each entry contains:
    • vehicle_id as a uint8
    • create_stamp as uint64
    • valid_after_stamp as uint64
    • pose_x as a double
    • pose_y as a double
    • pose_yaw as a double
    • IPS_update_age_nanoseconds as a uint64
    • odometer_distance as a double
    • imu_acceleration_forward as a double
    • imu_acceleration_left as a double
    • imu_acceleration_up as a double
    • imu_yaw as a double
    • imu_yaw_rate as a double
    • speed as a double
    • battery_voltage as a double
    • motor_current as a double
    • motor_throttle as a double
    • steering_servo as a double
    • is_real as a boolean
  • vehicle_observation_list as a struct array. Each entry contains:
    • vehicle_id as a uint8
    • create_stamp as uint64
    • valid_after_stamp as uint64
    • pose_x as a double
    • pose_y as a double
    • pose_yaw as a double
  • active_vehicle_ids as a list of int32
  • is_valid as a boolean

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

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):

  • dds_domain as uint32

ReadyStatus Writer

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);

VehicleCommandTrajectory Writer

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);

VehicleCommandSpeedCurvature Writer

Analogous to VehicleCommandTrajectory, but to send VehicleCommandSpeedCurvature messages. Instead of trajectory_points, you can set speed and curvature as scalar doubles here.

VehicleCommandDirect Writer

Analogous to VehicleCommandTrajectory, but to send VehicleCommandDirect messages. Instead of trajectory_points, you can set motor_throttle and steering_servo as scalar doubles here.

VehicleCommandPathTracking Writer

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);

Always make sure to clear your objects after you have used them

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.

  • No labels