Need help updating function while using getkey

Hi everyone,

I am writing a leader-follower turtlebot3 burger script and I am controlling the leader bot with an xbox controller and the follower bot follows the leader bot using the lidar scan provided on the follower bot as well as the velocities that the leader is set to.

Issues have arrived because the way I am using the xbox controller to move the bot:

I am using a program called Gopher360 which basically maps the bluetooth inputs from the controller to the pc to ASCII, and using the ASCII integers as parameters for the getkey.m function I am using the function getkey.m which I will post below my Turtlebot's function code.

The issue I'm having is the fact that I need to constantly keep inputting leader commands for the follower to adjust angles to turn, slow down, speed up based on distance, etc.

The reason for this is after each iteration, I need to call the getkey function again to determine whether or not to turn, stop, slow down, etc. for the leader. This method does get the job done, however, I'm not happy with it. I am wondering if you guys notice any better way to write this program that will avoid the issue of waiting for a getkey to edit follower actions, while still having the benefit of being able to control the leader without interruption.

All insight is, of course, appreciated. I am not an expert - please bare with me for any bad programming practices - simply a student working on their senior design!

This is my function for operating the leader-follower system:

%Turtlebots
%Turtlebot Leader and Follower System

% function [] = Turtlebots()
%

%Initialize subscriber and publishers
laser_sub = rossubscriber(TurtleBot_Topic.laser);
leader_velocity_pub = rospublisher('/cmd_vel', 'geometry_msgs/Twist');
follower_velocity_pub = rospublisher('/cmd_vel2', 'geometry_msgs/Twist');

%Initialize rosmessages
velocity_msg_Leader = rosmessage(leader_velocity_pub);
velocity_msg_Follower = rosmessage(follower_velocity_pub);

%Not using these variables anymore, still keeping them here for future modifications.
linearFlag = 0;
turnFlag = 0;
flagLeft = 0;
flagRight = 0;

