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