From ad286a6b3aaa5341bbc536f624ac4bcd46348faa Mon Sep 17 00:00:00 2001 From: David Conner Date: Tue, 2 Jul 2024 11:55:06 -0400 Subject: [PATCH] Add option to globally set desired update rate by calling set_default_rate from behavior --- flexbe_core/flexbe_core/core/ros_state.py | 34 ++++++++++++++++++++--- 1 file changed, 30 insertions(+), 4 deletions(-) diff --git a/flexbe_core/flexbe_core/core/ros_state.py b/flexbe_core/flexbe_core/core/ros_state.py index bfbafcf..b0e445e 100644 --- a/flexbe_core/flexbe_core/core/ros_state.py +++ b/flexbe_core/flexbe_core/core/ros_state.py @@ -42,8 +42,9 @@ class RosState(State): """A state to interface with ROS.""" - _node = None _breakpoints = None + _default_rate_hz = 10.0 # Default best effort update rate + _node = None @staticmethod def initialize_ros(node): @@ -65,12 +66,12 @@ def __init__(self, *args, **kwargs): """Initialize RosState instance.""" super().__init__(*args, **kwargs) - self._desired_period_ns = (1 / 10) * 1e9 - if 'desired_rate' in kwargs: Logger.localinfo('RosState: Set desired state update ' f"rate to {kwargs['desired_rate']} Hz.") - self._desired_period_ns = (1 / kwargs['desired_rate']) * 1e9 + self.set_rate(kwargs['desired_rate']) + else: + self.set_rate(RosState._default_rate_hz) self._is_controlled = False @@ -103,6 +104,31 @@ def set_rate(self, desired_rate): """ self._desired_period_ns = (1 / desired_rate) * 1e9 + @classmethod + def set_default_rate(cls, desired_rate): + """ + Set the desired best effort execution rate of all states. + + i.e., the rate with which the execute method is being called. + + Note: The rate is best-effort, real-time support is not yet available. + + This must be called at behavior level PRIOR to any states being created. + Typically, add import to behavior MANUAL_IMPORT section + from flexbe_core.core import RosState + The, in the MANUAL_INIT section + RosState.set_default_rate(5.0) + + Note: This will change the default update rate for any states created afterwards, + so beware when importing sub-behaviors with different rates defined! + + @type desired_rate: float + @param desired_rate: The desired rate in Hz. + """ + cls._default_rate_hz = desired_rate + Logger.localinfo('RosState: Set the default state update ' + f'rate for behavior to {desired_rate} Hz.') + def _enable_ros_control(self): self._is_controlled = True