%Start the clock
tic
while toc<30
    key = getkey; %Wait for controller/ascii input
    if (key == 30)%D-Pad Up
        velocity_msg_Leader.Angular.Z = 0.0;
        velocity_msg_Leader.Linear.X = .15;
        send(leader_velocity_pub, velocity_msg_Leader);
        mostRecentVel = velocity_msg_Leader.Linear.X;
        linearFlag = 1;
        %Send message to follower to copy this info.
    elseif (key == 28)%D-Pad Left
        %Send message that robot is turning to follower
        %Follower must recieve message and calculate how far to travel before
        %turning.
        velocity_msg_Leader.Angular.Z = 1.5708;
        velocity_msg_Leader.Linear.X = 0.0;
        send(leader_velocity_pub, velocity_msg_Leader);
        velocity_msg_Follower.Linear.X = mostRecentVel * 0.25; % Slow Down for Turn to Complete
        send(follower_velocity_pub, velocity_msg_Follower);
        pause(1);
        flagLeft = 1;
        turnFlag = 1;
         linearFlag = 0;
    elseif (key == 29)%D-Pad Right
        %Send message that robot is turning to follower
        velocity_msg_Leader.Angular.Z = -1.5708;
        velocity_msg_Leader.Linear.X = 0.0;
        send(leader_velocity_pub, velocity_msg_Leader);
        velocity_msg_Follower.Linear.X = mostRecentVel * 0.25; % Slow DOwn for Turn to Complete
        send(follower_velocity_pub, velocity_msg_Follower);
        pause(1);
        flagRight = 1;
        turnFlag = 1;
        linearFlag = 0;
    elseif (key == 32)%Left Trigger
        velocity_msg_Leader.Angular.Z = 0.0;
        velocity_msg_Leader.Linear.X = 0.0;
        send(leader_velocity_pub, velocity_msg_Leader);
        %Send message to follower to copy this info.
    end
    velocity_msg_Leader.Angular.Z = 0.0;        %Set the angular velocity to 0 to ensure turns are only 90 degrees.
    send(leader_velocity_pub, velocity_msg_Leader);    %Send Velocity
    
    %Follower Code
    minDistThreshold = 0.1524;  % Distance threshold (m) for following not closer than 0.5ft
    maxDistThreshold = 0.4572; % Distance threshold (m) for following not further than 1.5ft
    
    %Easier Variable Names for velocity information from leader bot
    LeaderX = velocity_msg_Leader.Linear.X;
    LeaderZ = velocity_msg_Leader.Angular.Z;
    
    %Lidar Scan
    scan_data = receive(laser_sub);
    plot(scan_data);
    [data, angles] = readCartesian(scan_data);
    
    x = data(:,1);
    y = data(:,2);
    
    %Compute the distances of all objects detected
    dist = sqrt(x.^2 + y.^2);
    %Find which object is closest to follower (Will be leader bot 99% of the time)
    %Store the index of that object to acquire angle needed to align with leader
    [minDist,index] = min(dist);
    
    if (minDist < maxDistThreshold && minDist > minDistThreshold * 2)
        if ((angles(index) > -pi/2) && (angles(index) < pi/2)) 
                
                angleToLeader = angles(index);
                
                velocity_msg_Follower.Angular.Z = 1.5 * angleToLeader;     %Set angular Velocity of follower to 90 degrees per 2 seconds
                velocity_msg_Follower.Linear.X = LeaderX + .05;
                send(follower_velocity_pub, velocity_msg_Follower)   %Send Velocity
                pause(1)                                             %Pause for 2 seconds to complete 90 degree turn

        end
        
    elseif (minDist < 1.5 * minDistThreshold)
        
        angleToLeader = angles(index);
        velocity_msg_Follower.Angular.Z = 1.5 * angleToLeader;    %Set Angular Vel
        velocity_msg_Follower.Linear.X = LeaderX * .75;           %Set linear velocity to leader's current velocity - ratio to distance
        send(follower_velocity_pub, velocity_msg_Follower)        %Send Velocity
        pause(1);
    elseif (minDist < minDistThreshold)
        
        angleToLeader = angles(index);
        velocity_msg_Follower.Angular.Z = 1.5* angleToLeader;  %Set Angular Vel
        velocity_msg_Follower.Linear.X = LeaderX * .25;        %Set linear velocity to leader's current velocity - ratio to distance
        send(follower_velocity_pub, velocity_msg_Follower)     %Send Velocity
        pause(1);
    else
        angleToLeader = angles(index);
        velocity_msg_Follower.Angular.Z = 1.5 * angleToLeader; %Set Angular Vel
        velocity_msg_Follower.Linear.X = LeaderX;              %Set linear velocity to leader's current velocity
        send(follower_velocity_pub, velocity_msg_Follower)     %Send Velocity
        pause(1);
    end
    
        velocity_msg_Follower.Angular.Z = -angleToLeader;      % Adjust angle slightly for corrections on any possible overshoot
        velocity_msg_Follower.Linear.X = LeaderX;              %Set linear velocity to leader's current velocity
        send(follower_velocity_pub, velocity_msg_Follower)     %Send Velocity
        pause(.025)
        
        velocity_msg_Follower.Angular.Z = 0;
        velocity_msg_Follower.Linear.X = LeaderX;                   %Set linear velocity to leader's current velocity
        send(follower_velocity_pub, velocity_msg_Follower)     %Send Velocity
    
end
%Let TurtleBot stop before disconnect from it
velocity_msg_Follower.Angular.Z = 0.0;
velocity_msg_Follower.Linear.X = 0.0;
send(follower_velocity_pub, velocity_msg_Follower);

velocity_msg_Leader.Angular.Z = 0.0;
velocity_msg_Leader.Linear.X = 0.0;
send(leader_velocity_pub, velocity_msg_Leader);


% end

This is the getkey function I found online:

function [ch, tim] = getkey(N, nonascii)
% GETKEY - get a keypress
%   CH = GETKEY waits for a single keypress and returns the ASCII code. It
%   accepts all ascii characters, including backspace (8), space (32),
%   enter (13), etc, that can be typed on the keyboard.
%   Non-ascii keys (ctrl, alt, ..) return a NaN. CH is a double.
%
%   CH = GETKEY(N) waits for N keypresses and returns their ASCII codes.
%   GETKEY(1) is the same as GETKEY without arguments.
%
%   GETKEY('non-ascii') or GETKEY(N, 'non-ascii') uses non-documented
%   matlab features to return a string describing the key pressed.
%   In this way, keys like ctrl, alt, tab etc. can also distinguished.
%   The return is a string (when N = 1) or a cell array of strings.
%
%   [CH, T] = GETKEY(...) also returns the time between the start of the
%   function and each keypress. This is, however, not very accurate.
%
%   This function is kind of a workaround for "getch" in C. It uses a modal,
%   but non-visible window, which does show up in the taskbar.
%   C-language keywords: KBHIT, KEYPRESS, GETKEY, GETCH
%  
%   Example 1 - get a single ascii key
%      fprintf('\nPress any key: ') ;
%      ch = getkey ;
%      fprintf('%c\n',ch) ;
%
%   Example 2 - wait for a specific key
%      fprintf('\nPress the Ctrl-key within 3 presses: ') ;
%      ch = getkey(3,'non-ascii')
%      if ismember('control', ch), fprintf('OK\n') ;
%      else fprintf(' ... wrong keys ...\n') ; end
%    
%   Example 3 - Typing game
%       S = 'abcdefghjiklm' ; 
%       fprintf('Type "%s" as fast as possible ...\n', S) ;
%       [C, T] = getkey(numel(S)) ;
%       C = char(C) ; T = T(end)-T(1) ;
%       if ~isequal(S, C), fprintf('OOPS!!! ') ; end
%       fprintf('You typed "%s" in %.2f seconds.\n', C, T) ;
%
%  See also INPUT, UIWAIT
%           GETKEYWAIT (File Exchange)
% for Matlab 6.5 and higher
% version 2.1 (feb 2019)
% author : Jos van der Geest
% email  : [e-mail]
%
% History
% 1.0 2005 - creation
% 1.1 dec 2006 - modified lay-out and help
% 1.2 apr 2009 - tested for more recent MatLab releases
% 1.3 jan 2012 - modified a few properties, included check is figure still
%            exists (after comment on FEX by Andrew).
% 2.0 jun 2012 - added functionality to accept multiple key presses
% 2.1 feb 2019 - mondernised, added timing example
t00 = tic ; % start time of this function
% check the input arguments
narginchk(0,2)
switch nargin
    case 0
        nonascii = '' ;
        N = 1 ;
    case 1
        if ischar(N)
            nonascii = N ;
            N = 1 ;
        else
            nonascii = '' ;
        end
end
if numel(N) ~= 1 || ~isnumeric(N) || N < 1 || fix(N) ~= N
    error('N should be a positive integer scalar.') ;
end
% Determine the callback string to use
if strcmpi(nonascii,'non-ascii')
    % non-ascii characters are accepted
    nonascii = true ;
    callstr = 'set(gcbf, ''Userdata'', get(gcbf, ''Currentkey'')) ; uiresume ' ;
elseif isempty(nonascii)
    nonascii = false ;
    % only standard ascii characters are accepted
    callstr = 'set(gcbf, ''Userdata'', double(get(gcbf, ''Currentcharacter''))) ; uiresume ' ;
else
    error('String argument should be the string ''non-ascii''') ;
end
% Set up the figure
% May be the position property  should be individually tweaked to avoid visibility
fh = figure(...
    'name', 'Press a key', ...
    'keypressfcn', callstr, ...
    'windowstyle', 'modal', ...
    'numbertitle', 'off', ...
    'position', [0 0 1 1], ... % really small in the corner
    'userdata', 'timeout') ;
try
    ch = cell(1,N) ;
    tim = zeros(1,N) ;
    
    % loop to get N keypresses
    for k=1:N
        % Wait for something to happen, usually a key press so uiresume is
        % executed
        uiwait ;
        tim(k) = toc(t00) ; % get the time of the key press
        ch{k} = get(fh,'Userdata') ;  % and the key itself
        if isempty(ch{k})
            if nonascii
                ch{k} = NaN ;
            else
                ch{k} = '' ;
            end
        end
    end
    if ~nonascii
        ch = [ch{:}] ;
    else
        if N==1
            ch = ch{1} ; % return as a string
        end
        % return as a cell array of strings
    end
 catch
    % Something went wrong, return empty matrices.
    ch = [] ;
    tim = [] ;
end
% clean up the figure, if it still exists
if ishandle(fh)
    delete(fh) ;
end

[Walter Roberson]

https://www.mathworks.com/matlabcentral/fileexchange/8297-getkeywait

[Previn Savaya]

I think this might be exactly what I'm looking for! I will try it out when I get back to the lab on Friday and get back with you!

Happy Thanksgiving!

[Previn Savaya]

That worked!

However, now that I have a window to control the leader, I need to make the rest of the code more efficient with less pauses so I reach that window more often to prevent delays.

If I miss the .5 second or 1 second window to press a button, I need to wait for the entire loop to cycle through again. This is no problem. I can work on making this more efficient myself!

Thanks for the tip!