timer.py 4.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157
  1. #!/usr/bin/env python
  2. # Copyright (c) 2018-2020 Intel Corporation
  3. #
  4. # This work is licensed under the terms of the MIT license.
  5. # For a copy, see <https://opensource.org/licenses/MIT>.
  6. """
  7. This module provides access to the CARLA game time and contains a py_trees
  8. timeout behavior using the CARLA game time
  9. """
  10. import datetime
  11. import operator
  12. import py_trees
  13. class GameTime(object):
  14. """
  15. This (static) class provides access to the CARLA game time.
  16. The elapsed game time can be simply retrieved by calling:
  17. GameTime.get_time()
  18. """
  19. _current_game_time = 0.0 # Elapsed game time after starting this Timer
  20. _carla_time = 0.0
  21. _last_frame = 0
  22. _platform_timestamp = 0
  23. _init = False
  24. @staticmethod
  25. def on_carla_tick(timestamp):
  26. """
  27. Callback receiving the CARLA time
  28. Update time only when frame is more recent that last frame
  29. """
  30. if GameTime._last_frame < timestamp.frame:
  31. frames = timestamp.frame - GameTime._last_frame if GameTime._init else 1
  32. GameTime._current_game_time += timestamp.delta_seconds * frames
  33. GameTime._last_frame = timestamp.frame
  34. GameTime._platform_timestamp = datetime.datetime.now()
  35. GameTime._init = True
  36. GameTime._carla_time = timestamp.elapsed_seconds
  37. @staticmethod
  38. def restart():
  39. """
  40. Reset game timer to 0
  41. """
  42. GameTime._current_game_time = 0.0
  43. GameTime._carla_time = 0.0
  44. GameTime._last_frame = 0
  45. GameTime._init = False
  46. @staticmethod
  47. def get_time():
  48. """
  49. Returns elapsed game time
  50. """
  51. return GameTime._current_game_time
  52. @staticmethod
  53. def get_carla_time():
  54. """
  55. Returns elapsed game time
  56. """
  57. return GameTime._carla_time
  58. @staticmethod
  59. def get_wallclocktime():
  60. """
  61. Returns elapsed game time
  62. """
  63. return GameTime._platform_timestamp
  64. @staticmethod
  65. def get_frame():
  66. """
  67. Returns elapsed game time
  68. """
  69. return GameTime._last_frame
  70. class SimulationTimeCondition(py_trees.behaviour.Behaviour):
  71. """
  72. This class contains an atomic simulation time condition behavior.
  73. It uses the CARLA game time, not the system time which is used by
  74. the py_trees timer.
  75. Returns, if the provided rule was successfully evaluated
  76. """
  77. def __init__(self, timeout, comparison_operator=operator.gt, name="SimulationTimeCondition"):
  78. """
  79. Setup timeout
  80. """
  81. super(SimulationTimeCondition, self).__init__(name)
  82. self.logger.debug("%s.__init__()" % (self.__class__.__name__))
  83. self._timeout_value = timeout
  84. self._start_time = 0.0
  85. self._comparison_operator = comparison_operator
  86. def initialise(self):
  87. """
  88. Set start_time to current GameTime
  89. """
  90. self._start_time = GameTime.get_time()
  91. self.logger.debug("%s.initialise()" % (self.__class__.__name__))
  92. def update(self):
  93. """
  94. Get current game time, and compare it to the timeout value
  95. Upon successfully comparison using the provided comparison_operator,
  96. the status changes to SUCCESS
  97. """
  98. elapsed_time = GameTime.get_time() - self._start_time
  99. if not self._comparison_operator(elapsed_time, self._timeout_value):
  100. new_status = py_trees.common.Status.RUNNING
  101. else:
  102. new_status = py_trees.common.Status.SUCCESS
  103. self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
  104. return new_status
  105. class TimeOut(SimulationTimeCondition):
  106. """
  107. This class contains an atomic timeout behavior.
  108. It uses the CARLA game time, not the system time which is used by
  109. the py_trees timer.
  110. """
  111. def __init__(self, timeout, name="TimeOut"):
  112. """
  113. Setup timeout
  114. """
  115. super(TimeOut, self).__init__(timeout, name=name)
  116. self.timeout = False
  117. def update(self):
  118. """
  119. Upon reaching the timeout value the status changes to SUCCESS
  120. """
  121. new_status = super(TimeOut, self).update()
  122. if new_status == py_trees.common.Status.SUCCESS:
  123. self.timeout = True
  124. return new_status