www.gusucode.com > ros 工具箱 matlab源码程序 > ros/rostime.m

    function varargout = rostime(varargin)
%ROSTIME Access ROS time functionality
%   T = ROSTIME returns a default ROS time object. The properties for seconds 
%   and nanoseconds are set to 0.
%
%   T = ROSTIME(TOTALSECS) initializes the time values for seconds and
%   nanoseconds based on the TOTALSECS input. TOTALSECS represents the time
%   in seconds as a floating-point number.
%
%   T = ROSTIME(SECS, NSECS) initializes the time values based on the SECS
%   (seconds) and NSECS (nanoseconds) inputs. Both inputs have to be
%   integers. Large values for NSECS are wrapped automatically and the
%   remainders are added to the value for SECS.
%
%   T = ROSTIME('now') returns the current ROS time in T. If the
%   "/use_sim_time" ROS parameter is set to true, this returns the
%   simulation time published on the "/clock" topic. Otherwise, the
%   function returns your computer's system time. T is a
%   robotics.ros.msg.Time object. If no output argument is given, the
%   current time (in seconds) is printed to the screen.
%
%   [T, ISSIMTIME] = ROSTIME('now') also returns a boolean ISSIMTIME
%   that indicates if T is simulation time (true) or system time (false).
%
%   T = ROSTIME('now','system') always returns your machine's system
%   time, even if ROS publishes simulation time on the "/clock" topic.
%   If no output argument is given, the system time (in seconds) is
%   printed to the screen.
%
%   The system time in ROS follows the Unix / POSIX time standard. POSIX
%   time is defined as the time that has elapsed since 00:00:00 Coordinated
%   Universal Time (UTC), 1 January 1970, not counting leap seconds.
%
%   ROSTIME can be used to timestamp messages or to measure time in the
%   ROS network.
%
%   Example:
%      % Create time object from seconds and nanoseconds
%      t1 = ROSTIME(1500,200000)
%
%      % Create time object for total seconds
%      t2 = ROSTIME(500.14671)
%
%      % Add 3 seconds to the time and calculate the duration between the
%      % two times
%      t2 = t2 + 3
%      dur = t1 - t2
%
%      % Show the current ROS time
%      ROSTIME now
%
%      % Return the current time in a time structure
%      t = ROSTIME('now')
%
%      % Timestamp message data with current system time
%      point = rosmessage('geometry_msgs/PointStamped');
%      point.Header.Stamp = ROSTIME('now','system');
%      point.Point.X = 5;
%
%   See also rosduration.

%   Copyright 2014-2016 The MathWorks, Inc.

narginchk(0,2);

try
    % Return right away for default time object
    if nargin == 0
        varargout{1} = robotics.ros.msg.Time;
        return;
    end
    
    % There is at least one argument. Decide which parser to use, based on
    % the data type.
    validateattributes(varargin{1}, {'numeric','char'}, {}, 'rostime');
    
    if isnumeric(varargin{1})
        % Parse second and optional nanosecond input and return time object
        varargout{1} = parseNumericInputs(varargin{:});
        return
    end
    
    % Assume that the user wants to use a string operation
    validOperations = {'now'};
    [operation, isSystemTime] = parseOperationInputs(validOperations, varargin{:});
    
    if nargout == 0
        rostimeImpl(operation, isSystemTime);
    else
        [varargout{1:nargout}] = rostimeImpl(operation, isSystemTime);
    end
catch ex
    % Save stack traces and exception causes internally, but do not
    % print them to the console
    rosex = robotics.ros.internal.ROSException.fromException(ex);
    throwAsCaller(rosex);
end
end


function timeObj = parseNumericInputs(varargin)
%parseNumericInputs Parse the numeric arguments provided by the user
%   Valid syntaxes:
%   - ROSTIME(TOTALSECS)
%   - ROSTIME(SECS, NSECS)

import robotics.ros.internal.Parsing.isValidUnsignedSecsNsecs
import robotics.ros.internal.Parsing.validateTotalUnsignedSeconds
import robotics.ros.internal.Parsing.validateUnsignedSeconds
import robotics.ros.internal.Parsing.validateSignedNanoseconds
import com.mathworks.toolbox.robotics.ros.TimeOperations.totalNsecs

