Page History

	% Set the matlab 

domain 

ID 

for communicating with the 

middleware (which is 

)
% Send first ready signal 

to 

the 

Middleware to 

indicate 

that 

your 

program 

is ready to operate
% The signal needs to be sent for all assigned vehicle ids separately (usually, one script on one NUC only manages one of the IDs)
% Also works for simulated time 

- period etc are set in Middleware, so 

you can ignore the timestamp field
for i = 1 : length(vehicle_ids)

	% Create a new msg of type ReadyStatus (idl)
    ready_msg = ReadyStatus;

	% Set the values of ready_msg
    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;

	% Send the ready msg
    readyStatusWriter.write(ready_msg);

end

 / stop if a stop signal

 was received
got_stop = false;

got_start = false;

while(true)

	% Read the newest system trigger 

message, if one exists
    trigger = SystemTrigger;
    sampleCount = 0;

    [trigger, status, sampleCount, sampleInfo] = systemTriggerReader.take(trigger);

	% Go through all 

received 

messages 

since 

the last check, if they exist (ssampleCount gives 

the number of read messages)
	% Any received message would indicate "start", but you have to check if a stop signal was received as well
    while sampleCount > 0
		% Check for stop signal
        if 

trigger.next_start().nanoseconds() ==

 trigger_stop
         

   got_stop = true;
   

elseif trigger.next_start().nanoseconds() >= 0
      

      got_

start = true;
      

 end

		% Read the next sample and continue until sampleCount is zero again (all samples have been read then)
     

  [trigger, status, sampleCount, sampleInfo] = systemTriggerReader.take(trigger);
    end

	% Stop the loop if a signal was received, then proceed with handling a stop signal (stop the program) or a start signal (continue)
    if got_stop | got_start
        break;
    end
end

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
    ...

% Read the current vehicle information from the according reader, if such data is available
% Due to the set waitset, you wait for up to 10 seconds at take(...) if no data is available
sample = VehicleStateList;
status = 0;
stateSampleCount = 0;
sampleInfo = DDS.SampleInfo;
[sample, status, stateSampleCount, sampleInfo] = stateReader.take(sample);

% 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
% Thus, you should get rid of old data at the end of any iteration ('clear' stateReader)

% Check if any new message was received, then proceed with handling the data, computing your solution and finally sending back a command to the vehicle(s) your script controls
if stateSampleCount > 0 
    ...

stamp = TimeStamp;

stamp.nanoseconds = uint64(time);

point1.t = stamp;

% Set other trajectory values

 - position and velocity in 2D coordinates
point1.px = trajectory_point(1);

point1.py = trajectory_point(2);

point1.vx = trajectory_point(3);

point1.vy = trajectory_point(4);

% Append to the list of all trajectory points
trajectory_points = [trajectory_points [point1]];

trajectory.trajectory_points = trajectory_points;

% Send the trajectory to the vehicle

trajectoryWriter.write(trajectory);