Package rospy :: Package impl :: Module simtime

Source Code for Module rospy.impl.simtime

 1  # Software License Agreement (BSD License) 
 2  # 
 3  # Copyright (c) 2008, Willow Garage, Inc. 
 4  # All rights reserved. 
 5  # 
 6  # Redistribution and use in source and binary forms, with or without 
 7  # modification, are permitted provided that the following conditions 
 8  # are met: 
 9  # 
10  #  * Redistributions of source code must retain the above copyright 
11  #    notice, this list of conditions and the following disclaimer. 
12  #  * Redistributions in binary form must reproduce the above 
13  #    copyright notice, this list of conditions and the following 
14  #    disclaimer in the documentation and/or other materials provided 
15  #    with the distribution. 
16  #  * Neither the name of Willow Garage, Inc. nor the names of its 
17  #    contributors may be used to endorse or promote products derived 
18  #    from this software without specific prior written permission. 
19  # 
20  # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
21  # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
22  # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 
23  # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
24  # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 
25  # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 
26  # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
27  # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
28  # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 
29  # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 
30  # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
31  # POSSIBILITY OF SUCH DAMAGE. 
32  # 
33  # Revision $Id$ 
34   
35  """Internal-use: Support for simulated clock.""" 
36   
37  import traceback 
38   
39  import rosgraph 
40  from rosgraph_msgs.msg import Clock 
41   
42  import rospy.core 
43  import rospy.rostime 
44  import rospy.topics 
45   
46  # ROS clock topics and parameter config 
47  _ROSCLOCK = '/clock' 
48  _USE_SIMTIME = '/use_sim_time' 
49   
50  # Subscriber handles for /clock and /time 
51  _rostime_sub = None 
52  _rosclock_sub = None 
53   
54 -def _is_use_simtime():
55 # in order to prevent circular dependencies, this does not use the 56 # builtin libraries for interacting with the parameter server, at least 57 # until I reorganize the client vs. internal APIs better. 58 master_uri = rosgraph.get_master_uri() 59 m = rospy.core.xmlrpcapi(master_uri) 60 code, msg, val = m.getParam(rospy.names.get_caller_id(), _USE_SIMTIME) 61 if code == 1 and val: 62 return True 63 return False
64 65 from rospy.rostime import _set_rostime
66 -def _set_rostime_clock_wrapper(time_msg):
67 _set_rostime(time_msg.clock)
68 -def _set_rostime_time_wrapper(time_msg):
69 _set_rostime(time_msg.rostime)
70
71 -def init_simtime():
72 """ 73 Initialize the ROS time system by connecting to the /time topic 74 IFF the /use_sim_time parameter is set. 75 """ 76 import logging 77 logger = logging.getLogger("rospy.simtime") 78 try: 79 if not _is_use_simtime(): 80 logger.info("%s is not set, will not subscribe to simulated time [%s] topic"%(_USE_SIMTIME, _ROSCLOCK)) 81 else: 82 global _rostime_sub, _clock_sub 83 if _rostime_sub is None: 84 logger.info("initializing %s core topic"%_ROSCLOCK) 85 _clock_sub = rospy.topics.Subscriber(_ROSCLOCK, Clock, _set_rostime_clock_wrapper, queue_size=1) 86 logger.info("connected to core topic %s"%_ROSCLOCK) 87 88 _set_rostime(rospy.rostime.Time(0, 0)) 89 rospy.rostime.set_rostime_initialized(True) 90 return True 91 except Exception as e: 92 logger.error("Unable to initialize %s: %s\n%s", _ROSCLOCK, e, traceback.format_exc()) 93 return False
94