ROS时钟仿真的初探

ros时钟仿真的初探

如果设置了ros时钟仿真参数，发布一个时间的clock话题，就可以控制ros系统的时间。
但是当发布clock话题的节点没有运行，会出现什么反应？
我得出的答案是
当其他所有节点线程遇到延时，执行rospy.sleep，rate.sleep等函数时，会一直卡住，while循环不跳出。一直等待 ，直到时钟初始化。

让我们顺腾摸瓜一起看看ros 的python代码是如何运作的。
过程中对代码进行逻辑分析，增加注释，这样更好的理解ros系统底层组织结构。
甚至照猫画虎依葫芦画瓢，开发自己的机器人系统。哈哈，别吹了。
赶紧用咱们程序猿的透视眼，再开启一目十行的代码浏览模式，行动吧！

先附上官方API文档
https://docs.ros.org/api/rospy/html/rospy-module.html#sleep
可以对照在线说明，在本地找到相关的py文件。以win10+melodic为例。

从包含判断仿真参数的python文件开始：
C:\opt\ros\melodic\x64\lib\site-packages\rospy\impl
simtime.py
注释内部使用 支持时钟仿真

# ROS clock topics and parameter config 参数配置
_ROSCLOCK = '/clock'
_USE_SIMTIME = '/use_sim_time'

# Subscriber handles for /clock and /time订阅者
_rostime_sub = None
_rosclock_sub = None

#判断是否使用仿真
#请求master获取参数 返回
def _is_use_simtime():
    # in order to prevent circular dependencies, this does not use the
    # builtin libraries for interacting with the parameter server, at least
    # until I reorganize the client vs. internal APIs better.
    master_uri = rosgraph.get_master_uri()
    m = rospy.core.xmlrpcapi(master_uri)
    code, msg, val = m.getParam(rospy.names.get_caller_id(), _USE_SIMTIME)
    if code == 1 and val:
        return True
    return False

from rospy.rostime import _set_rostime
def _set_rostime_clock_wrapper(time_msg):#这里是订阅时钟的callback
    _set_rostime(time_msg.clock)#收到了就进行设置，这个设置函数定义在哪里呢？？------->_set_rostime问题
def _set_rostime_time_wrapper(time_msg):
    _set_rostime(time_msg.rostime)
    
def init_simtime():
    """
    Initialize the ROS time system by connecting to the /time topic
    IFF the /use_sim_time parameter is set.
    如果使用仿真参数设置了，ros系统时钟是 发布的/time话题 初始
    """    
    import logging
    logger = logging.getLogger("rospy.simtime")
    try:
        if not _is_use_simtime():
            logger.info("%s is not set, will not subscribe to simulated time [%s] topic"%(_USE_SIMTIME, _ROSCLOCK))
        else:
            global _rostime_sub, _clock_sub
            if _rostime_sub is None:
                logger.info("initializing %s core topic"%_ROSCLOCK)
#使用仿真这里订阅
                _clock_sub = rospy.topics.Subscriber(_ROSCLOCK, Clock, _set_rostime_clock_wrapper, queue_size=1)
                logger.info("connected to core topic %s"%_ROSCLOCK)
#初始为0，。。。等待订阅执行callback进行设置
                _set_rostime(rospy.rostime.Time(0, 0))
#设置时间已经初始化的标记
        rospy.rostime.set_rostime_initialized(True)#1111111111111
        return True
    except Exception as e:
        logger.error("Unable to initialize %s: %s\n%s", _ROSCLOCK, e, traceback.format_exc())
        return False

从目录在C:\Users\Administrator\.ros\log的log文件可以看到时钟是否是仿真的状态。

master的log显示增加/use_sim_time参数

[rosmaster.master][INFO] 2019-11-17 23:00:06,096: +PARAM [/use_sim_time] by /roslaunch

是仿真状态时节点log输出

[rospy.simtime][INFO] 2019-11-17 23:47:16,391: initializing /clock core topic
[rospy.simtime][INFO] 2019-11-17 23:47:16,401: connected to core topic /clock

master的log显示订阅了 clock话题

[rosmaster.master][INFO] 2019-11-17 23:47:16,401: +SUB [/clock] /serial_node http://SC-201904021108:59930/

如果不是仿真节点输出如下

[rospy.simtime][INFO] 2019-11-19 17:22:26,328: /use_sim_time is not set, will not subscribe to simulated time [/clock] topic

_set_rostime定义在
C:\opt\ros\melodic\x64\lib\site-packages\rospy
rostime.py

## /time support. This hooks into the rospy Time representation and
## allows it to be overriden with data from the /time topic.

