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 8c34a136..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") @@ -85,7 +98,10 @@ def update(self) -> py_trees.common.Status: self.feedback_message = f"No message received on the topic '{self.topic_name}'" # pylint: disable= attribute-defined-outside-init result = py_trees.common.Status.RUNNING elif self.msg_count > 1: - if self.comparison_operator(self.average_latency, self.latency): + if self.comparison_operator(self.latency, time.time() - self.last_receive_time): + self.feedback_message = f"Failed to receive message within the expected latency threshold ({self.latency} seconds)" # pylint: disable= attribute-defined-outside-init + result = py_trees.common.Status.FAILURE + elif self.comparison_operator(self.average_latency, self.latency): result = py_trees.common.Status.RUNNING self.feedback_message = f'Latency within range: expected {self.comparison_operator_feedback} {self.latency} s, actual {self.average_latency} s' # pylint: disable= attribute-defined-outside-init else: diff --git a/scenario_execution_ros/test/test_assert_topic_latency.py b/scenario_execution_ros/test/test_assert_topic_latency.py index 8e659b7d..3c3d7112 100644 --- a/scenario_execution_ros/test/test_assert_topic_latency.py +++ b/scenario_execution_ros/test/test_assert_topic_latency.py @@ -100,6 +100,9 @@ def tearDown(self): # Case 13: Test continues running if message arrives within the latency time. # Case 14: Test fails if the topic provided doesn't show up in the latency time. +# 5. Retrigger + # Case 15: Test succeeds with timeout as action fails everytime (as recorded latency is greater than actual latency) and repeat gets triggered because of failure_is_success modifier. + def test_case_1(self): scenario_content = """ @@ -339,3 +342,25 @@ def test_case_14(self): """ self.execute(scenario_content) self.assertFalse(self.scenario_execution_ros.process_results()) + + def test_case_15(self): + scenario_content = """ +import osc.ros +import osc.helpers + +scenario test_assert_topic_latency: + do parallel: + serial: + repeat() + assert_topic_latency( + topic_name: '/bla', + latency: 0.5s, + topic_type: 'std_msgs.msg.String') with: + failure_is_success() + time_out: serial: + wait elapsed(10s) + emit end + +""" + self.execute(scenario_content) + self.assertTrue(self.scenario_execution_ros.process_results())