%Create msg of type trajectory
trajectory = VehicleCommandTrajectory;
% Set the vehicle ID
trajectory.vehicle_id = uint8(vehicle_id);
% In this example, we only send one trajectory point (which is not sufficient for interpolation)
trajectory_points = [];
point1 = TrajectoryPoint;
% This trajectory point should be considered in the future, so add some nanoseconds to the current time, then set the time stamp for the trajectory point
% t_eval here is the time that we got from the middleware in the VehicleStateList message - you are not supposed to use your own clock / timer (else you would have trouble working with simulated time)
time = t_eval + 400000000;
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); |