javaTimeObj = org.ros.message.Time;
timeObj = robotics.ros.msg.Time(javaTimeObj);

% We already ascertained that there are only 1 or 2 inputs
switch length(varargin)
    case 1
        % Syntax: ROSTIME(TOTALSECS)
        % Parse the floating-point seconds.
        totalSecs = varargin{1};
        [timeObj.Sec, timeObj.Nsec] = validateTotalUnsignedSeconds(totalSecs, 'rostime', 'totalSecs');
        
    case 2
        % Syntax: ROSTIME(SECS, NSECS)
        % Parse the integer seconds and nanoseconds.
        secs = varargin{1};
        nsecs = varargin{2};
        validSecs = validateUnsignedSeconds(secs, 'rostime', 'secs');
        
        % Normalize the input to create a valid time object.
        % Take the overflow from nanoseconds larger than 1e9 into account.        
        [validNsecs, secOverflow] = validateSignedNanoseconds(nsecs, true, 'rostime', 'nsecs');
        
        validSecs = validSecs + secOverflow;
        
        % The resulting time needs to be within the valid time limits.
        % Otherwise, an overflow occurred and we should throw an error.
        if ~isValidUnsignedSecsNsecs(validSecs, validNsecs)
            error(message('robotics:ros:time:ResultTimeInvalid'));
        end
        
        javaTimeObj.secs = validSecs;
        javaTimeObj.nsecs = validNsecs;
                
    otherwise
        assert(false);
end

end


function [validOperation, isSystemTime] = parseOperationInputs(validOperations, varargin)
%parseOperationInputs Parse the string arguments provided by the user
%   Valid syntaxes:
%   - ROSTIME('now')
%   - ROSTIME('now', 'system')

% We already ascertained that there is at least one input
assert(length(varargin) >= 1);

isSystemTime = false;

operation = varargin{1};
validOperation = validatestring(operation, validOperations, 'rostime', 'operation');

if length(varargin) == 2
    % Syntax: ROSTIME('now', 'system')
    
    validatestring(varargin{2}, {'system'}, 'rostime', 'provider');
    
    % If the validation passes, we know that the user wants system time
    isSystemTime = true;
end

end


function varargout = rostimeImpl(operation, isSystemTime)
%rostimeImpl Actual implementation of rostime functionality
%   This retrieves either the system or current ROS time.

% The operation should be parsed and valid
switch operation
    case 'now'
        
        if isSystemTime
            % Return or print system time
            sysTime = getCurrentSystemTime;
            if nargout > 0
                varargout{1} = sysTime;
                return;
            end
            printTime(sysTime);            
        else
            % Let ROS decide what time to return (system or simulation time)
            [currentTime, isSimTime] = getCurrentROSTime;
            
            if nargout > 0
                varargout{1} = currentTime;
                varargout{2} = isSimTime;
                return;
            end
            
            printTime(currentTime);            
        end

    otherwise
        assert(false);
end
end

function [currentTime, isSimTime] = getCurrentROSTime
%getCurrentROSTime Retrieve the current ROS time

% Return the current time
timeObj = robotics.ros.internal.Time([]);
currentTime = timeObj.CurrentTime;
isSimTime = timeObj.IsSimulationTime;
end

function sysTime = getCurrentSystemTime
%getCurrentSystemTime Retrieve Unix system time as ROS time structure

% Get Unix epoch time through Java call
timeInMillis = java.lang.System.currentTimeMillis;

% Construct time object
sysTime = robotics.ros.msg.Time;
sysTime.Sec = int32(timeInMillis / 1000);
sysTime.Nsec = int32(mod(timeInMillis,1000)) * 1000000;
end

function printTime(currentTime)
%printTime Print time (in seconds) to the console

if isempty(currentTime)
    secs = 0;
else
    secs = double(currentTime.Sec) + double(currentTime.Nsec) / 1e9;
end

disp([num2str(secs) ' ' message('robotics:ros:time:CurrentTimeSeconds').getString]);
end