fix asset_topic_latency action (#210)

* fix action

* add support for retrigger

* add text case

* fix typo

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:

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())