From 35ff68bb1d7a79f8a6d73c8bba9af10f1c836cea Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Tue, 15 Oct 2024 18:42:26 +0200 Subject: [PATCH 1/4] add param type_check --- docs/libraries.rst | 4 ++ .../actions/assert_topic_latency.py | 51 ++++++++++--------- .../scenario_execution_ros/lib_osc/ros.osc | 1 + 3 files changed, 33 insertions(+), 23 deletions(-) diff --git a/docs/libraries.rst b/docs/libraries.rst index 3be0bc3d..cf1f90e2 100644 --- a/docs/libraries.rst +++ b/docs/libraries.rst @@ -1261,6 +1261,10 @@ Check the latency of the specified topic (in system time). If the check with ``c - ``string`` - - Class of message type, only required when 'wait_for_first_message' is set to false (e.g. ``std_msgs.msg.String``) + * - ``type_check`` + - ``bool`` + - ``true`` + - If False, the action will not verify that the topic type matches the specified type. ``bag_play()`` 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 0670483e..0b6c5412 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,10 +26,11 @@ class AssertTopicLatency(BaseAction): - def __init__(self, topic_name: str, topic_type: str, wait_for_first_message: bool): + def __init__(self, topic_name: str, topic_type: str, wait_for_first_message: bool, type_check: bool): super().__init__() self.topic_name = topic_name self.topic_type = topic_type + self.type_check = type_check self.latency = None self.comparison_operator_feedback = None self.comparison_operator = None @@ -112,32 +113,36 @@ def update(self) -> py_trees.common.Status: return result def check_topic(self): - if self.wait_for_first_message: - available_topics = self.node.get_topic_names_and_types() - for name, topic_type in available_topics: - if name == self.topic_name: - topic_type = topic_type[0].replace('/', '.') - if self.topic_type: - if self.topic_type == topic_type: - self.call_subscriber() - self.is_topic = True - return True - else: - return False # Invalid topic or type speficied. - else: - self.topic_type = topic_type - self.call_subscriber() - self.is_topic = True # 'topic_type' is not specified, the process will continue with the found one - return True - # Topic not available wait.... - return True - else: + result = True + if not self.wait_for_first_message: if not self.topic_type: - return False # Topic type must be specified. (wait_for_first_message == False) + result = False # Topic type must be specified. (wait_for_first_message == False) else: self.call_subscriber() self.is_topic = True # wait_for_first_message' is set to false, the process will proceed with the specified topic and type - return True + else: + available_topics = self.node.get_topic_names_and_types() + topic_found = False + for name, topic_types in available_topics: + if name == self.topic_name: + topic_found = True + if not topic_types: + result = False + break + current_topic_type = topic_types[0].replace('/', '.') + if not self.topic_type: + self.topic_type = current_topic_type # 'topic_type' is not specified, the process will continue with the found one + self.call_subscriber() + self.is_topic = True + elif self.type_check and self.topic_type != current_topic_type: + result = False + else: + self.call_subscriber() + self.is_topic = True + break + if not topic_found: + pass # Topic not available wait.... + return result def call_subscriber(self): datatype_in_list = self.topic_type.split(".") diff --git a/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc b/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc index 7f664a80..91ddc4c7 100644 --- a/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc +++ b/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc @@ -70,6 +70,7 @@ action assert_topic_latency: rolling_average_count: int = 1 # the check is done aganist the rolling average over x elements wait_for_first_message: bool = true # start measuring with the first received message topic_type: string # class of message type, only required when wait_for_first_message is set to false (e.g. std_msgs.msg.String) + type_check: bool = true # If False, the action will not verify that the topic type matches the specified type. action bag_play: # play back a ros bag From 67a55844184d5ed5f420e1e3a5d4fd6f64aa24aa Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Thu, 17 Oct 2024 15:58:56 +0200 Subject: [PATCH 2/4] fix --- docs/libraries.rst | 5 ---- .../actions/assert_topic_latency.py | 25 ++++++++----------- .../scenario_execution_ros/lib_osc/ros.osc | 1 - 3 files changed, 11 insertions(+), 20 deletions(-) diff --git a/docs/libraries.rst b/docs/libraries.rst index cf1f90e2..7f1c7cb3 100644 --- a/docs/libraries.rst +++ b/docs/libraries.rst @@ -1261,11 +1261,6 @@ Check the latency of the specified topic (in system time). If the check with ``c - ``string`` - - Class of message type, only required when 'wait_for_first_message' is set to false (e.g. ``std_msgs.msg.String``) - * - ``type_check`` - - ``bool`` - - ``true`` - - If False, the action will not verify that the topic type matches the specified type. - ``bag_play()`` ^^^^^^^^^^^^^^^ 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 0b6c5412..866edcd4 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 @@ -18,19 +18,18 @@ from collections import deque import py_trees # pylint: disable=import-error from rclpy.node import Node -import importlib import time from scenario_execution_ros.actions.conversions import get_comparison_operator, get_qos_preset_profile from scenario_execution.actions.base_action import BaseAction, ActionError +from rosidl_runtime_py.utilities import get_message class AssertTopicLatency(BaseAction): - def __init__(self, topic_name: str, topic_type: str, wait_for_first_message: bool, type_check: 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.type_check = type_check self.latency = None self.comparison_operator_feedback = None self.comparison_operator = None @@ -129,12 +128,11 @@ def check_topic(self): if not topic_types: result = False break - current_topic_type = topic_types[0].replace('/', '.') if not self.topic_type: - self.topic_type = current_topic_type # 'topic_type' is not specified, the process will continue with the found one + self.topic_type = topic_types[0] # 'topic_type' is not specified, the process will continue with the found one self.call_subscriber() self.is_topic = True - elif self.type_check and self.topic_type != current_topic_type: + elif self.topic_type != topic_types[0]: result = False else: self.call_subscriber() @@ -145,17 +143,16 @@ def check_topic(self): return result def call_subscriber(self): - datatype_in_list = self.topic_type.split(".") - topic_type = getattr( - importlib.import_module(".".join(datatype_in_list[:-1])), - datatype_in_list[-1] - ) - + topic_type = self.topic_type.replace('.', '/') + message_class = get_message(topic_type) + if message_class is None: + raise ValueError(f"Message type '{topic_type}' could not be found.") self.subscription = self.node.create_subscription( - msg_type=topic_type, + msg_type=message_class, topic=self.topic_name, callback=self._callback, - qos_profile=get_qos_preset_profile(['sensor_data'])) + qos_profile=get_qos_preset_profile(['sensor_data']) + ) def _callback(self, msg): self.first_message_received = True diff --git a/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc b/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc index 91ddc4c7..7f664a80 100644 --- a/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc +++ b/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc @@ -70,7 +70,6 @@ action assert_topic_latency: rolling_average_count: int = 1 # the check is done aganist the rolling average over x elements wait_for_first_message: bool = true # start measuring with the first received message topic_type: string # class of message type, only required when wait_for_first_message is set to false (e.g. std_msgs.msg.String) - type_check: bool = true # If False, the action will not verify that the topic type matches the specified type. action bag_play: # play back a ros bag From 3d38ed41d8abe316542069bd8dda0a8c03407561 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Thu, 17 Oct 2024 16:00:01 +0200 Subject: [PATCH 3/4] fix docs --- docs/libraries.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/libraries.rst b/docs/libraries.rst index 7f1c7cb3..3be0bc3d 100644 --- a/docs/libraries.rst +++ b/docs/libraries.rst @@ -1262,6 +1262,7 @@ Check the latency of the specified topic (in system time). If the check with ``c - - Class of message type, only required when 'wait_for_first_message' is set to false (e.g. ``std_msgs.msg.String``) + ``bag_play()`` ^^^^^^^^^^^^^^^ From 97eac1ac156a679210e2d7473daecea7a25ed086 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Thu, 17 Oct 2024 16:26:58 +0200 Subject: [PATCH 4/4] fix --- .../scenario_execution_ros/actions/assert_topic_latency.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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 866edcd4..c9c3dc54 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 @@ -128,11 +128,12 @@ def check_topic(self): if not topic_types: result = False break + current_topic_type = topic_types[0].replace('/', '.') if not self.topic_type: - self.topic_type = topic_types[0] # 'topic_type' is not specified, the process will continue with the found one + self.topic_type = current_topic_type # 'topic_type' is not specified, the process will continue with the found one self.call_subscriber() self.is_topic = True - elif self.topic_type != topic_types[0]: + elif self.topic_type != current_topic_type: result = False else: self.call_subscriber()