diff --git a/scenario_execution_ros/scenario_execution_ros/actions/assert_topic_latency.py b/scenario_execution_ros/scenario_execution_ros/actions/assert_topic_latency.py index f2ecef35..0670483e 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/assert_topic_latency.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/assert_topic_latency.py @@ -26,24 +26,26 @@ class AssertTopicLatency(BaseAction): - def __init__(self, topic_name: str, topic_type: str, latency: float, comparison_operator: bool, rolling_average_count: int, wait_for_first_message: bool): + def __init__(self, topic_name: str, topic_type: str, wait_for_first_message: bool): super().__init__() self.topic_name = topic_name self.topic_type = topic_type - self.latency = latency - self.comparison_operator_feedback = comparison_operator[0] - self.comparison_operator = get_comparison_operator(comparison_operator) - self.rolling_average_count = rolling_average_count - self.rolling_average_count_queue = deque(maxlen=rolling_average_count) + self.latency = None + self.comparison_operator_feedback = None + self.comparison_operator = None + self.rolling_average_count = None + self.rolling_average_count_queue = None self.wait_for_first_message = wait_for_first_message + self.initial_wait_for_first_message = wait_for_first_message self.first_message_received = False self.node = None self.subscription = None self.last_receive_time = 0 self.msg_count = 0 self.average_latency = 0. - self.timer = 0 + self.timer = time.time() self.is_topic = False + self.retrigger = None def setup(self, **kwargs): try: @@ -59,13 +61,24 @@ def setup(self, **kwargs): elif not success and not self.wait_for_first_message: raise ActionError("Topic type must be specified. Please provide a valid topic type.", action=self) - def execute(self): - if self.timer != 0: - raise ActionError("Action does not yet support to get retriggered", action=self) + def execute(self, latency: float, comparison_operator: bool, rolling_average_count: int): + self.latency = latency + self.comparison_operator_feedback = comparison_operator[0] + self.comparison_operator = get_comparison_operator(comparison_operator) + self.rolling_average_count = rolling_average_count + self.rolling_average_count_queue = deque(maxlen=rolling_average_count) self.timer = time.time() + self.last_receive_time = 0 + self.msg_count = 0 + self.average_latency = 0. + self.is_topic = False + self.retrigger = True def update(self) -> py_trees.common.Status: result = py_trees.common.Status.FAILURE + if self.retrigger: + self.wait_for_first_message = self.initial_wait_for_first_message + self.retrigger = False if not self.is_topic: self.check_topic() self.logger.info(f"Waiting for the topic '{self.topic_name}' to become available")