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