_rostime_initialized = False  #初始化标记
_rostime_current = None
_rostime_cond = threading.Condition()

# subclass genpy to provide abstraction layer
class Duration(genpy.Duration):#继承自genpy.Duration
class Time(genpy.Time):#继承自genpy.Time

def _set_rostime(t):
    print("---------------------_set_rostime(",t,")")
    """Callback to update ROS time from a ROS Topic"""
    if isinstance(t, genpy.Time):
        t = Time(t.secs, t.nsecs)
    elif not isinstance(t, Time):
        raise ValueError("must be Time instance: %s"%t.__class__)
    global _rostime_current
    _rostime_current = t
    try:
        _rostime_cond.acquire()
        _rostime_cond.notifyAll()
    finally:
        _rostime_cond.release()
    
def get_rostime():
    """
    Get the current time as a L{Time} object    
    @return: current time as a L{rospy.Time} object
    @rtype: L{Time}
    """
    if not _rostime_initialized:#先判断初始化标记_rostime_initialized，Have you called init_node()?看来这个标记是在init_node中设置的。
        raise rospy.exceptions.ROSInitException("time is not initialized. Have you called init_node()?")
    if _rostime_current is not None:
        # initialize with sim time初始化仿真时间
        #rospy.loginfo('notnone get_rostime=%s'%_rostime_current  )
        print("notnone get_rostime=",_rostime_current) 
        return _rostime_current
    else:
        print("--------------------none get_rostime=", time.time()) #非仿真时的设置
        # initialize with wallclock
        float_secs = time.time()
        secs = int(float_secs)
        nsecs = int((float_secs - secs) * 1000000000)
        #rospy.loginfo('none get_rostime=%s'%secs  )
        return Time(secs, nsecs)

def set_rostime_initialized(val):#init_node中调用这个函数。------------------------>找找 init_node
    """
    Internal use.
    Mark rostime as initialized. This flag enables other routines to
    throw exceptions if rostime is being used before the underlying
    system is initialized.
    @param val: value for initialization state
    @type  val: bool
    """
    print("--------------------set_rostime_initialized",val) 
    global _rostime_initialized
    _rostime_initialized = val

def is_rostime_initialized():
   """
    Internal use.
    @return: True if rostime has been initialized
    @rtype: bool
    """
    return _rostime_initialized    

def get_rostime_cond():
    """
    internal API for helper routines that need to wait on time updates
    @return: rostime conditional var
    @rtype: threading.Cond
    """
    return _rostime_cond

def is_wallclock():
    """
    Internal use for ROS-time routines.
    @return: True if ROS is currently using wallclock time
    @rtype: bool
    """
    return _rostime_current == None
    
def switch_to_wallclock():
    """
    Internal use.
    Switch ROS to wallclock time. This is mainly for testing purposes.
    """
    global _rostime_current
    _rostime_current = None
    try:
        _rostime_cond.acquire()
        _rostime_cond.notifyAll()
    finally:
        _rostime_cond.release()

def wallsleep(duration):
    """
    Internal use.
    Windows interrupts time.sleep with an IOError exception
    when a signal is caught. Even when the signal is handled
    by a callback, it will then proceed to throw IOError when
    the handling has completed. 

    Refer to https://code.ros.org/trac/ros/ticket/3421.

    So we create a platform dependant wrapper to handle this
    here.
    """
    if sys.platform in ['win32']: # cygwin seems to be ok
        try:
            time.sleep(duration)
        except IOError:
            pass
    else:
        time.sleep(duration) 

父类
genpy.Duration
genpy.Time
的定义
C:\opt\ros\melodic\x64\lib\site-packages\genpy
rostime.py

其中主要的内容

class TVal(object):
class Time(TVal):
class Duration(TVal):

 init_node
创建节点初始化节点必然调用
C:\opt\ros\melodic\x64\lib\site-packages\rospy
client.py

_init_node_args = None

