
% 
% Demo - Read 2D pose from TurtleBot3
%
% 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 
end 


% reset TurtleBot's 2D pose (x,y,theta) -> (0,0,0) 
tbot.resetPose();

% start by moving the robot in a circular motion 
tbot.setVelocity(0.1, 0.25)


%---------------------------------------------------------
% Execute loop at fixed frequency
%---------------------------------------------------------

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

% run for 10 seconds: 50 iterations (50 x 1/5 = 10s) 
for it = 1:1:50  

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


    % display
    figure(1); clf; hold on;            % clear figure, hold plots
    drawTurtleBot(x, y, theta);         % draw Robot    
    % draw x-y axis
    quiver(0,0,1,0,'r')                 % draw arrow for x-axis 
    quiver(0,0,0,1,'g')                 % draw arrow for y-axis 
    axis([-3, 3, -3, 3])                % the limits for the current axes [xmin xmax ymin ymax]
    grid on;                            % enable grid 
    xlabel('x')                         % axis labels 
    ylabel('y')
    title('demo Pose')                  % add title 
  
    % adaptive pause
    waitfor(r);
end

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

% stop robot 
tbot.stop();
 