add support for retrigger

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