def init_node(name, argv=None, anonymous=False, log_level=None, disable_rostime=False, disable_rosout=False, disable_signals=False, xmlrpc_port=0, tcpros_port=0):
    """
    Register client node with the master under the specified name.
    This MUST be called from the main Python thread unless
    disable_signals is set to True. Duplicate calls to init_node are
    only allowed if the arguments are identical as the side-effects of
    this method are not reversible.

    @param name: Node's name. This parameter must be a base name,
        meaning that it cannot contain namespaces (i.e. '/')
    @type  name: str
    
    @param argv: Command line arguments to this program, including
        remapping arguments (default: sys.argv). If you provide argv
        to init_node(), any previously created rospy data structure
        (Publisher, Subscriber, Service) will have invalid
        mappings. It is important that you call init_node() first if
        you wish to provide your own argv.
    @type  argv: [str]

    @param anonymous: if True, a name will be auto-generated for the
        node using name as the base.  This is useful when you wish to
        have multiple instances of the same node and don't care about
        their actual names (e.g. tools, guis). name will be used as
        the stem of the auto-generated name. NOTE: you cannot remap
        the name of an anonymous node.  
    @type anonymous: bool

    @param log_level: log level for sending message to /rosout and log
        file, which is INFO by default. For convenience, you may use
        rospy.DEBUG, rospy.INFO, rospy.ERROR, rospy.WARN, rospy.FATAL
    @type  log_level: int
    
    @param disable_signals: If True, rospy will not register its own
        signal handlers. You must set this flag if (a) you are unable
        to call init_node from the main thread and/or you are using
        rospy in an environment where you need to control your own
        signal handling (e.g. WX). If you set this to True, you should
        call rospy.signal_shutdown(reason) to initiate clean shutdown.

        NOTE: disable_signals is overridden to True if
        roslib.is_interactive() is True.
        
    @type  disable_signals: bool
    
    @param disable_rostime: for internal testing only: suppresses
        automatic subscription to rostime-----------------------------抑制rostime（仿真）的自动订阅。false 默认不抑制仿真。false==仿真
    @type  disable_rostime: bool

    @param disable_rosout: for internal testing only: suppress
        auto-publication of rosout
    @type  disable_rostime: bool

    @param xmlrpc_port: If provided, it will use this port number for the client
        XMLRPC node. 
    @type  xmlrpc_port: int

    @param tcpros_port: If provided, the TCPROS server will listen for
        connections on this port
    @type  tcpros_port: int

    @raise ROSInitException: if initialization/registration fails
    @raise ValueError: if parameters are invalid (e.g. name contains a namespace or is otherwise illegal)
    """
    if argv is None:
        argv = sys.argv
    else:
        # reload the mapping table. Any previously created rospy data
        # structure does *not* reinitialize based on the new mappings.
        rospy.names.reload_mappings(argv)

    if not name:
        raise ValueError("name must not be empty")

    # this test can be eliminated once we change from warning to error in the next check
    if rosgraph.names.SEP in name:
        raise ValueError("namespaces are not allowed in node names")

    global _init_node_args

    # #972: allow duplicate init_node args if calls are identical
    # NOTE: we don't bother checking for node name aliases (e.g. 'foo' == '/foo').
    if _init_node_args:
        if _init_node_args != (name, argv, anonymous, log_level, disable_rostime, disable_signals):
            raise rospy.exceptions.ROSException("rospy.init_node() has already been called with different arguments: "+str(_init_node_args))
        else:
            return #already initialized

    # for scripting environments, we don't want to use the ROS signal
    # handlers
    disable_signals = disable_signals or roslib.is_interactive()
    _init_node_args = (name, argv, anonymous, log_level, disable_rostime, disable_signals)
        
    if not disable_signals:
        # NOTE: register_signals must be called from main thread
        rospy.core.register_signals() # add handlers for SIGINT/etc...
    else:
        logdebug("signal handlers for rospy disabled")

    # check for name override
    mappings = rospy.names.get_mappings()
    if '__name' in mappings:
        name = mappings['__name']
        if anonymous:
            logdebug("[%s] WARNING: due to __name setting, anonymous setting is being changed to false"%name)
            anonymous = False
        
    if anonymous:
        # not as good as a uuid/guid, but more readable. can't include
        # hostname as that is not guaranteed to be a legal ROS name
        name = "%s_%s_%s"%(name, os.getpid(), int(time.time()*1000))

    # check for legal base name once all changes have been made to the name
    if not rosgraph.names.is_legal_base_name(name):
        import warnings
        warnings.warn("'%s' is not a legal ROS base name. This may cause problems with other ROS tools."%name, stacklevel=2)

    # use rosgraph version of resolve_name to avoid remapping
    resolved_node_name = rosgraph.names.resolve_name(name, rospy.core.get_caller_id())
    rospy.core.configure_logging(resolved_node_name)
    # #1810
    rospy.names.initialize_mappings(resolved_node_name)
    
    logger = logging.getLogger("rospy.client")
    logger.info("init_node, name[%s], pid[%s]", resolved_node_name, os.getpid())
            
    # node initialization blocks until registration with master-------------------向master注册
    node = rospy.impl.init.start_node(os.environ, resolved_node_name, port=xmlrpc_port, tcpros_port=tcpros_port) 
    rospy.core.set_node_uri(node.uri)
    rospy.core.add_shutdown_hook(node.shutdown)    
          
    if rospy.core.is_shutdown():
        logger.warn("aborting node initialization as shutdown has been triggered")
        raise rospy.exceptions.ROSInitException("init_node interrupted before it could complete")

    # upload private params (set via command-line) to parameter server
    _init_node_params(argv, name)

    rospy.core.set_initialized(True)

    if not disable_rosout:#非 禁止rosout
        rospy.impl.rosout.init_rosout()#创建/rosout话题的发布者
        rospy.impl.rosout.load_rosout_handlers(log_level)
    #zzz time.sleep(3) 
    
    if not disable_rostime: #disable_rostime名字有点异议，注释说 disable_rostime代表抑制仿真。
        #仿真。
        #订阅clock话题再 set_rostime_initialized(True)标记
        #rospy.loginfo("zzzinit_node1" )
        #---------------------看看前文就知道rospy.impl.simtime.init_simtime()是不是做了这些工作。
        if not rospy.impl.simtime.init_simtime():
            raise rospy.exceptions.ROSInitException("Failed to initialize time. Please check logs for additional details")
    else:
        #不仿真直接标记
        rospy.rostime.set_rostime_initialized(True)#1111111111111
        
    logdebug("init_node, name[%s], pid[%s]", resolved_node_name, os.getpid())    
    # advertise logging level services
    Service('~get_loggers', GetLoggers, _get_loggers)
    Service('~set_logger_level', SetLoggerLevel, _set_logger_level)

 

