Skip to end of metadata
Go to start of metadata

This example is a guideline on how to write a Matlab script that, together with the Middleware, communicates with a vehicle. This tutorial ist based on our "basic_circle example" which you can find in

https://github.com/embedded-software-laboratory/cpm_lab/tree/master/high_level_controller/examples/matlab/basic_circle

Use our init script, called init_script.m, which is located in the software repository under software/hlc/matlab. Please consult RTI DDS and Matlab to get an idea of how to work with DDS in Matlab. The Middleware-specific IDL file is only relevant for the communication between Middleware and Matlab script and is thus not part of the cpm library.

Your Matlab script is not supposed to directly communicate with the vehicle or to perform timing operations with the LCC. This is the Middleware's task, so your task is just to properly communicate with the Middleware (if you only want to control the vehicle).

Setup

First steps - what the HLC scripts need to include

  • 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 is responsible for
  • Receive VehicleStateList messages, which include the current states and observations of the 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 after the message was received. The history for this signal is set to 1, but you may still get an outdated signal here if you missed a period during your computation and read the next VehicleStateList in the middle of that next period. In that case, it may be better to skip that period as well and wait for the following one to start.
  • Send vehicle command messages as a result of the communication including the vehicle ID to the Middleware, which propagates these to the vehicle
  • React to stop signals sent by the LCC and propagated to the HLC by the Middleware

The structure of your code

Your function head may differ, but you must use varargin as your last parameter to pass vehicle IDs to your script. This allows you to define, for your script, which vehicle(s) it should be responsible for. Any previous parameters you define are your own 'custom' parameters and need to be specified as additional parameters in the Lab Control Centers' UI before starting your script (if you wish to start it using the UI).

function main(varargin)
...
vehicle_ids = varargin;

The init_file covers the important steps:  It loads the IDL files from the cpm library and the middleware.  It creates its own .m files from the IDL files. They are put in a subdirectory so that you can maintain a clean folder structure. Furthermore, you need to set some quality of service files so that the DDS communication with the Middleware works as desired. They are loaded via XML files. If you want to look up the settings in NDDS_QOS_PROFILES, your script will search for your QOS settings in the given files. Then the script sets some constants. In DDS, you need to define topics for your communication, so you might store the topic names in a variable. Finally the script adds the DDS Participants, Reader and Writer: The systemTriggerReader and the readyStatusWriter also use their own QOS settings, this time from QOS_READY_TRIGGER. These latter settings make sure that the timing communication is reliable. The Middleware uses these settings as well - if you do not use it, the reader/writer will not match.

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:

% Assuming that vehicle_ids contains the IDs your script is responsible for
for i = 1 : length(vehicle_ids)
    ready_msg = ReadyStatus;
    ready_msg.source_id = strcat('hlc_', num2str(vehicle_ids{i}));
    ready_stamp = TimeStamp;
    ready_stamp.nanoseconds = uint64(0);
    ready_msg.next_start_stamp = ready_stamp;
    readyStatusWriter.write(ready_msg);
end

You use the ReadyStatus type here, which was defined in one of the IDL files you imported. You can see the values of this type in the IDL file.

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

These signals, which, in this example, you can obtain using the stateReader, contain two fields which are vital to the correct usage of your script.

  1. The messages contain current information about the whereabouts and settings of your vehicle as well as any other in the simulation. Use this data for planning.
  2. The messages contain timing information. Use these when you send commands to your vehicle - they contain the current time you set for the messages.

This signal has a third purpose as well. It shows the script when to start the computation, similar to a timing signal. The desired computation cycle is the following:

  • Wait for a new message
  • After receiving a message, start your computation
  • Then send the new command(s) to the vehicle
  • Remove messages that were received during computation, as they are old and potentially unusable, and wait for the next message
  • ...

The history for this signal is set to 1, but you may still get an outdated signal here if you missed a period during your computation and read the next VehicleStateList in the middle of that next period. In that case, it may be better to skip that period as well and wait for the following one to start.

sample = VehicleStateList;
status = 0;
stateSampleCount = 0;
sampleInfo = DDS.SampleInfo;
[sample, status, stateSampleCount, sampleInfo] = stateReader.take(sample);

% Check if any new message was received (as said before, you might have to skip another period)
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.

%Create msg
trajectory = VehicleCommandTrajectory;
% Set the vehicle ID
trajectory.vehicle_id = uint8(vehicle_id);
trajectory_points = [];
point1 = TrajectoryPoint;

% This trajectory point should be considered in the future, so add some nanoseconds to the current time
time = t_eval + 400000000;
stamp = TimeStamp;
stamp.nanoseconds = uint64(time);
point1.t = stamp;

% Set other trajectory values
point1.px = trajectory_point(1);
point1.py = trajectory_point(2);
point1.vx = trajectory_point(3);
point1.vy = trajectory_point(4);

trajectory_points = [trajectory_points [point1]];
trajectory.trajectory_points = trajectory_points;

% Send trajectory to vehicle
trajectoryWriter.write(trajectory);

Consider the timing signals

This is very important. The LCC might send a stop signal (trigger_stop = uint64(18446744073709551615);) (this is the highest number representable with a uint64_t type - all other numbers are considered to be real timing signals for simulated time, and should be ignored by your script, because they are already handled by the Middleware). This signal gets propagated to your script by the Middleware. You should regularly check if you received such a stop signal, so that your script does not run when the simulation was stopped.

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

if got_stop
disp("Stopping bc of stop signal");
% End your script here
end

The symbol trigger_stop is the highest value of uint64_t and should be defined as a constant at the beginning of the script. You can find it in various Matlab and C++ scripts.

Parameter Usage Example

./middleware --vehicle_ids=1,7,8 --node_id=middleware --wait_for_start=true --dds_domain=1 --simulated_time=false

Bash script

If you want to start your script using a bash script, you can do it like this:

/opt/MATLAB/R2019a/bin/matlab -logfile matlab.log -sd $script_dir -batch "$script_name(1, ${vehicle_id})"


  • No labels