
% 
% Demo - Read from TurtleBot3 LIDAR
%
% Launch the virtual machine.
% Open terminal, and start the empty Gazebo world: ./start-gazebo-empty.sh
%

% clear memory 
clearvars -except tbot

% Init TurtleBot3 connection (tbot object), only if required
if ( ~exist("tbot") )
    IP_TURTLEBOT = "192.168.10.110";         % virtual machine IP (or robot IP)
    IP_HOST_COMPUTER = "192.168.10.100";     % local machine IP
    tbot = TurtleBot3(IP_TURTLEBOT, IP_HOST_COMPUTER);   % init TurtleBot3
    % check version
    if( tbot.getVersion() < 11.1 ) error('TurtleBot v11.1 required'); end 

    % -----------------------
    %  Add 3D Objects
    % -----------------------
    % Delete all suported 3D objects  
    tbot.gazeboDeleteAllModels();

    % Place 3D Objects (2 cubes)
    % gazeboPlace3DCube(x, y, orientation, edgeSize, color);
    tbot.gazeboPlace3DCube(2, 1.5, 0, 0.5, 'r');        % red cube at (2, 1.5)
    tbot.gazeboPlace3DCube(3, -1, 0, 0.5, 'w');         % white cube at (3, -1)

    % Notes: 
    % -> All 3D object are placed JUST ONCE after the initial connection. 
    % -> There is no need to keep deleting, and inserting, the same objects every time we run the script.
end 


% set Turtlebot's 2D pose (x,y,theta) -> (0.25,0.5,0)
tbot.setPose(0.25, 0.5, 0);

% start by moving the robot forward
tbot.setVelocity(0.1, 0)


% -------------------------------------
%  read LIDAR data w/ fixed timming
% -------------------------------------

% init ratecontrol obj (enables to run a loop at a fixed frequency) 
r = rateControl(5);     % run at 5Hz

% run for 15 seconds: 75 iterations (75 x 1/5 = 15s) 
for it = 1:1:75

    % read TurtleBot pose (and timestamp) 
    %[x, y, theta, timestamp] = tbot.readPose();

    % load LIDAR data from robot 
    [lddata, ldtimestamp, scanMsg] = tbot.readLIDAR();

    % Get in range LIDAR data indexs (valid data - obstacles).
    [vidx] = tbot.getInRangeLIDARIdx(lddata);

    % load valid X/Y cartesian LIDAR readings          
    xydata = lddata.Cartesian(vidx,:);


    % Get out of range lidar data indexs (too near or too far readings).
    % [nidx] = tbot.getOutRangeLIDARIdx(lddata);
    % load obstacle free orientations (angles)  
    % freeAng = lddata.Angles(nidx);


    % display data (using regular plots w/ cartesian data)
    figure(1); clf;
    plot(xydata(:,1), xydata(:,2), 'b.', 'MarkerSize', 8)   % draw laser readings
    axis([-4, 4, -4, 4])                % the limits for the current axes [xmin xmax ymin ymax]
    grid on;                            % enable grid 
    xlabel('x')                         % axis labels 
    ylabel('y')
    title('demo LIDAR')

    % display LIDAR data (using ROS toolbox)
    %figure(2); clf;
    %rosPlot( scanMsg ) 

    % adaptive pause
    waitfor(r);
end

% check statistics of past execution periods
stats = statistics(r);

% stop robot 
tbot.stop()