C:\opt\ros\melodic\x64\lib\site-packages\rospy
timer.py

class Rate(object):
    """
    Convenience class for sleeping in a loop at a specified rate
    """
    
    def __init__(self, hz, reset=False):
        """
        Constructor.
        @param hz: hz rate to determine sleeping
        @type  hz: float
        @param reset: if True, timer is reset when rostime moved backward. [default: False]
        @type  reset: bool
        """
        # #1403
        self.last_time = rospy.rostime.get_rostime()
        self.sleep_dur = rospy.rostime.Duration(0, int(1e9/hz))
        self._reset = reset

    def _remaining(self, curr_time):
        """
        Calculate the time remaining for rate to sleep.
        @param curr_time: current time
        @type  curr_time: L{Time}
        @return: time remaining
        @rtype: L{Time}
        """
        # detect time jumping backwards
        if self.last_time > curr_time:
            self.last_time = curr_time

        # calculate remaining time
        elapsed = curr_time - self.last_time
        return self.sleep_dur - elapsed

    def remaining(self):
        """
        Return the time remaining for rate to sleep.
        @return: time remaining
        @rtype: L{Time}
        """
        curr_time = rospy.rostime.get_rostime()
        return self._remaining(curr_time)

    def sleep(self):
        """
        Attempt sleep at the specified rate. sleep() takes into
        account the time elapsed since the last successful
        sleep().
        
        @raise ROSInterruptException: if ROS shutdown occurs before
        sleep completes
        @raise ROSTimeMovedBackwardsException: if ROS time is set
        backwards
        """
        curr_time = rospy.rostime.get_rostime()
        try:
            sleep(self._remaining(curr_time))
        except rospy.exceptions.ROSTimeMovedBackwardsException:
            if not self._reset:
                raise
            self.last_time = rospy.rostime.get_rostime()
            return
        self.last_time = self.last_time + self.sleep_dur

        # detect time jumping forwards, as well as loops that are
        # inherently too slow
        if curr_time - self.last_time > self.sleep_dur * 2:
            self.last_time = curr_time

