diff --git a/examples/example_external_method/example_external_method/external_methods/factorial.py b/examples/example_external_method/example_external_method/external_methods/factorial.py index a8625bd6..d7135cf0 100644 --- a/examples/example_external_method/example_external_method/external_methods/factorial.py +++ b/examples/example_external_method/example_external_method/external_methods/factorial.py @@ -14,7 +14,7 @@ # # SPDX-License-Identifier: Apache-2.0 -def factorial(n: int): +def factorial(n: int): # pylint: disable=invalid-name fact = 1 for i in range(1, n+1): fact = fact * i diff --git a/libs/scenario_execution_nav2/scenario_execution_nav2/actions/nav_through_poses.py b/libs/scenario_execution_nav2/scenario_execution_nav2/actions/nav_through_poses.py index e2a14190..8312006f 100644 --- a/libs/scenario_execution_nav2/scenario_execution_nav2/actions/nav_through_poses.py +++ b/libs/scenario_execution_nav2/scenario_execution_nav2/actions/nav_through_poses.py @@ -32,7 +32,7 @@ def __init__(self, associated_actor, goal_poses: list, action_topic: str, namesp self.goal_poses = None super().__init__(self.namespace + '/' + action_topic, "nav2_msgs.action.NavigateThroughPoses", "") - def execute(self, associated_actor, goal_poses: list, action_topic: str, namespace_override: str) -> None: # pylint: disable=arguments-differ + def execute(self, associated_actor, goal_poses: list, action_topic: str, namespace_override: str) -> None: # pylint: disable=arguments-differ,arguments-renamed self.namespace = associated_actor["namespace"] if namespace_override: self.namespace = namespace_override diff --git a/libs/scenario_execution_nav2/scenario_execution_nav2/actions/nav_to_pose.py b/libs/scenario_execution_nav2/scenario_execution_nav2/actions/nav_to_pose.py index 91cad925..755198f2 100644 --- a/libs/scenario_execution_nav2/scenario_execution_nav2/actions/nav_to_pose.py +++ b/libs/scenario_execution_nav2/scenario_execution_nav2/actions/nav_to_pose.py @@ -33,7 +33,7 @@ def __init__(self, associated_actor, goal_pose: list, action_topic: str, namespa self.goal_pose = None super().__init__(self.namespace + '/' + action_topic, "nav2_msgs.action.NavigateToPose", "") - def execute(self, associated_actor, goal_pose: list, action_topic: str, namespace_override: str) -> None: # pylint: disable=arguments-differ + def execute(self, associated_actor, goal_pose: list, action_topic: str, namespace_override: str) -> None: # pylint: disable=arguments-differ,arguments-renamed self.namespace = associated_actor["namespace"] if namespace_override: self.namespace = namespace_override diff --git a/scenario_execution/scenario_execution/actions/base_action.py b/scenario_execution/scenario_execution/actions/base_action.py index 23f76253..c4e254fb 100644 --- a/scenario_execution/scenario_execution/actions/base_action.py +++ b/scenario_execution/scenario_execution/actions/base_action.py @@ -53,7 +53,7 @@ def initialise(self): if self._model.actor: final_args["associated_actor"] = self._model.actor.get_resolved_value(self.get_blackboard_client()) final_args["associated_actor"]["name"] = self._model.actor.name - self.execute(**final_args) + self.execute(**final_args) # pylint: disable=no-member def _set_base_properities(self, name, model, logger): self.name = name diff --git a/scenario_execution_ros/scenario_execution_ros/actions/ros_action_call.py b/scenario_execution_ros/scenario_execution_ros/actions/ros_action_call.py index 7e82e5ce..b906f537 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/ros_action_call.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/ros_action_call.py @@ -20,6 +20,7 @@ from rclpy.node import Node from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.action import ActionClient +from rclpy.qos import QoSProfile, DurabilityPolicy from rosidl_runtime_py.set_message import set_message_fields import py_trees # pylint: disable=import-error from action_msgs.msg import GoalStatus @@ -42,7 +43,7 @@ class RosActionCall(BaseAction): ros service call behavior """ - def __init__(self, action_name: str, action_type: str, data: str): + def __init__(self, action_name: str, action_type: str, data: str, transient_local: bool = False): super().__init__() self.node = None self.client = None @@ -56,6 +57,7 @@ def __init__(self, action_name: str, action_type: str, data: str): self.parse_data(data) self.current_state = ActionCallActionState.IDLE self.cb_group = ReentrantCallbackGroup() + self.transient_local = transient_local def setup(self, **kwargs): """ @@ -77,10 +79,19 @@ def setup(self, **kwargs): except ValueError as e: raise ValueError(f"Invalid action_type '{self.action_type}':") from e - self.client = ActionClient(self.node, self.action_type, self.action_name, callback_group=self.cb_group) + client_kwargs = { + "callback_group": self.cb_group, + } - def execute(self, action_name: str, action_type: str, data: str): - if self.action_name != action_name or self.action_type_string != action_type: + if self.transient_local: + qos_profile = QoSProfile(depth=1) + qos_profile.durability = DurabilityPolicy.TRANSIENT_LOCAL + client_kwargs["result_service_qos_profile"] = qos_profile + + self.client = ActionClient(self.node, self.action_type, self.action_name, **client_kwargs) + + def execute(self, action_name: str, action_type: str, data: str, transient_local: bool = False): + if self.action_name != action_name or self.action_type_string != action_type or self.transient_local != transient_local: raise ValueError(f"Updating action_name or action_type_string not supported.") self.parse_data(data) diff --git a/scenario_execution_ros/scenario_execution_ros/actions/ros_service_call.py b/scenario_execution_ros/scenario_execution_ros/actions/ros_service_call.py index f22e6428..7a3b5af8 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/ros_service_call.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/ros_service_call.py @@ -19,6 +19,7 @@ from enum import Enum from rclpy.node import Node from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.qos import QoSProfile, DurabilityPolicy from rosidl_runtime_py.set_message import set_message_fields import py_trees # pylint: disable=import-error from scenario_execution.actions.base_action import BaseAction @@ -39,7 +40,7 @@ class RosServiceCall(BaseAction): ros service call behavior """ - def __init__(self, service_name: str, service_type: str, data: str): + def __init__(self, service_name: str, service_type: str, data: str, transient_local: bool = False): super().__init__() self.node = None self.client = None @@ -55,6 +56,7 @@ def __init__(self, service_name: str, service_type: str, data: str): raise ValueError(f"Error while parsing sevice call data:") from e self.current_state = ServiceCallActionState.IDLE self.cb_group = ReentrantCallbackGroup() + self.transient_local = transient_local def setup(self, **kwargs): """ @@ -76,11 +78,23 @@ def setup(self, **kwargs): except ValueError as e: raise ValueError(f"Invalid service_type '{self.service_type}':") from e + client_kwargs = { + "callback_group": self.cb_group, + } + + if self.transient_local: + qos_profile = QoSProfile(depth=1) + qos_profile.durability = DurabilityPolicy.TRANSIENT_LOCAL + client_kwargs["qos_profile"] = qos_profile + self.client = self.node.create_client( - self.service_type, self.service_name, callback_group=self.cb_group) + self.service_type, + self.service_name, + **client_kwargs + ) - def execute(self, service_name: str, service_type: str, data: str): - if self.service_name != service_name or self.service_type_str != service_type or self.data_str != data: + def execute(self, service_name: str, service_type: str, data: str, transient_local: bool): + if self.service_name != service_name or self.service_type_str != service_type or self.data_str != data or self.transient_local != transient_local: raise ValueError("service_name, service_type and data arguments are not changeable during runtime.") self.current_state = ServiceCallActionState.IDLE 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 548c6a79..9ad70ab3 100644 --- a/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc +++ b/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc @@ -32,6 +32,7 @@ action action_call: action_name: string # name of the action to connect to action_type: string # class of the message type data: string # call content + transient_local: bool = false action assert_lifecycle_state: # Checks for the state of a lifecycle-managed node. @@ -115,6 +116,7 @@ action service_call: service_name: string # name of the service to connect to service_type: string # class of the message type (e.g. std_srvs.msg.Empty) data: string # call content + transient_local: bool = false action set_node_parameter: # Set a parameter of a node. diff --git a/test/scenario_execution_ros_test/scenario_execution_ros_test/workload_node.py b/test/scenario_execution_ros_test/scenario_execution_ros_test/workload_node.py index e0bfe8e3..6044514c 100644 --- a/test/scenario_execution_ros_test/scenario_execution_ros_test/workload_node.py +++ b/test/scenario_execution_ros_test/scenario_execution_ros_test/workload_node.py @@ -58,7 +58,7 @@ def main(args=None): rclpy.spin(node) except SystemExit: pass - except BaseException: # pylint: disable=broad-exception-caught + except BaseException: # pylint: disable=broad-except result = False if result: diff --git a/test/scenario_execution_test/scenario_execution_test/external_methods/external_methods.py b/test/scenario_execution_test/scenario_execution_test/external_methods/external_methods.py index bfe4f63b..3cea8723 100644 --- a/test/scenario_execution_test/scenario_execution_test/external_methods/external_methods.py +++ b/test/scenario_execution_test/scenario_execution_test/external_methods/external_methods.py @@ -14,8 +14,8 @@ # # SPDX-License-Identifier: Apache-2.0 -def test(n: int, text: str): - if n == 99: +def test(n: int, text: str): # pylint: disable=invalid-name + if n == 99: # pylint: disable=invalid-name raise ValueError("External Function Error!") return text diff --git a/tools/scenario_status/scenario_status/scenario_status_node.py b/tools/scenario_status/scenario_status/scenario_status_node.py index a0bfcbf7..dec1b19b 100644 --- a/tools/scenario_status/scenario_status/scenario_status_node.py +++ b/tools/scenario_status/scenario_status/scenario_status_node.py @@ -29,7 +29,7 @@ class ScenarioStatus(Node): - """Simple node subscribing to the py-trees-behaviour tree snapshot. The output is a + """Simple node subscribing to the py-trees-behaviour tree snapshot. The output is a string that describes any behavior state changes and timestamps.""" def __init__(self):