Page History

This example is a guideline on how to write a Matlab script that, together with the Middleware, communicates with a vehicle. This tutorial is based on our platoon example (which is just a series of vehicles following a preset path without much logic behind that behavior), which you can find in

https://gitgithub.rwth-aachen.de/CPM/Project/Lab/software/-com/embedded-software-laboratory/cpm_lab/tree/master/high_level_controller/examples/matlab/platoon

More specifically, we well take a look at main_vehicle_ids.m.

...

...

then you need to pass some, params as additional parameters in the LCC's UI (ignore the script path in the screenshot):

What your HLC scripts need to include

Import the init script

Set the Matlab domain ID to 1, go to the right directory for the init script, load the script, store the vehicle IDs. In this the case , for the reader that reads all vehicle states sent within the network , aggregated by the Middleware, a waitset is set to wait upon taking data from the reader if no data is yet available. The timeout is set to 10 seconds.

function main_vehicle_ids(varargin)
    matlabDomainID = 1;

    clc
    script_directoy = fileparts([mfilename('fullpath') '.m']);
    cd(script_directoy)

    % Initialize data readers/writers...
    init_script_path = fullfile('../', '/init_script.m');
    assert(isfile(init_script_path), 'Missing file "%s".', init_script_path);
    addpath(fileparts(init_script_path));
    [matlabParticipant, stateReader, trajectoryWriter, systemTriggerReader, readyStatusWriter, trigger_stop] = init_script(matlabDomainID);
    cd(script_directoy)

    vehicle_ids = varargin;

    %% wait for data if read() is used
    stateReader.WaitSet = true;
    stateReader.WaitSetTimeout = 10;

    %% Do not display figures
    set(0,'DefaultFigureVisible','off');

Tell the Middleware that your script is ready to operate

You must send a ReadySignal message after initialization - only the ID string matters, which must be of the form "hlc_" + vehicle_id, where the latter is the ID of the vehicle the HLC and thus your script is responsible for:

...

You use the ReadyStatus type here, which was defined in one of the IDL files that was imported for you using the init script. You can see the values of this type in the IDL file:

https://gitgithub.rwth-aachen.de/CPM/Project/Lab/software/-com/embedded-software-laboratory/cpm_lab/blob/master/cpm_lib/dds_idl/ReadyStatus.idl

This message tells the Middleware that your script is now ready to operate and to receive its data. Thus, at this point, your reader for vehicle states (here stateReader) should be initialized so that it can receive the data.

Consider the timing signals

This is very important. There are two signals you must consider: Start and stop signals. You have to check regularly for the stop signal, and once for the start signal:

...

if got_stop == false
    while(true)
    disp('Checking system trigger for stop signal');
    trigger = SystemTrigger;
    sampleCount = 0;
    [trigger, status, sampleCount, sampleInfo] = systemTriggerReader.take(trigger);
    break_while = false;
    while sampleCount > 0
        current_time = trigger.next_start().nanoseconds();
        if current_time == trigger_stop
            break_while = true;
        end
        [trigger, status, sampleCount, sampleInfo] = systemTriggerReader.take(trigger);
    end

    if break_while
        disp("Stopping bc of stop signal");
        break;
    end

    disp('Waiting for data');
    ...

Receive information about your vehicle

Information about your vehicle are contained in the VehicleStateList messages, which include the current states and observations of all vehicle as well as the current time. This signal is supposed to be the start signal for the HLC, so computation should start using this data directly after the message was received. 

...

...
disp('Waiting for data');
sample = VehicleStateList;
status = 0;
stateSampleCount = 0;
sampleInfo = DDS.SampleInfo;
[sample, status, stateSampleCount, sampleInfo] = stateReader.take(sample); %Due to the set waitset, you wait for up to 10 seconds at this point, unless old data is available

% In this example, messages that were received during computation that should be ignored are not thrown away before waiting for new data
% If your script is faster than the period with which it is called, this is not problematic, in any other case you will probably end up using wrong timing and outdated vehicle data
% Check if any new message was received
if stateSampleCount > 0 
    disp('Current time:');
    disp(sample.t_now);

    ...

Send commands to your vehicle

You need to send vehicle command messages as a result of your computation including the vehicle ID to the Middleware, which propagates these to the vehicle. The implementation of the computation of e.g. the vehicle's trajectory is not explained here and depends on your task. You are only given an example of how to send a simple trajectory here.

...