def sleep(duration):
    rospy.loginfo('zzz  sleep')
    """
    sleep for the specified duration in ROS time. If duration
    is negative, sleep immediately returns.
    
    @param duration: seconds (or rospy.Duration) to sleep
    @type  duration: float or Duration
    @raise ROSInterruptException: if ROS shutdown occurs before sleep
    completes
    @raise ROSTimeMovedBackwardsException: if ROS time is set
    backwards
    """
    if rospy.rostime.is_wallclock():
        rospy.loginfo('zzzrospy.rostime.is_wallclock')
        if isinstance(duration, genpy.Duration):
            duration = duration.to_sec()
        if duration < 0:
            return
        else:
            rospy.rostime.wallsleep(duration)

    else:
        initial_rostime = rospy.rostime.get_rostime()
        if not isinstance(duration, genpy.Duration):
            duration = genpy.Duration.from_sec(duration)

        rostime_cond = rospy.rostime.get_rostime_cond()

        # #3123
        if initial_rostime == genpy.Time(0):
            # break loop if time is initialized or node is shutdown 
            #也就是说没初始化&&没有 shutdown一直循环等待重复设置initial_rostime。initial_rostime == genpy.Time(0)是没有初始化
            while initial_rostime == genpy.Time(0) and \
                      not rospy.core.is_shutdown():
                with rostime_cond:
                    rostime_cond.wait(0.3)
                initial_rostime = rospy.rostime.get_rostime()
                rospy.loginfo('sleep_t=%s'%initial_rostime  )

        sleep_t = initial_rostime + duration
        rospy.loginfo('sleep_t=%s'%sleep_t )
        # break loop if sleep_t is reached, time moves backwards, or
        # node is shutdown
        #sleep时间没到&&时间没有倒流&&节点没有关闭一直循环
        while rospy.rostime.get_rostime() < sleep_t and \
              rospy.rostime.get_rostime() >= initial_rostime and \
                  not rospy.core.is_shutdown():
            with rostime_cond:
                rostime_cond.wait(0.5)
                

        if rospy.rostime.get_rostime() < initial_rostime:
            time_jump = (initial_rostime - rospy.rostime.get_rostime()).to_sec()
            raise rospy.exceptions.ROSTimeMovedBackwardsException(time_jump)
        if rospy.core.is_shutdown():
            raise rospy.exceptions.ROSInterruptException("ROS shutdown request")

class TimerEvent(object):
    """
    Constructor.
    @param last_expected: in a perfect world, this is when the previous callback should have happened
    @type  last_expected: rospy.Time
    @param last_real: when the callback actually happened
    @type  last_real: rospy.Time
    @param current_expected: in a perfect world, this is when the current callback should have been called
    @type  current_expected: rospy.Time
    @param last_duration: contains the duration of the last callback (end time minus start time) in seconds.
                          Note that this is always in wall-clock time.
    @type  last_duration: float
    """
    def __init__(self, last_expected, last_real, current_expected, current_real, last_duration):
        self.last_expected    = last_expected
        self.last_real        = last_real
        self.current_expected = current_expected
        self.current_real     = current_real
        self.last_duration    = last_duration

class Timer(threading.Thread):#-----------------------------继承了线程，以一定频率触发的计时器
    """
    Convenience class for calling a callback at a specified rate
    """

    def __init__(self, period, callback, oneshot=False, reset=False):
        """
        Constructor.
        @param period: desired period between callbacks
        @type  period: rospy.Duration
        @param callback: callback to be called
        @type  callback: function taking rospy.TimerEvent
        @param oneshot: if True, fire only once, otherwise fire continuously until shutdown is called [default: False]
        @type  oneshot: bool
        @param reset: if True, timer is reset when rostime moved backward. [default: False]
        @type  reset: bool
        """
        super(Timer, self).__init__()
        self._period   = period
        self._callback = callback
        self._oneshot  = oneshot
        self._reset = reset
        self._shutdown = False
        self.daemon = True
        self.start()

    def shutdown(self):
        """
        Stop firing callbacks.
        """
        self._shutdown = True
        
    def run(self):
        r = Rate(1.0 / self._period.to_sec(), reset=self._reset)
        current_expected = rospy.rostime.get_rostime() + self._period
        last_expected, last_real, last_duration = None, None, None
        while not rospy.core.is_shutdown() and not self._shutdown:
            try:
                r.sleep()
            except rospy.exceptions.ROSInterruptException as e:
                if rospy.core.is_shutdown():
                    break
                raise
            if self._shutdown:
                break
            current_real = rospy.rostime.get_rostime()
            start = time.time()
            self._callback(TimerEvent(last_expected, last_real, current_expected, current_real, last_duration))
            if self._oneshot:
                break
            last_duration = time.time() - start
            last_expected, last_real = current_expected, current_real
            current_expected += self._period

这里就比较亲切了，是一个发布talker节点的示例

import rospy
from std_msgs.msg import String

def talker():
    pub = rospy.Publisher('chatter', String, queue_size=10)
    rospy.init_node('talker', anonymous=True)#----------------------这里init_node，每个节点必不可少。
    rate = rospy.Rate(10) # 10hz
    rospy.loginfo("---------------------talkertest")
    while not rospy.is_shutdown():
        hello_str = "hello world %s" % rospy.get_time()
        rospy.loginfo(hello_str)
        pub.publish(hello_str)
        rospy.loginfo("while")
        rate.sleep()

if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException:
        pass