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