def init_simtime():
"""
Initialize the ROS time system by connecting to the /time topic
IFF the /use_sim_time parameter is set.
"""
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, roslib.msg.Clock, _set_rostime_clock_wrapper)
logger.info("connected to core topic %s" % _ROSCLOCK)
_set_rostime(rospy.rostime.Time(0, 0))
rospy.rostime.set_rostime_initialized(True)
return True
except Exception, e:
logger.error("Unable to initialize %s: %s\n%s", _ROSCLOCK, e,
traceback.format_exc())
return False
def test_set_rostime(self):
from rospy.rostime import _set_rostime
from rospy import Time
self.assertAlmostEqual(time.time(), rospy.get_time(), 1)
for t in [Time.from_sec(1.0), Time.from_sec(4.0)]:
_set_rostime(t)
self.assertEquals(t, rospy.get_rostime())
self.assertEquals(t.to_time(), rospy.get_time())
def test_set_rostime(self):
from rospy.rostime import _set_rostime
from rospy import Time
self.assertAlmostEqual(time.time(), rospy.get_time(), 1)
for t in [Time.from_sec(1.0), Time.from_sec(4.0)]:
_set_rostime(t)
self.assertEquals(t, rospy.get_rostime())
self.assertEquals(t.to_time(), rospy.get_time())
def test_sleep(self):
# test wallclock sleep
rospy.rostime.switch_to_wallclock()
rospy.sleep(0.1)
rospy.sleep(rospy.Duration.from_sec(0.1))
from rospy.rostime import _set_rostime
from rospy import Time
t = Time.from_sec(1.0)
_set_rostime(t)
self.assertEquals(t, rospy.get_rostime())
self.assertEquals(t.to_time(), rospy.get_time())
import threading
#start sleeper
self.failIf(test_sleep_done)
sleepthread = threading.Thread(target=sleeper, args=())
sleepthread.daemon = True
sleepthread.start()
time.sleep(1.0) #make sure thread is spun up
self.failIf(test_sleep_done)
t = Time.from_sec(1000000.0)
_set_rostime(t)
time.sleep(0.5) #give sleeper time to wakeup
self.assert_(test_sleep_done, "sleeper did not wake up")
#start duration sleeper
self.failIf(test_duration_sleep_done)
dursleepthread = threading.Thread(target=duration_sleeper, args=())
dursleepthread.daemon = True
dursleepthread.start()
time.sleep(1.0) #make sure thread is spun up
self.failIf(test_duration_sleep_done)
t = Time.from_sec(2000000.0)
_set_rostime(t)
time.sleep(0.5) #give sleeper time to wakeup
self.assert_(test_sleep_done, "sleeper did not wake up")
#start backwards sleeper
self.failIf(test_backwards_sleep_done)
backsleepthread = threading.Thread(target=backwards_sleeper, args=())
backsleepthread.daemon = True
backsleepthread.start()
time.sleep(1.0) #make sure thread is spun up
self.failIf(test_backwards_sleep_done)
t = Time.from_sec(1.0)
_set_rostime(t)
time.sleep(0.5) #give sleeper time to wakeup
self.assert_(test_backwards_sleep_done,
"backwards sleeper was not given an exception")
def test_sleep(self):
# test wallclock sleep
rospy.rostime.switch_to_wallclock()
rospy.sleep(0.1)
rospy.sleep(rospy.Duration.from_sec(0.1))
rospy.sleep(rospy.Duration.from_seconds(0.1))
from rospy.rostime import _set_rostime
from rospy import Time
t = Time.from_sec(1.0)
_set_rostime(t)
self.assertEquals(t, rospy.get_rostime())
self.assertEquals(t.to_time(), rospy.get_time())
import thread
#start sleeper
self.failIf(test_sleep_done)
thread.start_new_thread(sleeper, ())
time.sleep(1.0) #make sure thread is spun up
self.failIf(test_sleep_done)
t = Time.from_sec(1000000.0)
_set_rostime(t)
time.sleep(0.5) #give sleeper time to wakeup
self.assert_(test_sleep_done, "sleeper did not wake up")
#start duration sleeper
self.failIf(test_duration_sleep_done)
thread.start_new_thread(duration_sleeper, ())
time.sleep(1.0) #make sure thread is spun up
self.failIf(test_duration_sleep_done)
t = Time.from_sec(2000000.0)
_set_rostime(t)
time.sleep(0.5) #give sleeper time to wakeup
self.assert_(test_sleep_done, "sleeper did not wake up")
#start backwards sleeper
self.failIf(test_backwards_sleep_done)
thread.start_new_thread(backwards_sleeper, ())
time.sleep(1.0) #make sure thread is spun up
self.failIf(test_backwards_sleep_done)
t = Time.from_sec(1.0)
_set_rostime(t)
time.sleep(0.5) #give sleeper time to wakeup
self.assert_(test_backwards_sleep_done,
"backwards sleeper was not given an exception")
def test_sleep(self):
# test wallclock sleep
rospy.rostime.switch_to_wallclock()
rospy.sleep(0.1)
rospy.sleep(rospy.Duration.from_sec(0.1))
from rospy.rostime import _set_rostime
from rospy import Time
t = Time.from_sec(1.0)
_set_rostime(t)
self.assertEquals(t, rospy.get_rostime())
self.assertEquals(t.to_time(), rospy.get_time())
import thread
#start sleeper
self.failIf(test_sleep_done)
thread.start_new_thread(sleeper, ())
time.sleep(1.0) #make sure thread is spun up
self.failIf(test_sleep_done)
t = Time.from_sec(1000000.0)
_set_rostime(t)
time.sleep(0.5) #give sleeper time to wakeup
self.assert_(test_sleep_done, "sleeper did not wake up")
#start duration sleeper
self.failIf(test_duration_sleep_done)
thread.start_new_thread(duration_sleeper, ())
time.sleep(1.0) #make sure thread is spun up
self.failIf(test_duration_sleep_done)
t = Time.from_sec(2000000.0)
_set_rostime(t)
time.sleep(0.5) #give sleeper time to wakeup
self.assert_(test_sleep_done, "sleeper did not wake up")
#start backwards sleeper
self.failIf(test_backwards_sleep_done)
thread.start_new_thread(backwards_sleeper, ())
time.sleep(1.0) #make sure thread is spun up
self.failIf(test_backwards_sleep_done)
t = Time.from_sec(1.0)
_set_rostime(t)
time.sleep(0.5) #give sleeper time to wakeup
self.assert_(test_backwards_sleep_done, "backwards sleeper was not given an exception")
def init_simtime():
"""
Initialize the ROS time system by connecting to the /time topic
IFF the /use_sim_time parameter is set.
"""
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, roslib.msg.Clock, _set_rostime_clock_wrapper)
logger.info("connected to core topic %s"%_ROSCLOCK)
_set_rostime(rospy.rostime.Time(0, 0))
rospy.rostime.set_rostime_initialized(True)
return True
except Exception as e:
logger.error("Unable to initialize %s: %s\n%s", _ROSCLOCK, e, traceback.format_exc())
return False
def _set_rostime_time_wrapper(time_msg):
_set_rostime(time_msg.rostime)
def _set_rostime_clock_wrapper(time_msg):
_set_rostime(time_msg.clock)
