Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

update assert topic latency #211

Merged
merged 4 commits into from
Oct 17, 2024
Merged

update assert topic latency #211

Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
35ff68b
add param type_check
Nikhil-Singhal-06 Oct 15, 2024
67a5584
fix
Nikhil-Singhal-06 Oct 17, 2024
3d38ed4
fix docs
Nikhil-Singhal-06 Oct 17, 2024
97eac1a
fix
Nikhil-Singhal-06 Oct 17, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
65 changes: 34 additions & 31 deletions scenario_execution_ros/scenario_execution_ros/actions/assert_topic_latency.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,10 +18,10 @@
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):
Expand Down Expand Up @@ -112,45 +112,48 @@ 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.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(".")
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
Expand Down