ROSクロックシミュレーションの初期プローブ
rosクロックシミュレーションの初期プローブ
rosクロックシミュレーションパラメータを設定し、時間のclockトピックをパブリッシュすれば、rosシステムの時間を制御できます.しかしclockトピックをパブリッシュするノードが実行されない場合、どのような反応が現れますか?他のすべてのノードスレッドが遅延するとrospyが実行されます.sleep,rate.sleepなどの関数は、ずっと詰まっていて、whileループは飛び出しません.クロックが初期化されるまで待ちます.
瓜を触ってrosのpythonコードがどのように動作しているかを見てみましょう.過程の中でコードに対して論理分析を行って、注釈を増加して、このようにrosシステムの下層組織構造をもっとよく理解します.猫の虎のようにひょうたんに瓢箪を描き、自分のロボットシステムを開発することもある.ははは、吹くな.さっそく私たちのプログラム猿の透視眼を使って、更に1目10行のコード閲覧モードを開いて、行動しましょう!
まず公式APIドキュメントを添付しますhttps://docs.ros.org/api/rospy/html/rospy-module.html#sleepオンラインの説明と照らし合わせて、ローカルで関連するpyファイルを見つけることができます.win 10+melodicを例に挙げます.
判定シミュレーションパラメータを含むpythonファイルから:C:optrosmelodicx 64libsite-packagesrospyimplsimtime.pyコメント内部使用サポートクロックシミュレーション
ディレクトリはC:UsersAdministratorです.roslogのlogファイルは、クロックがシミュレーションされた状態かどうかを見ることができます.
masterのlog表示増加/use_sim_timeパラメータ
シミュレーション状態時ノードlog出力
マスターのロゴはclockトピックを購読していることを示しています
シミュレーションノードでなければ次のように出力されます.
_set_rostime定義はC:optrosmelodicx 64libsite-packagesrospyrostime.py
親genpy.Duration genpy.Timeの定義C:optrosmelodicx 64libsite-packagesgenpyrostime.py
主な内容
init_Node作成ノード初期化ノードは必ずC:optrosmelodicx 64libsite-packagesrospy clientを呼び出す.py
C:\opt\ros\melodic\x64\lib\site-packages\rospy timer.py
ここでは、talkerノードをパブリッシュする例です.
rosクロックシミュレーションパラメータを設定し、時間のclockトピックをパブリッシュすれば、rosシステムの時間を制御できます.しかしclockトピックをパブリッシュするノードが実行されない場合、どのような反応が現れますか?他のすべてのノードスレッドが遅延するとrospyが実行されます.sleep,rate.sleepなどの関数は、ずっと詰まっていて、whileループは飛び出しません.クロックが初期化されるまで待ちます.
瓜を触ってrosのpythonコードがどのように動作しているかを見てみましょう.過程の中でコードに対して論理分析を行って、注釈を増加して、このようにrosシステムの下層組織構造をもっとよく理解します.猫の虎のようにひょうたんに瓢箪を描き、自分のロボットシステムを開発することもある.ははは、吹くな.さっそく私たちのプログラム猿の透視眼を使って、更に1目10行のコード閲覧モードを開いて、行動しましょう!
まず公式APIドキュメントを添付しますhttps://docs.ros.org/api/rospy/html/rospy-module.html#sleepオンラインの説明と照らし合わせて、ローカルで関連するpyファイルを見つけることができます.win 10+melodicを例に挙げます.
判定シミュレーションパラメータを含むpythonファイルから:C:optrosmelodicx 64libsite-packagesrospyimplsimtime.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
%s", _ROSCLOCK, e, traceback.format_exc())
return False
ディレクトリはC:UsersAdministratorです.roslogの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マスターのロゴは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:optrosmelodicx 64libsite-packagesrospyrostime.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:optrosmelodicx 64libsite-packagesgenpyrostime.py
主な内容
class TVal(object):
class Time(TVal):
class Duration(TVal):init_Node作成ノード初期化ノードは必ずC:optrosmelodicx 64libsite-packagesrospy 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