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