diff --git a/.flake8 b/.flake8 index 7fc980c..ab05505 100644 --- a/.flake8 +++ b/.flake8 @@ -1,3 +1,5 @@ [flake8] exclude = .git max-line-length = 130 +ignore = W503, D100, D105, D107 # Ignore docstring requirements for public modules, magic methods, and __init__ functions. +# For W503 - https://www.flake8rules.com/rules/W503.html and https://peps.python.org/pep-0008/#should-a-line-break-before-or-after-a-binary-operator diff --git a/LICENSE b/LICENSE index 2321541..b646152 100644 --- a/LICENSE +++ b/LICENSE @@ -1,4 +1,4 @@ -Copyright 2015-2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +Copyright 2015-2024 Philipp Schillinger, Team ViGIR, Christopher Newport University BSD-3-Clause @@ -12,7 +12,7 @@ modification, are permitted provided that the following conditions are met: notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. - * Neither the name of the Philipp Schillinger, Team ViGIR, + * Neither the name of the Philipp Schillinger, Team ViGIR, Christopher Newport University nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. diff --git a/flexbe_core/flexbe_core/__init__.py b/flexbe_core/flexbe_core/__init__.py index f59d542..32ef254 100644 --- a/flexbe_core/flexbe_core/__init__.py +++ b/flexbe_core/flexbe_core/__init__.py @@ -1,4 +1,4 @@ -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -27,7 +27,7 @@ # POSSIBILITY OF SUCH DAMAGE. """ -Initialization for flexbe_core.core module. +Initialization for flexbe_core package. Please use EventState as parent class for new states because it extends all other parent classes. @@ -35,13 +35,11 @@ For a behavior, inherit from OperatableStateMachine as state machine. """ -from .core import ConcurrencyContainer, EventState # noqa: F401 -from .core import OperatableStateMachine, PriorityContainer # noqa: F401 from .behavior import Behavior # noqa: F401 - from .behavior_library import BehaviorLibrary # noqa: F401 - +from .core import ConcurrencyContainer, EventState # noqa: F401 +from .core import OperatableStateMachine, PriorityContainer # noqa: F401 from .logger import Logger # noqa: F401 from .state_logger import StateLogger # noqa: F401 diff --git a/flexbe_core/flexbe_core/behavior.py b/flexbe_core/flexbe_core/behavior.py index f35f416..b1674c4 100644 --- a/flexbe_core/flexbe_core/behavior.py +++ b/flexbe_core/flexbe_core/behavior.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -30,10 +30,11 @@ """This defines the superclass for all implemented behaviors.""" -from flexbe_msgs.msg import BehaviorSync -from flexbe_core.core import PreemptableState, OperatableStateMachine, LockableStateMachine +from flexbe_core.core import LockableStateMachine, OperatableStateMachine, PreemptableState from flexbe_core.logger import Logger +from flexbe_msgs.msg import BehaviorSync + class Behavior: """This is the superclass for all implemented behaviors.""" @@ -41,7 +42,7 @@ class Behavior: def __init__(self): """Call this superclass constructor first when overriding it with your behavior.""" self._state_machine = None - self.name = "unnamed behavior" + self.name = 'unnamed behavior' self.beh_id = 0 # Behavior id checksum assigned by processing the file contents self.contains = {} @@ -216,7 +217,7 @@ def prepare_for_switch(self, state): state._locked = True # make sure the state cannot transition during preparations states = self._get_states_of_path(state.path, self._state_machine) if states is None: - raise RuntimeError("Did not find locked state in new behavior!") + raise RuntimeError('Did not find locked state in new behavior!') state_container = state._parent state_container.remove_state(state) # remove from old state machine for sm in states[1:]: @@ -227,6 +228,7 @@ def prepare_for_switch(self, state): self.requested_state_path = state.path # set start after switch def get_current_states(self): + """Get all currently active (sub-)states.""" return self._state_machine.get_deep_states() def get_locked_state(self): @@ -242,6 +244,7 @@ def get_locked_state(self): @classmethod def preempt(cls): + """Preempt behavior.""" PreemptableState.preempt = True # For internal use only @@ -252,7 +255,7 @@ def _get_state_machine(self): return self._state_machine def _collect_contained(self, obj, path): - contain_list = {path + "/" + key: value for (key, value) in getattr(obj, 'contains', {}).items()} + contain_list = {path + '/' + key: value for (key, value) in getattr(obj, 'contains', {}).items()} add_to_list = {} for b_id, b_inst in contain_list.items(): add_to_list.update(self._collect_contained(b_inst, b_id)) @@ -260,6 +263,7 @@ def _collect_contained(self, obj, path): return contain_list def get_contained_behaviors(self): + """Get contained behaviors within this behavior.""" return self._collect_contained(self, '') def _set_typed_attribute(self, name, value): @@ -271,13 +275,14 @@ def _set_typed_attribute(self, name, value): elif isinstance(attr, float): value = float(value) elif isinstance(attr, bool): - value = (value != "0" and value.lower() != "false") + value = (value != '0' and value.lower() != 'false') elif isinstance(attr, dict): import yaml # pylint: disable=C0415 value = getattr(yaml, 'unsafe_load', yaml.load)(value) setattr(self, name, value) def set_up(self, beh_id, autonomy_level, debug): + """Set up the behavior.""" self.beh_id = beh_id self._autonomy_level = autonomy_level self._debug = debug @@ -287,7 +292,7 @@ def _get_states_of_path(self, path, container): if len(path_elements) < 2: return [container] # actually a state in this case state_label = path_elements[1] - new_path = "/".join(path_elements[1:]) + new_path = '/'.join(path_elements[1:]) # collect along the path and append self if state_label in container: childlist = self._get_states_of_path(new_path, container[state_label]) diff --git a/flexbe_core/flexbe_core/behavior_library.py b/flexbe_core/flexbe_core/behavior_library.py index 963ea69..8cf2c13 100644 --- a/flexbe_core/flexbe_core/behavior_library.py +++ b/flexbe_core/flexbe_core/behavior_library.py @@ -1,4 +1,4 @@ -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -33,6 +33,7 @@ import zlib from ament_index_python import get_packages_with_prefixes + from catkin_pkg.package import parse_package from flexbe_core.logger import Logger @@ -42,6 +43,7 @@ class BehaviorLibrary: """Provide access to all known behaviors.""" def __init__(self, node): + """Initialize BehaviorLibrary instance.""" self._node = node Logger.initialize(node) self.parse_packages() @@ -49,13 +51,13 @@ def __init__(self, node): def dump_packages(self): """Dump to screen for debugging.""" - print("\n\n------------------ Available Behaviors Workspace (sorted by assigned key) ------------------", flush=True) + print('\n\n------------------ Available Behaviors Workspace (sorted by assigned key) ------------------', flush=True) sorted_keys = list(self._behavior_lib.keys()) sorted_keys.sort() for be_key in sorted_keys: data = self._behavior_lib[be_key] print(f"{be_key:10d} : {data['class']:36s} - {data['name']:36s} - {data['file']:30s} - {data['package']}") - print("-----------------------------------------------------------------------------------------\n\n", flush=True) + print('-----------------------------------------------------------------------------------------\n\n', flush=True) def parse_packages(self): """Parse all ROS2 packages to update the internal behavior library.""" @@ -63,11 +65,11 @@ def parse_packages(self): for pkg_name, pkg_path in get_packages_with_prefixes().items(): pkg = parse_package(os.path.join(pkg_path, 'share', pkg_name)) for export in pkg.exports: - if export.tagname == "flexbe_behaviors": + if export.tagname == 'flexbe_behaviors': try: self._add_behavior_manifests(os.path.join(pkg_path, 'lib', pkg_name, 'manifest'), pkg_name) except KeyError as exc: - print(f"Error : duplicate behavior name found in {pkg_name} \n {exc}", flush=True) + print(f"Error : duplicate behavior name found in '{pkg_name}' \n {exc}", flush=True) raise exc def _add_behavior_manifests(self, path, pkg=None): @@ -86,7 +88,7 @@ def _add_behavior_manifests(self, path, pkg=None): entry_path = os.path.join(path, entry) if os.path.isdir(entry_path): self._add_behavior_manifests(entry_path, pkg) - elif entry.endswith(".xml") and not entry.startswith("#"): + elif entry.endswith('.xml') and not entry.startswith('#'): try: mrt = ET.parse(entry_path).getroot() except ET.ParseError as exc: @@ -95,24 +97,26 @@ def _add_behavior_manifests(self, path, pkg=None): continue # structure sanity check - if (mrt.tag != "behavior" - or len(mrt.findall(".//executable")) == 0 - or mrt.find("executable").get("package_path") is None - or len(mrt.find("executable").get("package_path").split(".")) < 2): + if ( + mrt.tag != 'behavior' + or len(mrt.findall('.//executable')) == 0 + or mrt.find('executable').get('package_path') is None + or len(mrt.find('executable').get('package_path').split('.')) < 2 + ): continue - exct = mrt.find("executable") - if pkg is not None and exct.get("package_path").split(".")[0] != pkg: + exct = mrt.find('executable') + if pkg is not None and exct.get('package_path').split('.')[0] != pkg: continue # ignore if manifest not in specified package - be_key = zlib.adler32(exct.get("package_path").encode()) & 0x7fffffff + be_key = zlib.adler32(exct.get('package_path').encode()) & 0x7fffffff if be_key in self._behavior_lib: - raise KeyError(f"Invalid behavior id key={be_key} for {exct.get('package_path')} " + raise KeyError(f"Invalid behavior id key='{be_key}' for {exct.get('package_path')} " f"- already exists for {self._behavior_lib[be_key]['package']}") self._behavior_lib[be_key] = { - "name": mrt.get("name"), - "package": ".".join(exct.get("package_path").split(".")[:-1]), - "file": exct.get("package_path").split(".")[-1], - "class": exct.get("class") + 'name': mrt.get('name'), + 'package': '.'.join(exct.get('package_path').split('.')[:-1]), + 'file': exct.get('package_path').split('.')[-1], + 'class': exct.get('class') } def get_behavior(self, be_key): @@ -127,7 +131,7 @@ def get_behavior(self, be_key): try: return self._behavior_lib[be_key] except KeyError: - Logger.logwarn(f"Did not find ID {be_key} in libary, updating...") + Logger.logwarn(f"Did not find ID '{be_key}' in libary, updating...") self.parse_packages() return self._behavior_lib.get(be_key, None) @@ -140,25 +144,25 @@ def find_behavior(self, be_identifier): @return Tuple (be_key, be_entry) corresponding to the name or (None, None) if not found. """ - if "/" in be_identifier: + if '/' in be_identifier: # Identifier in the form of package/Name # where only first slash delineates package - be_split = be_identifier.split("/") - be_package, be_name = be_split[0], "/".join(be_split[1:]) + be_split = be_identifier.split('/') + be_package, be_name = be_split[0], '/'.join(be_split[1:]) def __find_behavior(): return next((id, be) for (id, be) in self._behavior_lib.items() - if be["name"] == be_name and be["package"] == be_package) + if be['name'] == be_name and be['package'] == be_package) else: # Accept older form of only Name, but this will return the first matching name! be_package = None be_name = be_identifier def __find_behavior(): - return next((id, be) for (id, be) + return next((beh_id, be) for (beh_id, be) in self._behavior_lib.items() - if be["name"] == be_name) # Returns first matching name regardless of package + if be['name'] == be_name) # Returns first matching name regardless of package try: return __find_behavior() except StopIteration: @@ -196,7 +200,7 @@ def get_sourcecode_filepath(self, be_key, add_tmp=False): return None try: - module_path = __import__(be_entry["package"]).__path__[-1] + module_path = __import__(be_entry['package']).__path__[-1] except ImportError: try: # Attempt to replace prior use of ROS 1 package finder @@ -204,10 +208,10 @@ def get_sourcecode_filepath(self, be_key, add_tmp=False): f"""try using 'get_package_share_directory' instead""") from ament_index_python.packages import get_package_share_directory # pylint: disable=C0415 - module_path = get_package_share_directory(be_entry["package"]) + module_path = get_package_share_directory(be_entry['package']) except Exception as exc: Logger.logerr(f"""Cannot import behavior package '{be_entry["package"]}' """) raise exc - filename = be_entry["file"] + '.py' if not add_tmp else '_tmp.py' + filename = be_entry['file'] + '.py' if not add_tmp else '_tmp.py' return os.path.join(module_path, filename) diff --git a/flexbe_core/flexbe_core/core/__init__.py b/flexbe_core/flexbe_core/core/__init__.py index de9d56f..0772e08 100644 --- a/flexbe_core/flexbe_core/core/__init__.py +++ b/flexbe_core/flexbe_core/core/__init__.py @@ -1,4 +1,4 @@ -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -27,25 +27,22 @@ # POSSIBILITY OF SUCH DAMAGE. -"""Initialize of flexbe_core.core module.""" - -from .preemptable_state_machine import PreemptableStateMachine # noqa: F401 -from .operatable_state_machine import OperatableStateMachine # noqa: F401 -from .lockable_state_machine import LockableStateMachine # noqa: F401 -from .ros_state_machine import RosStateMachine # noqa: F401 -from .state_machine import StateMachine # noqa: F401 +"""Initialize of flexbe_core.core package.""" from .concurrency_container import ConcurrencyContainer # noqa: F401 -from .priority_container import PriorityContainer # noqa: F401 - -from .state import State # noqa: F401 -from .ros_state import RosState # noqa: F401 -from .manually_transitionable_state import ManuallyTransitionableState # noqa: F401 +from .event_state import EventState # noqa: F401 from .lockable_state import LockableState # noqa: F401 -from .preemptable_state import PreemptableState # noqa: F401 +from .lockable_state_machine import LockableStateMachine # noqa: F401 +from .manually_transitionable_state import ManuallyTransitionableState # noqa: F401 from .operatable_state import OperatableState # noqa: F401 -from .event_state import EventState # noqa: F401 - +from .operatable_state_machine import OperatableStateMachine # noqa: F401 +from .preemptable_state import PreemptableState # noqa: F401 +from .preemptable_state_machine import PreemptableStateMachine # noqa: F401 +from .priority_container import PriorityContainer # noqa: F401 +from .ros_state import RosState # noqa: F401 +from .ros_state_machine import RosStateMachine # noqa: F401 +from .state import State # noqa: F401 +from .state_machine import StateMachine # noqa: F401 from .state_map import StateMap # noqa: F401 from .user_data import UserData # noqa: F401 diff --git a/flexbe_core/flexbe_core/core/concurrency_container.py b/flexbe_core/flexbe_core/core/concurrency_container.py index 00f7a67..190da85 100644 --- a/flexbe_core/flexbe_core/core/concurrency_container.py +++ b/flexbe_core/flexbe_core/core/concurrency_container.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -36,7 +36,6 @@ """ from flexbe_core.core.event_state import EventState from flexbe_core.core.lockable_state_machine import LockableStateMachine -from flexbe_core.core.operatable_state import OperatableState from flexbe_core.core.operatable_state_machine import OperatableStateMachine from flexbe_core.core.priority_container import PriorityContainer from flexbe_core.core.topics import Topics @@ -65,7 +64,7 @@ def __init__(self, conditions=None, *args, **kwargs): @property def sleep_duration(self): """Sleep duration in seconds.""" - sleep_dur = float("inf") + sleep_dur = float('inf') for state in self._states: sleep_dur = min(sleep_dur, state.sleep_duration) @@ -82,23 +81,25 @@ def current_state_label(self): return self.name def get_required_autonomy(self, outcome, state): + """Return required autonomy level for this outcome.""" try: assert state in self._current_state, "get required autonomy in ConcurrencyContainer - state doesn't match!" return self._autonomy[state.name][outcome] except Exception as exc: Logger.error(f"Failure to retrieve autonomy for '{self.name}' in CC - " f" current state label='{self.name}' state='{state.name}' outcome='{outcome}'.") - Logger.localerr(f"{type(exc)} - {exc}\n\n {self._current_state}") - Logger.localerr(f"{self._autonomy}") + Logger.localerr(f'{type(exc)} - {exc}\n\n {self._current_state}') + Logger.localerr(f'{self._autonomy}') def _execute_current_state(self): + """Execute the current states within this concurrency container.""" # execute all states that are done with sleeping and determine next sleep duration self._inner_sync_request = False # clear prior request for lower level state self._current_state = [] # Concurrency container has multiple active states so use list self._manual_transition_requested = None - # Logger.localinfo(f"-concurrency container {self.name} is_controlled={self._is_controlled}" - # f" has transition request={self._sub.has_buffered(Topics._CMD_TRANSITION_TOPIC)}") + # Logger.localinfo(f"-concurrency container '{self.name}' is_controlled={self._is_controlled}" + # f' has transition request={self._sub.has_buffered(Topics._CMD_TRANSITION_TOPIC)}') if self._is_controlled and self._sub.has_buffered(Topics._CMD_TRANSITION_TOPIC): # Special handling in concurrency container - can be either CC or one of several internal states. command_msg = self._sub.get_from_buffer(Topics._CMD_TRANSITION_TOPIC) @@ -108,18 +109,20 @@ def _execute_current_state(self): outcome = self.outcomes[command_msg.outcome] self._manual_transition_requested = outcome self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, - CommandFeedback(command="transition", + CommandFeedback(command='transition', args=[command_msg.target, self.name])) - Logger.localwarn(f"--> Manually triggered outcome {outcome} of concurrency container {self.name}") + Logger.localwarn(f"--> Manually triggered outcome {outcome} of concurrency container '{self.name}'") self.on_exit(self.userdata, states=[s for s in self._states if (s.name not in self._returned_outcomes - or self._returned_outcomes[s.name] is None)]) + or self._returned_outcomes[s.name] is None) + ] + ) self._returned_outcomes = {} self._current_state = None self._last_outcome = outcome return outcome else: - Logger.localinfo(f"concurrency container {self.name} ") + Logger.localinfo(f"concurrency container '{self.name}' ") self._manual_transition_requested = command_msg for state in self._states: @@ -145,14 +148,14 @@ def _execute_current_state(self): state._publish_outcome(outcome) self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, - CommandFeedback(command="transition", + CommandFeedback(command='transition', args=[command_msg.target, state.name])) - Logger.localerr(f"--> Manually triggered outcome {outcome} ({command_msg.outcome}) " - f"of state {state.name} from inside concurrency {self.name}") + Logger.localerr(f'--> Manually triggered outcome {outcome} ({command_msg.outcome}) ' + f"of state '{state.name}' from inside concurrency {self.name}") continue else: - Logger.localerr(f"--> Invalid outcome {command_msg.outcome} request for state {state.name} " - f"from inside concurrency {self.name}\n{state.outcomes}") + Logger.localerr(f"--> Invalid outcome {command_msg.outcome} request for state '{state.name}' " + f"from inside concurrency '{self.name}'\n{state.outcomes}") if (PriorityContainer.active_container is not None and not all(a == s for a, s in zip(PriorityContainer.active_container.split('/'), @@ -219,7 +222,7 @@ def _execute_current_state(self): self._pub.publish(Topics._OUTCOME_REQUEST_TOPIC, OutcomeRequest(outcome=self.outcomes.index(outcome), target=self.path)) - Logger.localinfo("<-- Want result: %s > %s" % (self.name, outcome)) + Logger.localinfo('<-- Want result: %s > %s' % (self.name, outcome)) StateLogger.log('flexbe.operator', self, type='request', request=outcome, autonomy=self.parent.autonomy_level, required=self.parent.get_required_autonomy(outcome, self)) @@ -235,6 +238,7 @@ def _execute_current_state(self): return outcome def _execute_single_state(self, state, force_exit=False): + """Execute the next state in concurrent container.""" result = None try: with UserData(reference=self._userdata, remap=self._remappings[state.name], @@ -250,11 +254,12 @@ def _execute_single_state(self, state, force_exit=False): self._last_exception = exc Logger.logerr('ConcurrencyContainer: Failed to execute state %s:\n%s' % (self.current_state_label, str(exc))) import traceback # pylint: disable=C0415 - Logger.localinfo(traceback.format_exc().replace("%", "%%")) + Logger.localinfo(traceback.format_exc().replace('%', '%%')) return result def on_enter(self, userdata): # pylint: disable=W0613 + """Call on entering the concurrency container.""" super().on_enter(userdata) for state in self._states: # Force on_enter at state level (userdata passed by _execute_single_state) @@ -262,6 +267,7 @@ def on_enter(self, userdata): # pylint: disable=W0613 state._last_execution = None def on_exit(self, userdata, states=None): + """Call when concurrency container exits.""" for state in self._states if states is None else states: if state in self._returned_outcomes: continue # skip states that already exited themselves @@ -289,8 +295,8 @@ def get_deep_states(self): # Logger.localinfo(f"Concurrent get_deep_states: {self.name} {[state.path for state in deep_states]}") return deep_states elif self._current_state is not None: - Logger.localerr(f"ConcurrentContainer.get_deep_states {self.name} - current state is NOT a list!") - raise TypeError(f"ConcurrentContainer.get_deep_states {self.name} - current state is NOT a list!") + Logger.localerr(f"ConcurrentContainer.get_deep_states '{self.name}' - current state is NOT a list!") + raise TypeError(f"ConcurrentContainer.get_deep_states '{self.name}' - current state is NOT a list!") # Otherwise, either haven't fully entered, or all have returned outcomes return deep_states diff --git a/flexbe_core/flexbe_core/core/event_state.py b/flexbe_core/flexbe_core/core/event_state.py index 6c41790..f8e27a5 100644 --- a/flexbe_core/flexbe_core/core/event_state.py +++ b/flexbe_core/flexbe_core/core/event_state.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -30,16 +30,18 @@ """EventState.""" -from flexbe_msgs.msg import CommandFeedback -from std_msgs.msg import Bool, Empty +from flexbe_core.core.operatable_state import OperatableState from flexbe_core.core.preemptable_state import PreemptableState from flexbe_core.core.priority_container import PriorityContainer -from flexbe_core.core.operatable_state import OperatableState from flexbe_core.core.topics import Topics from flexbe_core.logger import Logger from flexbe_core.state_logger import StateLogger +from flexbe_msgs.msg import CommandFeedback + +from std_msgs.msg import Bool, Empty + @StateLogger.log_events('flexbe.events', start='on_start', stop='on_stop', @@ -50,6 +52,7 @@ class EventState(OperatableState): """A state that allows implementing certain events.""" def __init__(self, *args, **kwargs): + """Initialize EventState instance.""" super().__init__(*args, **kwargs) self.__execute = self.execute @@ -66,15 +69,15 @@ def _event_execute(self, *args, **kwargs): msg = self._sub.get_last_msg(Topics._CMD_PAUSE_TOPIC) self._sub.remove_last_msg(Topics._CMD_PAUSE_TOPIC) if msg.data: - Logger.localinfo("--> Pausing in state %s", self.name) - self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, CommandFeedback(command="pause")) + Logger.localinfo("--> Pausing in state '%s'", self.name) + self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, CommandFeedback(command='pause')) self._last_active_container = PriorityContainer.active_container # claim priority to propagate pause event PriorityContainer.active_container = self.path self._paused = True else: - Logger.localinfo("--> Resuming in state %s", self.name) - self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, CommandFeedback(command="resume")) + Logger.localinfo("--> Resuming in state '%s'", self.name) + self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, CommandFeedback(command='resume')) PriorityContainer.active_container = self._last_active_container self._last_active_container = None self._paused = False @@ -97,9 +100,9 @@ def _event_execute(self, *args, **kwargs): repeat = False if self._is_controlled and self._sub.has_msg(Topics._CMD_REPEAT_TOPIC): - Logger.localinfo("--> Repeating state %s", self.name) + Logger.localinfo("--> Repeating state '%s'", self.name) self._sub.remove_last_msg(Topics._CMD_REPEAT_TOPIC) - self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, CommandFeedback(command="repeat")) + self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, CommandFeedback(command='repeat')) repeat = True if repeat or outcome is not None and not PreemptableState.preempt: diff --git a/flexbe_core/flexbe_core/core/exceptions.py b/flexbe_core/flexbe_core/core/exceptions.py index 23a9c5e..12a0448 100644 --- a/flexbe_core/flexbe_core/core/exceptions.py +++ b/flexbe_core/flexbe_core/core/exceptions.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: diff --git a/flexbe_core/flexbe_core/core/lockable_state.py b/flexbe_core/flexbe_core/core/lockable_state.py index a108547..2b752ae 100644 --- a/flexbe_core/flexbe_core/core/lockable_state.py +++ b/flexbe_core/flexbe_core/core/lockable_state.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -30,13 +30,14 @@ """Implement LockableState that can prevent transition.""" -from std_msgs.msg import String - -from flexbe_msgs.msg import CommandFeedback -from flexbe_core.logger import Logger from flexbe_core.core.manually_transitionable_state import ManuallyTransitionableState from flexbe_core.core.topics import Topics +from flexbe_core.logger import Logger + +from flexbe_msgs.msg import CommandFeedback + +from std_msgs.msg import String class LockableState(ManuallyTransitionableState): @@ -49,6 +50,7 @@ class LockableState(ManuallyTransitionableState): """ def __init__(self, *args, **kwargs): + """Initialize LockableState instance.""" super().__init__(*args, **kwargs) self.__execute = self.execute self.execute = self._lockable_execute @@ -57,6 +59,7 @@ def __init__(self, *args, **kwargs): self._stored_outcome = None def _lockable_execute(self, *args, **kwargs): + """Execute lockable portion of state.""" if self._is_controlled and self._sub.has_msg(Topics._CMD_LOCK_TOPIC): msg = self._sub.get_last_msg(Topics._CMD_LOCK_TOPIC) self._sub.remove_last_msg(Topics._CMD_LOCK_TOPIC) @@ -95,6 +98,7 @@ def _lockable_execute(self, *args, **kwargs): return outcome def _execute_lock(self, target): + """Execute lock.""" if target in (self.path, ''): target = self.path found_target = True @@ -103,11 +107,11 @@ def _execute_lock(self, target): found_target = self._parent.lock(target) # provide feedback about lock self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, - CommandFeedback(command="lock", args=[target, target if found_target else ""])) + CommandFeedback(command='lock', args=[target, target if found_target else ''])) if not found_target: - Logger.logwarn(f"Wanted to lock {target}, but could not find it in current path {self.path}.") + Logger.logwarn(f"Wanted to lock '{target}', but could not find it in current path '{self.path}'.") else: - Logger.localinfo(f"--> Locking in state {target}") + Logger.localinfo(f"--> Locking in state '{target}'") def _execute_unlock(self, target): if target == self.path or (self._locked and target == ''): @@ -118,11 +122,11 @@ def _execute_unlock(self, target): found_target = self._parent.unlock(target) # provide feedback about unlock self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, - CommandFeedback(command="unlock", args=[target, target if found_target else ""])) + CommandFeedback(command='unlock', args=[target, target if found_target else ''])) if not found_target: - Logger.logwarn(f"Wanted to unlock {target}, but could not find it in current path {self.path}.") + Logger.logwarn(f"Wanted to unlock '{target}', but could not find it in current path '{self.path}'.") else: - Logger.localinfo(f"--> Unlocking in state {target}") + Logger.localinfo(f"--> Unlocking in state '{target}'") def _enable_ros_control(self): if not self._is_controlled: @@ -139,4 +143,5 @@ def _disable_ros_control(self): self._sub.unsubscribe_topic(Topics._CMD_UNLOCK_TOPIC, inst_id=id(self)) def is_locked(self): + """Check is locked.""" return self._locked diff --git a/flexbe_core/flexbe_core/core/lockable_state_machine.py b/flexbe_core/flexbe_core/core/lockable_state_machine.py index 1af9101..50ca269 100644 --- a/flexbe_core/flexbe_core/core/lockable_state_machine.py +++ b/flexbe_core/flexbe_core/core/lockable_state_machine.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -46,13 +46,16 @@ class LockableStateMachine(RosStateMachine): path_for_switch = None def __init__(self, *args, **kwargs): + """Initialize LockableStateMachine instance.""" super().__init__(*args, **kwargs) self._locked = False def _is_internal_transition(self, transition_target): + """Is a valid transition.""" return transition_target in self._labels def transition_allowed(self, state, outcome): + """Is transition allowed.""" if outcome is None or outcome == 'None': return True transition_target = self._transitions[state].get(outcome) @@ -64,9 +67,10 @@ def transition_allowed(self, state, outcome): # for switching def execute(self, userdata): + """Execute lockable SM logic.""" if (LockableStateMachine.path_for_switch is not None and LockableStateMachine.path_for_switch.startswith(self.path)): - path_segments = LockableStateMachine.path_for_switch.replace(self.path, "", 1).split("/") + path_segments = LockableStateMachine.path_for_switch.replace(self.path, '', 1).split('/') wanted_state = path_segments[1] self._current_state = self._labels[wanted_state] if len(path_segments) <= 2: @@ -74,21 +78,25 @@ def execute(self, userdata): return super().execute(userdata) def replace_userdata(self, userdata): + """Replace userdata.""" self._userdata = userdata def replace_state(self, state): + """Replace state (while locked).""" old_state = self._labels[state.name] state._parent = old_state._parent self._states[self._states.index(old_state)] = state self._labels[state.name] = state def remove_state(self, state): + """Remove state.""" del self._labels[state.name] self._states.remove(state) # for locking def lock(self, path): + """Lock this state.""" if path == self.path: self._locked = True return True @@ -99,6 +107,7 @@ def lock(self, path): return False def unlock(self, path): + """Unlock this state.""" if path == self.path: self._locked = False return True @@ -109,9 +118,11 @@ def unlock(self, path): return False def is_locked(self): + """Is this state locked.""" return self._locked def is_locked_inside(self): + """Is interior state locked.""" if self._locked: return True for state in self._states: @@ -125,6 +136,7 @@ def is_locked_inside(self): return False def get_locked_state(self): + """Get state that is locked inside.""" if self._locked: return self for state in self._states: diff --git a/flexbe_core/flexbe_core/core/manually_transitionable_state.py b/flexbe_core/flexbe_core/core/manually_transitionable_state.py index c64e29b..df134d1 100644 --- a/flexbe_core/flexbe_core/core/manually_transitionable_state.py +++ b/flexbe_core/flexbe_core/core/manually_transitionable_state.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -31,12 +31,12 @@ """ManuallyTransitionableState.""" -from flexbe_msgs.msg import CommandFeedback, OutcomeRequest - from flexbe_core.core.ros_state import RosState from flexbe_core.core.topics import Topics from flexbe_core.logger import Logger +from flexbe_msgs.msg import CommandFeedback, OutcomeRequest + class ManuallyTransitionableState(RosState): """ @@ -46,6 +46,7 @@ class ManuallyTransitionableState(RosState): """ def __init__(self, *args, **kwargs): + """Initialize ManuallyTransitionableState instance.""" super().__init__(*args, **kwargs) self.__execute = self.execute self.execute = self._manually_transitionable_execute @@ -58,15 +59,15 @@ def _manually_transitionable_execute(self, *args, **kwargs): if self._is_controlled and self._sub.has_buffered(Topics._CMD_TRANSITION_TOPIC): command_msg = self._sub.get_from_buffer(Topics._CMD_TRANSITION_TOPIC) self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, - CommandFeedback(command="transition", args=[command_msg.target, self.name])) + CommandFeedback(command='transition', args=[command_msg.target, self.name])) if command_msg.target != self.name: - Logger.logwarn("Requested outcome for state %s but active state is %s" % + Logger.logwarn("Requested outcome for state '%s' but active state is '%s'" % (command_msg.target, self.name)) else: self._force_transition = True outcome = self.outcomes[command_msg.outcome] self._manual_transition_requested = outcome - Logger.localinfo("--> Manually triggered outcome %s of state %s" % (outcome, self.name)) + Logger.localinfo("--> Manually triggered outcome '%s' of state '%s'" % (outcome, self.name)) return outcome # otherwise, return the normal outcome self._force_transition = False diff --git a/flexbe_core/flexbe_core/core/operatable_state.py b/flexbe_core/flexbe_core/core/operatable_state.py index 35c683f..357e70b 100644 --- a/flexbe_core/flexbe_core/core/operatable_state.py +++ b/flexbe_core/flexbe_core/core/operatable_state.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -31,16 +31,16 @@ """OperatableState.""" -from std_msgs.msg import UInt32, String - -from flexbe_msgs.msg import OutcomeRequest - from flexbe_core.core.preemptable_state import PreemptableState -from flexbe_core.core.topics import Topics from flexbe_core.core.state_map import StateMap +from flexbe_core.core.topics import Topics from flexbe_core.logger import Logger from flexbe_core.state_logger import StateLogger +from flexbe_msgs.msg import OutcomeRequest + +from std_msgs.msg import String, UInt32 + @StateLogger.log_outcomes('flexbe.outcomes') class OperatableState(PreemptableState): @@ -53,6 +53,7 @@ class OperatableState(PreemptableState): """ def __init__(self, *args, **kwargs): + """Initialize the OperatableState instance.""" super().__init__(*args, **kwargs) self.__execute = self.execute self.execute = self._operatable_execute @@ -74,7 +75,7 @@ def _operatable_execute(self, *args, **kwargs): if outcome != self._last_requested_outcome: self._pub.publish(Topics._OUTCOME_REQUEST_TOPIC, OutcomeRequest(outcome=self.outcomes.index(outcome), target=self.path)) - Logger.localinfo("<-- Want result: %s > %s" % (self.name, outcome)) + Logger.localinfo("<-- Want result: '%s' > '%s'" % (self.name, outcome)) StateLogger.log('flexbe.operator', self, type='request', request=outcome, autonomy=self.parent.autonomy_level, required=self.parent.get_required_autonomy(outcome, self)) @@ -91,9 +92,9 @@ def _publish_outcome(self, outcome): """Update the UI and logs about this outcome.""" # 0 outcome status denotes no outcome, not index so add +1 for valid outcome (subtract in mirror) outcome_index = self.outcomes.index(outcome) - Logger.localinfo(f"State result: {self.name} > {outcome}") + Logger.localinfo(f"State result: '{self.name}' -> '{outcome}'") self._pub.publish(Topics._OUTCOME_TOPIC, UInt32(data=StateMap.hash(self, outcome_index))) - self._pub.publish(Topics._DEBUG_TOPIC, String(data="%s > %s" % (self.path, outcome))) + self._pub.publish(Topics._DEBUG_TOPIC, String(data='%s > %s' % (self.path, outcome))) if self._force_transition: StateLogger.log('flexbe.operator', self, type='forced', forced=outcome, requested=self._last_requested_outcome) diff --git a/flexbe_core/flexbe_core/core/operatable_state_machine.py b/flexbe_core/flexbe_core/core/operatable_state_machine.py index cc1aef0..4eb000e 100644 --- a/flexbe_core/flexbe_core/core/operatable_state_machine.py +++ b/flexbe_core/flexbe_core/core/operatable_state_machine.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -32,8 +32,6 @@ """OperatableStateMachine.""" from enum import Enum -from std_msgs.msg import Empty, Int32, UInt32, UInt8, String - from flexbe_core.core.operatable_state import OperatableState from flexbe_core.core.preemptable_state_machine import PreemptableStateMachine from flexbe_core.core.ros_state import RosState @@ -45,6 +43,8 @@ from flexbe_msgs.msg import BehaviorSync, CommandFeedback, Container, ContainerStructure, OutcomeRequest +from std_msgs.msg import Empty, Int32, String, UInt32, UInt8 + class OperatableStateMachine(PreemptableStateMachine): """ @@ -64,6 +64,7 @@ class ContainerType(Enum): ConcurrencyContainer = 3 def __init__(self, *args, **kwargs): + """Initialize OperatableStateMachine instance.""" super().__init__(*args, **kwargs) self.id = None self._autonomy = {} @@ -152,8 +153,8 @@ def _add_to_structure_msg(self, structure_msg, state_map): state_msg.transitions = [self._transitions[state.name][outcome] for outcome in state.outcomes] state_msg.autonomy = [self._autonomy[state.name][outcome] for outcome in state.outcomes] except Exception as exc: # pylint: disable=W0703 - Logger.logerr(f"Error building container structure for {state.name}!") - Logger.localerr(f"{type(exc)} - {exc}") + Logger.logerr(f"Error building container structure for '{state.name}'!") + Logger.localerr(f"'{type(exc)}' - {exc}") return container_msg def get_latest_status(self): @@ -189,7 +190,7 @@ def _execute_current_state(self): self._last_exception = exc Logger.logerr('Failed to execute state %s:\n%s' % (self.current_state_label, str(exc))) import traceback # pylint: disable=C0415 - Logger.localinfo(traceback.format_exc().replace("%", "%%")) # Guard against exeception including format! + Logger.localinfo(traceback.format_exc().replace('%', '%%')) # Guard against exeception including format! if self._is_controlled: # reset previously requested outcome if applicable @@ -206,7 +207,7 @@ def _execute_current_state(self): self._pub.publish(Topics._OUTCOME_REQUEST_TOPIC, OutcomeRequest(outcome=self.outcomes.index(outcome), target=self.path)) - Logger.localinfo("<-- Want result: %s > %s" % (self.name, outcome)) + Logger.localinfo("<-- Want result: '%s' -> '%s'" % (self.name, outcome)) StateLogger.log('flexbe.operator', self, type='request', request=outcome, autonomy=self.parent.autonomy_level, required=self.parent.get_required_autonomy(outcome, self)) @@ -228,14 +229,14 @@ def _publish_outcome(self, outcome): outcome_index = self.outcomes.index(outcome) except Exception as exc: # pylint: disable=W0703 outcome_index = 0 - Logger.localerr("State Machine outcome error : %s > %s (%d) (%d) (%s)" + Logger.localerr('State Machine outcome error : %s > %s (%d) (%d) (%s)' % (self.name, outcome, outcome_index, self._state_id, self.__class__.__name__)) raise exc - Logger.localinfo("State Machine result: %s > %s (%d) (%d) (%s)" + Logger.localinfo('State Machine result: %s > %s (%d) (%d) (%s)' % (self.name, outcome, outcome_index, self._state_id, self.__class__.__name__)) self._pub.publish(Topics._OUTCOME_TOPIC, UInt32(data=StateMap.hash(self, outcome_index))) - self._pub.publish(Topics._DEBUG_TOPIC, String(data="%s > %s" % (self.path, outcome))) + self._pub.publish(Topics._DEBUG_TOPIC, String(data='%s > %s' % (self.path, outcome))) if self._force_transition: StateLogger.log('flexbe.operator', self, type='forced', forced=outcome, requested=self._last_requested_outcome) @@ -251,24 +252,27 @@ def process_sync_request(self): if self._inner_sync_request: self._inner_sync_request = False self._pub.publish(Topics._MIRROR_SYNC_TOPIC, self.get_latest_status()) - self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, CommandFeedback(command="sync", args=[])) - Logger.localinfo("<-- Sent synchronization message to mirror.") + self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, CommandFeedback(command='sync', args=[])) + Logger.localinfo('<-- Sent synchronization message to mirror.') else: Logger.error('Inner sync processed for %s - but no sync request flag?' % (self.name)) def is_transition_allowed(self, label, outcome): + """Is transition allowed.""" return self._autonomy[label].get(outcome, -1) < OperatableStateMachine.autonomy_level def get_required_autonomy(self, outcome, state): + """Get required autonomy.""" try: assert self.current_state_label == state.name, "get required autonomys in OSM state doesn't match!" return self._autonomy[self.current_state_label][outcome] except Exception: # pylint: disable=W0703 Logger.error(f"Failure to retrieve autonomy for '{self.name}' - " f" current state label='{self.name}' outcome='{outcome}'.") - Logger.localerr(f"{self._autonomy}") + Logger.localerr(f'{self._autonomy}') def destroy(self): + """Destroy this state machine.""" Logger.localinfo(f"Destroy state machine '{self.name}': {self.id} inst_id={id(self)} ...") self._notify_stop() # Recursively stop the states @@ -289,7 +293,7 @@ def destroy(self): self._pub.remove_publisher(Topics._OUTCOME_TOPIC) self._pub.remove_publisher(Topics._OUTCOME_REQUEST_TOPIC) - Logger.localinfo(" state logger shutdown ...") + Logger.localinfo(' state logger shutdown ...') StateLogger.shutdown() def confirm(self, name, beh_id): @@ -353,28 +357,28 @@ def _set_autonomy_level(self, msg): self._preempt_cb(msg) else: OperatableStateMachine.autonomy_level = msg.data - self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, CommandFeedback(command="autonomy", args=[])) + self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, CommandFeedback(command='autonomy', args=[])) def _sync_callback(self, msg): Logger.localwarn(f"--> Synchronization requested ... ({self.id}) '{self.name}' ") self._inner_sync_request = True # Flag to process at the end of spin loop def _attach_callback(self, msg): - Logger.localinfo("--> Enabling attach control...") + Logger.localinfo('--> Enabling attach control...') # set autonomy level OperatableStateMachine.autonomy_level = msg.data # enable control of states self._enable_ros_control() self._inner_sync_request = True # send command feedback - cfb = CommandFeedback(command="attach") + cfb = CommandFeedback(command='attach') cfb.args.append(self.name) self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, cfb) - Logger.localinfo("<-- Sent attach confirm.") + Logger.localinfo('<-- Sent attach confirm.') def _mirror_structure_callback(self, msg): if self._structure: - Logger.localinfo(f"--> Sending behavior structure to mirror id={msg.data} ...") + Logger.localinfo(f'--> Sending behavior structure to mirror id={msg.data} ...') self._pub.publish(Topics._MIRROR_STRUCTURE_TOPIC, self._structure) self._inner_sync_request = True # enable control of states since a mirror is listening @@ -394,7 +398,7 @@ def _notify_start(self): state._notify_start() def _notify_stop(self): - Logger.localinfo(f"Notify stop for {self.name} {self.path} ({id(self)}) ") + Logger.localinfo(f"Notify stop for '{self.name}' from '{self.path}' ({id(self)}) ") for state in self._states: if isinstance(state, OperatableState): @@ -408,6 +412,7 @@ def _notify_stop(self): self._structure = None # Flag for destruction def on_exit(self, userdata): + """Call on exiting the statemachine.""" if self._current_state is not None: udata = UserData(reference=self.userdata, input_keys=self._current_state.input_keys, diff --git a/flexbe_core/flexbe_core/core/preemptable_state.py b/flexbe_core/flexbe_core/core/preemptable_state.py index c07df7e..cbc5372 100644 --- a/flexbe_core/flexbe_core/core/preemptable_state.py +++ b/flexbe_core/flexbe_core/core/preemptable_state.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -31,14 +31,14 @@ """PreemptableState.""" -from std_msgs.msg import Empty - -from flexbe_msgs.msg import CommandFeedback - from flexbe_core.core.lockable_state import LockableState from flexbe_core.core.topics import Topics from flexbe_core.logger import Logger +from flexbe_msgs.msg import CommandFeedback + +from std_msgs.msg import Empty + class PreemptableState(LockableState): """ @@ -50,6 +50,7 @@ class PreemptableState(LockableState): preempt = False def __init__(self, *args, **kwargs): + """Initialize PreemptableState instance.""" super().__init__(*args, **kwargs) self.__execute = self.execute self.execute = self._preemptable_execute @@ -59,13 +60,13 @@ def __init__(self, *args, **kwargs): def _preemptable_execute(self, *args, **kwargs): if self._is_controlled and self._sub.has_msg(Topics._CMD_PREEMPT_TOPIC): self._sub.remove_last_msg(Topics._CMD_PREEMPT_TOPIC) - self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, CommandFeedback(command="preempt")) + self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, CommandFeedback(command='preempt')) PreemptableState.preempt = True - Logger.localinfo("--> Behavior will be preempted") + Logger.localinfo('--> Behavior will be preempted') if PreemptableState.preempt: if not self._is_controlled: - Logger.localinfo("Behavior will be preempted") + Logger.localinfo('Behavior will be preempted') self._force_transition = True return self._preempted_name @@ -75,7 +76,7 @@ def _notify_skipped(self): # make sure we dont miss a preempt even if not being executed if self._is_controlled and self._sub.has_msg(Topics._CMD_PREEMPT_TOPIC): self._sub.remove_last_msg(Topics._CMD_PREEMPT_TOPIC) - self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, CommandFeedback(command="preempt")) + self._pub.publish(Topics._CMD_FEEDBACK_TOPIC, CommandFeedback(command='preempt')) PreemptableState.preempt = True def _enable_ros_control(self): diff --git a/flexbe_core/flexbe_core/core/preemptable_state_machine.py b/flexbe_core/flexbe_core/core/preemptable_state_machine.py index ea7af2d..9f8b5aa 100644 --- a/flexbe_core/flexbe_core/core/preemptable_state_machine.py +++ b/flexbe_core/flexbe_core/core/preemptable_state_machine.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -32,14 +32,15 @@ """A state machine that can be preempted.""" import threading -import rclpy -from std_msgs.msg import Empty - from flexbe_core.core.lockable_state_machine import LockableStateMachine from flexbe_core.core.preemptable_state import PreemptableState from flexbe_core.core.topics import Topics from flexbe_core.logger import Logger +import rclpy + +from std_msgs.msg import Empty + class PreemptableStateMachine(LockableStateMachine): """ @@ -51,6 +52,7 @@ class PreemptableStateMachine(LockableStateMachine): _preempted_name = 'preempted' def __init__(self, *args, **kwargs): + """Initialize instance.""" super().__init__(*args, **kwargs) self._status_lock = threading.Lock() self._last_deep_states_list = None @@ -70,6 +72,7 @@ def _preempt_cb(self, msg): @staticmethod def add(label, state, transitions=None, remapping=None): + """Add state to SM.""" transitions[PreemptableState._preempted_name] = PreemptableStateMachine._preempted_name LockableStateMachine.add(label, state, transitions, remapping) @@ -78,28 +81,29 @@ def _valid_targets(self): return super()._valid_targets + [PreemptableStateMachine._preempted_name] def spin(self, userdata=None): + """Spin the execute loop for preemptable portion.""" outcome = None while rclpy.ok(): outcome = self.execute(userdata) # Store the information for safely passing to heartbeat thread deep_states = self.get_deep_states() - assert isinstance(deep_states, list), f"Expecting a list here, not {deep_states}" + assert isinstance(deep_states, list), f'Expecting a list here, not {deep_states}' if deep_states != self._last_deep_states_list: # Logger.localinfo(f"New deep states for '{self.name}' len={len(deep_states)} " - # f"deep states: {[dpst.path for dpst in deep_states if dpst is not None]}") + # f'deep states: {[dpst.path for dpst in deep_states if dpst is not None]}') with self._status_lock: self._last_deep_states_list = deep_states # else: # Logger.localinfo(f"Same old deep states for '{self.name}' len={len(deep_states)} - " - # f"deep states: {[dpst.name for dpst in deep_states]}") + # f'deep states: {[dpst.name for dpst in deep_states]}') if self._inner_sync_request: # Top-level state machine with sync request self.process_sync_request() if outcome is not None: - Logger.loginfo(f"PreemptableStateMachine {self.name} spin() - done with outcome={outcome}") + Logger.loginfo(f"PreemptableStateMachine '{self.name}' spin() - done with outcome={outcome}") break self.wait(seconds=self.sleep_duration) @@ -112,9 +116,10 @@ def get_latest_status(self): Note: Mirror uses derived version that cleans up mirror paths """ - raise NotImplementedError("Do not call this version directly - " - "either OperatableStateMachine or Mirror should override") + raise NotImplementedError('Do not call this version directly - ' + 'either OperatableStateMachine or Mirror should override') @classmethod def process_sync_request(cls): - Logger.localinfo("Ignoring PreemptableState process_sync_request") + """Process sync request (ignored here - should be handled by derived state).""" + Logger.localinfo('Ignoring PreemptableState process_sync_request') diff --git a/flexbe_core/flexbe_core/core/priority_container.py b/flexbe_core/flexbe_core/core/priority_container.py index d7f2924..2678912 100644 --- a/flexbe_core/flexbe_core/core/priority_container.py +++ b/flexbe_core/flexbe_core/core/priority_container.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -39,11 +39,13 @@ class PriorityContainer(OperatableStateMachine): active_container = None def __init__(self, conditions=None, *args, **kwargs): + """Initialize instance of PriorityContainer.""" super().__init__(*args, **kwargs) self._parent_active_container = None self._type = OperatableStateMachine.ContainerType.PriorityContainer.value def execute(self, *args, **kwargs): + """Execute the priority container.""" if (PriorityContainer.active_container is None or not all(p == PriorityContainer.active_container.split('/')[i] for i, p in enumerate(self.path.split('/')))): diff --git a/flexbe_core/flexbe_core/core/ros_state.py b/flexbe_core/flexbe_core/core/ros_state.py index 1c9d7b8..bfbafcf 100644 --- a/flexbe_core/flexbe_core/core/ros_state.py +++ b/flexbe_core/flexbe_core/core/ros_state.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -31,13 +31,13 @@ """A state to interface with ROS.""" -from rclpy.exceptions import ParameterNotDeclaredException -from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached - from flexbe_core.core.state import State from flexbe_core.logger import Logger +from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached from flexbe_core.state_logger import StateLogger +from rclpy.exceptions import ParameterNotDeclaredException + class RosState(State): """A state to interface with ROS.""" @@ -56,20 +56,21 @@ def initialize_ros(node): if RosState._breakpoints is None: try: RosState._breakpoints = node.get_parameter('breakpoints').get_parameter_value().string_array_value - Logger.localinfo(f"RosState: using breakpoints={RosState._breakpoints}") + Logger.localinfo(f'RosState: using breakpoints={RosState._breakpoints}') except ParameterNotDeclaredException: Logger.localinfo("RosState: No 'breakpoints' parameter is defined") RosState._breakpoints = [] def __init__(self, *args, **kwargs): + """Initialize RosState instance.""" super().__init__(*args, **kwargs) self._desired_period_ns = (1 / 10) * 1e9 - if "desired_rate" in kwargs: + 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 + f"rate to {kwargs['desired_rate']} Hz.") + self._desired_period_ns = (1 / kwargs['desired_rate']) * 1e9 self._is_controlled = False diff --git a/flexbe_core/flexbe_core/core/ros_state_machine.py b/flexbe_core/flexbe_core/core/ros_state_machine.py index 7486d0a..64df63f 100644 --- a/flexbe_core/flexbe_core/core/ros_state_machine.py +++ b/flexbe_core/flexbe_core/core/ros_state_machine.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -30,10 +30,10 @@ """A state machine to interface with ROS.""" -from rclpy.duration import Duration - -from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached from flexbe_core.core.state_machine import StateMachine +from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached + +from rclpy.duration import Duration class RosStateMachine(StateMachine): @@ -43,11 +43,13 @@ class RosStateMachine(StateMachine): @staticmethod def initialize_ros(node): + """Initialize ROS node information.""" RosStateMachine._node = node ProxyPublisher.initialize(node) ProxySubscriberCached.initialize(node) def __init__(self, *args, **kwargs): + """Initialize instance of ROSStateMachine.""" super().__init__(*args, **kwargs) self._is_controlled = False diff --git a/flexbe_core/flexbe_core/core/state.py b/flexbe_core/flexbe_core/core/state.py index 6d5d6a4..b5eb5a2 100644 --- a/flexbe_core/flexbe_core/core/state.py +++ b/flexbe_core/flexbe_core/core/state.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -47,6 +47,7 @@ class State: _preempted_name = 'preempted' # Define name here, but handle logic in derived class def __init__(self, *args, **kwargs): + """Initilize state instance.""" self._outcomes = _remove_duplicates(kwargs.get('outcomes', [])) io_keys = kwargs.get('io_keys', []) self._input_keys = _remove_duplicates(kwargs.get('input_keys', []) + io_keys) @@ -59,57 +60,69 @@ def __init__(self, *args, **kwargs): self._type = 0 # Basic states are type 0, containers have non-zero type def __str__(self): + """Return name of this state.""" return self._name def execute(self, userdata): - pass + """Execute this state.""" @property def sleep_duration(self): + """Return desired sleep duration.""" return 0. @property def outcomes(self): + """Return possible outcomes.""" return self._outcomes @property def input_keys(self): + """Return input userdata keys.""" return self._input_keys @property def output_keys(self): + """Return output userdata keys.""" return self._output_keys # instance properties @property def name(self): + """Return state name.""" return self._name def set_name(self, value): + """Set the name of this state.""" if self._name is not None: - raise StateError("Cannot change the name of a state!") + raise StateError('Cannot change the name of a state!') self._name = value @property def parent(self): + """Get parent state.""" return self._parent def set_parent(self, value): + """Set parent of this state.""" if self._parent is not None: - raise StateError("Cannot change the parent of a state!") + raise StateError('Cannot change the parent of a state!') self._parent = value @property def state_id(self): + """Return state id (hash code).""" return self._state_id @property def path(self): - return "" if self.parent is None else self.parent.path + "/" + self.name + """Return path from root state machine.""" + return '' if self.parent is None else self.parent.path + '/' + self.name @property def type(self): + """Return state type.""" return self._type diff --git a/flexbe_core/flexbe_core/core/state_machine.py b/flexbe_core/flexbe_core/core/state_machine.py index 43b2681..ba84c23 100644 --- a/flexbe_core/flexbe_core/core/state_machine.py +++ b/flexbe_core/flexbe_core/core/state_machine.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -30,10 +30,10 @@ """Implement FlexBE Statemachine.""" -from flexbe_core.logger import Logger +from flexbe_core.core.exceptions import StateError, StateMachineError from flexbe_core.core.state import State from flexbe_core.core.user_data import UserData -from flexbe_core.core.exceptions import StateError, StateMachineError +from flexbe_core.logger import Logger class StateMachine(State): @@ -42,6 +42,7 @@ class StateMachine(State): _currently_opened_container = None def __init__(self, *args, **kwargs): + """Initialize the StateMachine instance.""" super().__init__(*args, **kwargs) self._states = [] self._labels = {} @@ -53,33 +54,39 @@ def __init__(self, *args, **kwargs): self._previously_opened_container = None def __enter__(self): + """Enter the SM.""" self._previously_opened_container = StateMachine._currently_opened_container StateMachine._currently_opened_container = self def __exit__(self, *args): + """Exit the SM.""" StateMachine._currently_opened_container = self._previously_opened_container self._previously_opened_container = None def __contains__(self, label): + """Check if label in this SM.""" return label in self._labels def __getitem__(self, label): + """Get item corresponding to label.""" return self._labels[label] def __iter__(self): + """Iterate over states in SM.""" return iter(state.name for state in self._states) # construction @staticmethod def add(label, state, transitions, remapping=None): + """Add state to SM.""" self = StateMachine.get_opened_container() if self is None: raise StateMachineError("No container opened, activate one first by: 'with state_machine:'") if label in self._labels: - raise StateMachineError("The label %s has already been added to this state machine!" % label) + raise StateMachineError("The label '%s' has already been added to this state machine!" % label) if label in self._outcomes: - raise StateMachineError("The label %s is an outcome of this state machine!" % label) + raise StateMachineError("The label '%s' is an outcome of this state machine!" % label) # add state to this state machine self._states.append(state) self._labels[label] = state @@ -91,16 +98,19 @@ def add(label, state, transitions, remapping=None): @staticmethod def get_opened_container(): + """Get open container from this SM.""" return StateMachine._currently_opened_container def wait(self, seconds=None): + """Wait.""" # This should not be called; expect to call ros_state_machine version instead! - Logger.localinfo(f"Error calling StateMachine.wait Dummy wait method for " - f"{self.name} seconds={seconds}") - raise RuntimeError(f"Error calling StateMachine.wait Dummy wait method for " - f"{self.name} seconds={seconds}") + Logger.localinfo(f'Error calling StateMachine.wait Dummy wait method for ' + f"'{self.name}' seconds={seconds}") + raise RuntimeError(f'Error calling StateMachine.wait Dummy wait method for ' + f"'{self.name}' seconds={seconds}") def spin(self, userdata=None): + """Spin the SM execute loop.""" outcome = None while True: outcome = self.execute(userdata) @@ -113,16 +123,19 @@ def spin(self, userdata=None): return outcome def execute(self, userdata): + """Execute the SM.""" if self._current_state is None: self.assert_consistent_transitions() self._current_state = self.initial_state self._userdata = userdata if userdata is not None else UserData() self._userdata(add_from=self._own_userdata) - # Logger.localinfo(f"Entering StateMachine {self.name} ({self._state_id}) initial state='{self._current_state.name}'") + # Logger.localinfo(f"Entering StateMachine '{self.name}' " + # f"({self._state_id}) initial state='{self._current_state.name}'") outcome = self._execute_current_state() return outcome def _execute_current_state(self): + """Execute the currently active state in this SM.""" with UserData(reference=self._userdata, remap=self._remappings[self._current_state.name], input_keys=self._current_state.input_keys, output_keys=self._current_state.output_keys ) as userdata: @@ -153,35 +166,41 @@ def _execute_current_state(self): @property def userdata(self): + """Return userdata for this SM.""" return self._own_userdata @property def current_state(self): + """Return reference to the currently active state.""" if self._current_state is not None: return self._current_state - raise StateMachineError("No state active!") + raise StateMachineError('No state active!') @property def current_state_label(self): + """Return name of the currently active state.""" if self._current_state is not None: return self._current_state.name - raise StateMachineError("No state active!") + raise StateMachineError('No state active!') @property def initial_state(self): + """Return the initial state.""" if len(self._states) > 0: return self._states[0] - raise StateMachineError("No states added yet!") + raise StateMachineError('No states added yet!') @property def initial_state_label(self): + """Return the name of the initial state.""" return self.initial_state.name @property def sleep_duration(self): + """Return how long to sleep between execute steps.""" if self._current_state is not None: return self._current_state.sleep_duration @@ -206,9 +225,11 @@ def get_deep_states(self): @property def _valid_targets(self): + """Get list of validate outcomes.""" return list(self._labels.keys()) + self.outcomes def assert_consistent_transitions(self): + """Validate that transitions are consistent.""" for transitions in self._transitions.values(): for transition_target in transitions.values(): if transition_target not in self._valid_targets: diff --git a/flexbe_core/flexbe_core/core/state_map.py b/flexbe_core/flexbe_core/core/state_map.py index 9fdbc85..b33eb5a 100644 --- a/flexbe_core/flexbe_core/core/state_map.py +++ b/flexbe_core/flexbe_core/core/state_map.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Christopher Newport University +# Copyright 2024 Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -29,6 +29,7 @@ # POSSIBILITY OF SUCH DAMAGE. import zlib + from flexbe_core.logger import Logger @@ -44,8 +45,9 @@ def __init__(self): self._num_collision_processed = 0 def __str__(self): - return (f"State map with {len(self._state_map)} entries" - "." if self._num_collision_processed == 0 else f" with {self._num_collision_processed} collisions!") + """Return string with state map information.""" + return (f'State map with {len(self._state_map)} entries' + '.' if self._num_collision_processed == 0 else f' with {self._num_collision_processed} collisions!') def __getitem__(self, index): """Get existing state if possible, or return None.""" @@ -90,7 +92,7 @@ def add_state(self, path, state): while state._state_id in self._state_map: collisions += 1 if collisions > 20: - raise KeyError(f"Unable to avoid collisions in StateMap with {path}") + raise KeyError(f"Unable to avoid collisions in StateMap with '{path}'") hash_path += path state._state_id = self._hash_path(hash_path) self._state_map[state._state_id] = state @@ -98,7 +100,7 @@ def add_state(self, path, state): else: if state._state_id in self._state_map: Logger.error(f"State '{path}' : id={state._state_id} is already in map!") - raise KeyError(f"Existing state in StateMap with {path}") + raise KeyError(f"Existing state in StateMap with '{path}'") self._state_map[state._state_id] = state def get_state(self, state_id): @@ -106,7 +108,7 @@ def get_state(self, state_id): if state_id in self._state_map: return self._state_map[state_id] - Logger.error("State id={state_id} is not in the state map!") + Logger.error(f'State id={state_id} is not in the state map!') return None @classmethod diff --git a/flexbe_core/flexbe_core/core/topics.py b/flexbe_core/flexbe_core/core/topics.py index 3198f83..3dfe7ad 100644 --- a/flexbe_core/flexbe_core/core/topics.py +++ b/flexbe_core/flexbe_core/core/topics.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Christopher Newport University +# Copyright 2024 Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -28,13 +28,12 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. -from std_msgs.msg import Bool, Empty, String, UInt32, UInt8 - from flexbe_msgs.action import BehaviorExecution +from flexbe_msgs.msg import BEStatus, BehaviorLog, BehaviorRequest, BehaviorSelection, BehaviorSync +from flexbe_msgs.msg import CommandFeedback, ContainerStructure +from flexbe_msgs.msg import OutcomeRequest -from flexbe_msgs.msg import BehaviorLog, BehaviorRequest, BehaviorSelection, BehaviorSync -from flexbe_msgs.msg import BEStatus, CommandFeedback -from flexbe_msgs.msg import ContainerStructure, OutcomeRequest +from std_msgs.msg import Bool, Empty, String, UInt32, UInt8 class Topics: @@ -62,7 +61,7 @@ class Topics: _ONBOARD_STATUS_TOPIC = 'flexbe/status' # Onboard behavior engine status _OUTCOME_REQUEST_TOPIC = 'flexbe/outcome_request' # OCS request outcome _OUTCOME_TOPIC = 'flexbe/mirror/outcome' # State outcomes used in mirror to track status - _REQUEST_BEHAVIOR_TOPIC = "flexbe/request_behavior" + _REQUEST_BEHAVIOR_TOPIC = 'flexbe/request_behavior' _REQUEST_STRUCTURE_TOPIC = 'flexbe/request_mirror_structure' # Request state machine structure from onboard _START_BEHAVIOR_TOPIC = 'flexbe/start_behavior' # OCS or launcher command to start behavior _STATE_LOGGER_TOPIC = 'flexbe/state_logger' @@ -102,4 +101,5 @@ class Topics: @staticmethod def get_type(topic): + """Return topic msg type given topic name.""" return Topics._topic_types[topic] diff --git a/flexbe_core/flexbe_core/core/user_data.py b/flexbe_core/flexbe_core/core/user_data.py index b37de8a..bf6b867 100644 --- a/flexbe_core/flexbe_core/core/user_data.py +++ b/flexbe_core/flexbe_core/core/user_data.py @@ -1,6 +1,6 @@ #!/user/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -31,6 +31,7 @@ """UserData Class.""" from copy import deepcopy + from flexbe_core.core.exceptions import UserDataError @@ -38,6 +39,7 @@ class UserData: """UserData Class.""" def __init__(self, reference=None, input_keys=None, output_keys=None, remap=None): + """Initialize UserData instance.""" self._data = {} self._reference = reference if reference is not None else {} self._input_keys = input_keys @@ -72,7 +74,7 @@ def __getitem__(self, key): if self._output_keys is not None and key not in self._output_keys: self._data[key] = value self._hashes[key] = hash(repr(value)) - if getattr(value.__class__, "_has_header", False): + if getattr(value.__class__, '_has_header', False): # This is specific to rospy: If the value here is a message and has a header, # it will automatically be modified during publishing by rospy. # So to avoid hash issues, we need to return a copy. @@ -115,6 +117,7 @@ def __setattr__(self, key, value): self[key] = value def __call__(self, reference=None, add_from=None, update_from=None, remove_key=None): + """Call user data.""" self._reference = reference if reference is not None else self._reference if isinstance(add_from, UserData): for key, value in add_from._data.items(): @@ -127,14 +130,16 @@ def __call__(self, reference=None, add_from=None, update_from=None, remove_key=N del self._data[remove_key] def __len__(self): + """Get total length of data and references.""" return len(self._data) + len(self._reference) def __str__(self): + """Return string representing mapped user data.""" if isinstance(self._reference, UserData): data_str = '\n '.join(str(self._reference).split('\n')) else: data_str = str(self._reference) - return ("UserData object with %d data entries:\n" - " Input Keys: %s\n Output Keys: %s\n Data: %s\n Remapping: %s\n Reference: %s" + return ('UserData object with %d data entries:\n' + ' Input Keys: %s\n Output Keys: %s\n Data: %s\n Remapping: %s\n Reference: %s' % (len(self), str(self._input_keys), str(self._output_keys), str(self._data), str(self._remap), data_str)) diff --git a/flexbe_core/flexbe_core/logger.py b/flexbe_core/flexbe_core/logger.py index 7abea03..26d0c3d 100644 --- a/flexbe_core/flexbe_core/logger.py +++ b/flexbe_core/flexbe_core/logger.py @@ -31,13 +31,14 @@ """Realize behavior-specific logging.""" -from rclpy.exceptions import ParameterNotDeclaredException -from rclpy.node import Node -from rclpy.duration import Duration - from flexbe_core.core.topics import Topics + from flexbe_msgs.msg import BehaviorLog +from rclpy.duration import Duration +from rclpy.exceptions import ParameterNotDeclaredException +from rclpy.node import Node + class Logger: """Realize behavior-specific logging.""" @@ -57,31 +58,33 @@ class Logger: @staticmethod def initialize(node: Node): + """Initialize the logger instance.""" Logger._node = node Logger._pub = node.create_publisher(BehaviorLog, Topics._BEHAVIOR_LOGGING_TOPIC, 100) Logger._last_logged = {} # Optional parameters that can be defined try: - size_param = node.get_parameter("max_throttle_logging_size") + size_param = node.get_parameter('max_throttle_logging_size') if size_param.type_ == size_param.Type.INTEGER: Logger.MAX_LAST_LOGGED_SIZE = size_param.value - except ParameterNotDeclaredException as exc: + except ParameterNotDeclaredException: pass try: - clear_param = node.get_parameter("throttle_logging_clear_ratio") + clear_param = node.get_parameter('throttle_logging_clear_ratio') if clear_param.type_ in (clear_param.Type.INTEGER, clear_param.Type.DOUBLE): Logger.LAST_LOGGED_CLEARING_RATIO = clear_param.value - except ParameterNotDeclaredException as exc: + except ParameterNotDeclaredException: pass - Logger._node.get_logger().debug(f"Enable throttle logging option with " - f"max size={Logger.MAX_LAST_LOGGED_SIZE} " - f"clear ratio={Logger.LAST_LOGGED_CLEARING_RATIO}") + Logger._node.get_logger().debug(f'Enable throttle logging option with ' + f'max size={Logger.MAX_LAST_LOGGED_SIZE} ' + f'clear ratio={Logger.LAST_LOGGED_CLEARING_RATIO}') @staticmethod def log(text: str, severity: int): + """Log message.""" if Logger._node is None: raise RuntimeError('Unable to log, run "Logger.initialize" first to define the target ROS node.') # send message with logged text @@ -94,8 +97,9 @@ def log(text: str, severity: int): @staticmethod def log_throttle(period: float, text: str, severity: int): + """Log unique messages once and don't repeat messages.""" # create unique identifier for each logging message - log_id = f"{severity}_{text}" + log_id = f'{severity}_{text}' time_now = Logger._node.get_clock().now() # only log when it's the first time or period time has passed for the logging message if log_id not in Logger._last_logged.keys() or \ @@ -113,99 +117,120 @@ def log_throttle(period: float, text: str, severity: int): @staticmethod def local(text: str, severity: int): + """Local logging to terminal.""" if Logger._node is None: raise RuntimeError('Unable to log, run "Logger.initialize" first to define the target ROS node.') if severity == Logger.REPORT_INFO: Logger._node.get_logger().info(text) elif severity == Logger.REPORT_WARN: - Logger._node.get_logger().warning(f"\033[93m{text}\033[0m") + Logger._node.get_logger().warning(f'\033[93m{text}\033[0m') elif severity == Logger.REPORT_HINT: - Logger._node.get_logger().info(f"\033[94mBehavior Hint: {text}\033[0m") + Logger._node.get_logger().info(f'\033[94mBehavior Hint: {text}\033[0m') elif severity == Logger.REPORT_ERROR: - Logger._node.get_logger().error(f"\033[91m{text}\033[0m") + Logger._node.get_logger().error(f'\033[91m{text}\033[0m') elif severity == Logger.REPORT_DEBUG: - Logger._node.get_logger().debug(f"\033[92m{text}\033[0m") + Logger._node.get_logger().debug(f'\033[92m{text}\033[0m') else: - Logger._node.get_logger().debug(f"\033[95m{text}\033[91m(unknown log level {str(severity)})\033[0m") + Logger._node.get_logger().debug(f'\033[95m{text}\033[91m(unknown log level {str(severity)})\033[0m') # NOTE: Below text strings can only have single % symbols if they are being treated # as format strings with appropriate arguments (otherwise replace with %% for simple string without args) @staticmethod def logdebug(text: str, *args): + """Log debug.""" Logger.log(text % args, Logger.REPORT_DEBUG) @staticmethod def loginfo(text: str, *args): + """Log info.""" Logger.log(text % args, Logger.REPORT_INFO) @staticmethod def logwarn(text: str, *args): + """Log warn.""" Logger.log(text % args, Logger.REPORT_WARN) @staticmethod def loghint(text: str, *args): + """Log hint.""" Logger.log(text % args, Logger.REPORT_HINT) @staticmethod def logerr(text: str, *args): + """Log error.""" Logger.log(text % args, Logger.REPORT_ERROR) @staticmethod def logdebug_throttle(period: float, text: str, *args): + """Log debug throttle.""" Logger.log_throttle(period, text % args, Logger.REPORT_DEBUG) @staticmethod def loginfo_throttle(period: float, text: str, *args): + """Log info throttle.""" Logger.log_throttle(period, text % args, Logger.REPORT_INFO) @staticmethod def logwarn_throttle(period: float, text: str, *args): + """Log warn throttle.""" Logger.log_throttle(period, text % args, Logger.REPORT_WARN) @staticmethod def loghint_throttle(period: float, text: str, *args): + """Log hint throttle.""" Logger.log_throttle(period, text % args, Logger.REPORT_HINT) @staticmethod def logerr_throttle(period: float, text: str, *args): + """Log error throttle.""" Logger.log_throttle(period, text % args, Logger.REPORT_ERROR) @staticmethod def localdebug(text: str, *args): + """Local debug.""" Logger.local(text % args, Logger.REPORT_DEBUG) @staticmethod def localinfo(text: str, *args): + """Local info.""" Logger.local(text % args, Logger.REPORT_INFO) @staticmethod def localwarn(text: str, *args): + """Local warn.""" Logger.local(text % args, Logger.REPORT_WARN) @staticmethod def localhint(text: str, *args): + """Local hint.""" Logger.local(text % args, Logger.REPORT_HINT) @staticmethod def localerr(text: str, *args): + """Local error.""" Logger.local(text % args, Logger.REPORT_ERROR) @staticmethod def debug(text: str, *args): + """Log debug.""" Logger.logdebug(text, *args) @staticmethod def info(text: str, *args): + """Log info.""" Logger.loginfo(text, *args) @staticmethod def warning(text: str, *args): + """Log warning.""" Logger.logwarn(text, *args) @staticmethod def hint(text: str, *args): + """Log hint.""" Logger.loghint(text, *args) @staticmethod def error(text: str, *args): + """Log error.""" Logger.logerr(text, *args) diff --git a/flexbe_core/flexbe_core/proxy/__init__.py b/flexbe_core/flexbe_core/proxy/__init__.py index 2475929..62fb02c 100644 --- a/flexbe_core/flexbe_core/proxy/__init__.py +++ b/flexbe_core/flexbe_core/proxy/__init__.py @@ -1,4 +1,4 @@ -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -27,7 +27,7 @@ # POSSIBILITY OF SUCH DAMAGE. -"""Initialize of the flexbe_core.proxy module.""" +"""Initialize of the flexbe_core.proxy package.""" from .proxy_action_client import ProxyActionClient # noqa: F401 from .proxy_publisher import ProxyPublisher # noqa: F401 diff --git a/flexbe_core/flexbe_core/proxy/proxy_action_client.py b/flexbe_core/flexbe_core/proxy/proxy_action_client.py index 5218677..18bb43b 100644 --- a/flexbe_core/flexbe_core/proxy/proxy_action_client.py +++ b/flexbe_core/flexbe_core/proxy/proxy_action_client.py @@ -29,12 +29,14 @@ """A proxy for calling actions provides a single point for all state action interfaces.""" from functools import partial -from threading import Timer, Lock +from threading import Lock, Timer from action_msgs.msg import GoalStatus -from rclpy.action import ActionClient + from flexbe_core.logger import Logger +from rclpy.action import ActionClient + class ProxyActionClient: """A proxy for calling actions.""" @@ -61,13 +63,13 @@ def initialize(node): def shutdown(): """Shuts this proxy down by resetting all action clients.""" try: - print(f"Shutdown proxy action clients with {len(ProxyActionClient._clients)} topics ...") + print(f'Shutdown proxy action clients with {len(ProxyActionClient._clients)} topics ...') for topic, client_dict in ProxyActionClient._clients.items(): try: ProxyActionClient._clients[topic] = None ProxyActionClient._node.destroy_client(client_dict['client']) except Exception as exc: # pylint: disable=W0703 - Logger.error(f"Something went wrong during shutdown of proxy action client for {topic}!\n{str(exc)}") + Logger.error(f"Something went wrong during shutdown of proxy action client for '{topic}'!\n{str(exc)}") ProxyActionClient._result.clear() ProxyActionClient._result_status.clear() @@ -95,7 +97,8 @@ def __init__(self, topics=None, wait_duration=10): @classmethod def setupClient(cls, topic, action_type, wait_duration=10): - Logger.localerr("Deprecated: Use ProxyActionClient.setup_client instead!") + """Set up proxy action client (Deprecated).""" + Logger.localerr('Deprecated: Use ProxyActionClient.setup_client instead!') cls.setup_client(topic, action_type, wait_duration=10) @classmethod @@ -124,10 +127,10 @@ def setup_client(cls, topic, action_type, wait_duration=None): Logger.localinfo(f'Existing action client for {topic}' f' with same action type name, but different instance - re-create client!') else: - Logger.localwarn(f"Existing action client for {topic} " + Logger.localwarn(f"Existing action client for '{topic}' " f"with {ProxyActionClient._clients[topic]['count']} references\n" - f" with same action type name, but different instance\n" - f" just re-create client with 1 reference - but be warned!") + f' with same action type name, but different instance\n' + f' just re-create client with 1 reference - but be warned!') # Destroy the existing client in executor thread client = ProxyActionClient._clients[topic]['client'] @@ -137,7 +140,7 @@ def setup_client(cls, topic, action_type, wait_duration=None): action_type, topic), 'count': 1} else: - raise TypeError("Trying to replace existing action client with different action type") + raise TypeError('Trying to replace existing action client with different action type') else: ProxyActionClient._clients[topic]['count'] = ProxyActionClient._clients[topic]['count'] + 1 @@ -180,13 +183,13 @@ def send_goal(cls, topic, goal, wait_duration=0.0): # To avoid rclpy TypeErrors, we will automatically convert to the base type # used in the original service/publisher clients new_goal = client._action_type.Goal() - Logger.localinfo(f" converting goal {str(type(new_goal))} vs. {str(type(goal))}") - assert new_goal.__slots__ == goal.__slots__, f"Message attributes for {topic} do not match!" + Logger.localinfo(f" converting goal '{str(type(new_goal))}' vs. '{str(type(goal))}'") + assert new_goal.__slots__ == goal.__slots__, f"Message attributes for '{topic}' do not match!" for attr in goal.__slots__: setattr(new_goal, attr, getattr(goal, attr)) else: - raise TypeError(f"Invalid goal type {goal.__class__.__name__}" - f" (vs. {ProxyActionClient._clients[topic]._action_type.Goal.__name__}) for topic {topic}") + raise TypeError(f"Invalid goal type '{goal.__class__.__name__}'" + f" (vs. '{ProxyActionClient._clients[topic]._action_type.Goal.__name__}') for topic '{topic}'") else: # Same class definition instance as stored new_goal = goal @@ -417,5 +420,5 @@ def destroy_client(cls, client, topic): del client Logger.localinfo(f'Destroyed the proxy action client for {topic}!') except Exception as exc: # pylint: disable=W0703 - Logger.error("Something went wrong destroying proxy action client" - f" for {topic}!\n {type(exc)} - {str(exc)}") + Logger.error('Something went wrong destroying proxy action client' + f" for '{topic}'!\n {type(exc)} - {str(exc)}") diff --git a/flexbe_core/flexbe_core/proxy/proxy_publisher.py b/flexbe_core/flexbe_core/proxy/proxy_publisher.py index 39ebb09..2236d54 100644 --- a/flexbe_core/flexbe_core/proxy/proxy_publisher.py +++ b/flexbe_core/flexbe_core/proxy/proxy_publisher.py @@ -1,4 +1,4 @@ -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -33,7 +33,7 @@ Provides a single point for comminications for all states in behavior """ -from threading import Timer, Event, Lock +from threading import Event, Lock, Timer from flexbe_core.logger import Logger from flexbe_core.proxy.qos import QOS_DEFAULT @@ -56,13 +56,13 @@ def initialize(node): def shutdown(): """Shuts this proxy down by resetting all publishers.""" try: - print(f"Shutdown proxy publisher with {len(ProxyPublisher._topics)} topics ...") + print(f'Shutdown proxy publisher with {len(ProxyPublisher._topics)} topics ...') for topic, pub in ProxyPublisher._topics.items(): try: ProxyPublisher._topics[topic] = None ProxyPublisher._node.destroy_publisher(pub['publisher']) except Exception as exc: # pylint: disable=W0703 - Logger.error(f"Something went wrong during shutdown of proxy publisher for {topic}!\n%s %s", + Logger.error(f"Something went wrong during shutdown of proxy publisher for '{topic}'!\n%s %s", type(exc), str(exc)) ProxyPublisher._topics.clear() @@ -91,7 +91,8 @@ def __init__(self, topics=None, qos=None, **kwargs): @classmethod def createPublisher(cls, topic, msg_type, qos=None, **kwargs): - Logger.localerr("Deprecated: Use ProxyPublisher.create_publisher instead!") + """Create publisher for topic (Deprecated).""" + Logger.localerr('Deprecated: Use ProxyPublisher.create_publisher instead!') cls.create_publisher(topic, msg_type, qos, **kwargs) @classmethod @@ -128,10 +129,10 @@ def create_publisher(cls, topic, msg_type, qos=None, **kwargs): ProxyPublisher._topics[topic]['publisher'] = ProxyPublisher._node.create_publisher(msg_type, topic, qos) ProxyPublisher._topics[topic]['count'] = 1 else: - Logger.info(f"Mis-matched msg_types ({msg_type.__name__} vs." - f" {ProxyPublisher._topics[topic]['publisher'].msg_type.__name__}) for {topic}" - f" (possibly due to reload of behavior)!") - raise TypeError(f"Trying to replace existing publisher with different msg type for {topic}") + Logger.info(f"Mis-matched msg_types ('{msg_type.__name__}' vs." + f" {ProxyPublisher._topics[topic]['publisher'].msg_type.__name__}) for '{topic}'" + ' (possibly due to reload of behavior)!') + raise TypeError(f"Trying to replace existing publisher with different msg type for '{topic}'") else: ProxyPublisher._topics[topic]['count'] = ProxyPublisher._topics[topic]['count'] + 1 @@ -198,13 +199,13 @@ def publish(cls, topic, msg): str(id(msg_type)))) new_msg = msg_type() - assert new_msg.__slots__ == msg.__slots__, f"Message attributes for {topic} do not match!" + assert new_msg.__slots__ == msg.__slots__, f"Message attributes for '{topic}' do not match!" for attr in msg.__slots__: setattr(new_msg, attr, getattr(msg, attr)) else: - raise TypeError(f"Invalid request type {msg.__class__.__name__}" - f" (vs. {msg_type.__name__}) for topic {topic}") + raise TypeError(f"Invalid request type '{msg.__class__.__name__}'" + f" (vs. {msg_type.__name__}) for topic '{topic}'") else: # Same class definition instance as stored new_msg = msg @@ -213,9 +214,9 @@ def publish(cls, topic, msg): with cls._publisher_sync_lock: ProxyPublisher._topics[topic]['publisher'].publish(new_msg) except Exception as exc: # pylint: disable=W0703 - Logger.warning('Something went wrong when publishing to %s!\n%s: %s' % (topic, str(type(exc)), str(exc))) + Logger.warning("Something went wrong when publishing to '%s'!\n%s: %s" % (topic, str(type(exc)), str(exc))) import traceback # pylint: disable=C0415 - Logger.localinfo(traceback.format_exc().replace("%", "%%")) + Logger.localinfo(traceback.format_exc().replace('%', '%%')) @classmethod def number_of_subscribers(cls, topic): @@ -227,7 +228,7 @@ def number_of_subscribers(cls, topic): """ pub = ProxyPublisher._topics.get(topic) if pub is None: - Logger.error("Publisher %s not yet registered, need to add it first!" % topic) + Logger.error("Publisher '%s' not yet registered, need to add it first!" % topic) return -1 return pub['publisher'].get_subscription_count() @@ -244,7 +245,7 @@ def wait_for_any(cls, topic, timeout=5.0): """ pub = ProxyPublisher._topics.get(topic) if pub is None: - Logger.error("Publisher %s not yet registered, need to add it first!" % topic) + Logger.error("Publisher '%s' not yet registered, need to add it first!" % topic) return False tmr = Timer(.5, ProxyPublisher._print_wait_warning, [topic]) @@ -259,17 +260,17 @@ def wait_for_any(cls, topic, timeout=5.0): # Problem here if not available: - Logger.error("Waiting for subscribers on %s timed out!" % topic) + Logger.error("Waiting for subscribers on '%s' timed out!" % topic) return False if warning_sent: - Logger.info("Finally found subscriber on %s..." % (topic)) + Logger.info("Finally found subscriber on '%s'..." % (topic)) return True @classmethod def _print_wait_warning(cls, topic): - Logger.warning("Waiting for subscribers on %s..." % (topic)) + Logger.warning("Waiting for subscribers on '%s'..." % (topic)) @classmethod def _wait_for_subscribers(cls, pub, timeout=5.0): @@ -289,10 +290,10 @@ def destroy_publisher(cls, pub, topic): """Handle publisher destruction from within the executor threads.""" try: if ProxyPublisher._node.destroy_publisher(pub): - Logger.localinfo(f'Destroyed the proxy publisher for {topic}!') + Logger.localinfo(f"Destroyed the proxy publisher for '{topic}'!") else: - Logger.localwarn(f'Some issue destroying the proxy publisher for {topic}!') + Logger.localwarn(f"Some issue destroying the proxy publisher for '{topic}'!") del pub except Exception as exc: # pylint: disable=W0703 - Logger.error("Something went wrong destroying proxy publisher" - f" for {topic}!\n {type(exc)} - {str(exc)}") + Logger.error('Something went wrong destroying proxy publisher' + f" for '{topic}'!\n {type(exc)} - {str(exc)}") diff --git a/flexbe_core/flexbe_core/proxy/proxy_service_caller.py b/flexbe_core/flexbe_core/proxy/proxy_service_caller.py index e7403eb..3f709b7 100644 --- a/flexbe_core/flexbe_core/proxy/proxy_service_caller.py +++ b/flexbe_core/flexbe_core/proxy/proxy_service_caller.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -31,12 +31,12 @@ """A proxy for providing single service connection for FlexBE behaviors.""" -from threading import Timer, Lock - -import rclpy +from threading import Lock, Timer from flexbe_core.logger import Logger +import rclpy + class ProxyServiceCaller: """A proxy for calling services.""" @@ -57,18 +57,18 @@ def initialize(node): def shutdown(): """Shut down this proxy by resetting all service callers.""" try: - print(f"Shutdown proxy service caller with {len(ProxyServiceCaller._services)} topics ...") + print(f'Shutdown proxy service caller with {len(ProxyServiceCaller._services)} topics ...') for topic, service in ProxyServiceCaller._services.items(): try: ProxyServiceCaller._services[topic] = None try: ProxyServiceCaller._node.destroy_client(service) except Exception as exc: # pylint: disable=W0703 - Logger.error("Something went wrong destroying service client" - f" for {topic}!\n {type(exc)} - {str(exc)}") + Logger.error('Something went wrong destroying service client' + f" for '{topic}'!\n {type(exc)} - {str(exc)}") except Exception as exc: # pylint: disable=W0703 - Logger.error("Something went wrong during shutdown of proxy service" - f" caller for {topic}!\n {type(exc)} - {exc}") + Logger.error('Something went wrong during shutdown of proxy service' + f" caller for '{topic}'!\n {type(exc)} - {exc}") ProxyServiceCaller._results.clear() @@ -91,7 +91,8 @@ def __init__(self, topics=None, wait_duration=10): @classmethod def setupService(cls, topic, srv_type, wait_duration=10): - Logger.localerr("Deprecated: Use ProxyServiceCaller.setup_service instead!") + """Set up the service caller.""" + Logger.localerr('Deprecated: Use ProxyServiceCaller.setup_service instead!') cls.setup_service(topic, srv_type, wait_duration) @classmethod @@ -120,10 +121,10 @@ def setup_service(cls, topic, srv_type, wait_duration=None): Logger.localinfo(f'Existing service for {topic} with same message type name,' f' but different instance - re-create service!') else: - Logger.localwarn(f"Existing service for {topic} with " + Logger.localwarn(f"Existing service for '{topic}' with " f"{ProxyServiceCaller._services[topic]['count']} references\n" - f" with same service type name, but different instance\n" - f" just re-create client with 1 reference - but be warned!") + f' with same service type name, but different instance\n' + f' just re-create client with 1 reference - but be warned!') ProxyServiceCaller._node.executor.create_task(ProxyServiceCaller.destroy_service, ProxyServiceCaller._services[topic]['service'], topic) @@ -132,7 +133,7 @@ def setup_service(cls, topic, srv_type, wait_duration=None): topic), 'count': 1} else: - raise TypeError("Trying to replace existing service caller with different service msg type") + raise TypeError('Trying to replace existing service caller with different service msg type') else: ProxyServiceCaller._services[topic]['count'] = ProxyServiceCaller._services[topic]['count'] + 1 @@ -182,18 +183,18 @@ def call(cls, topic, request, wait_duration=1.0): new_request = client.srv_type.Request() for attr, val in vars(new_request): # Validate that attributes in common - assert hasattr(request, attr), "Types must share common attributes!" + assert hasattr(request, attr), 'Types must share common attributes!' for attr, val in vars(request): setattr(new_request, attr, val) else: - raise TypeError(f"Invalid request type {request.__class__.__name__}" - f" (vs. {client.srv_type.Request.__name__}) " - f"for topic {topic}") + raise TypeError(f'Invalid request type {request.__class__.__name__}' + f' (vs. {client.srv_type.Request.__name__}) ' + f"for topic '{topic}'") else: # Same class definition instance as stored new_request = request - Logger.loginfo("Client about to call service") + Logger.loginfo('Client about to call service') return client.call(new_request) @@ -229,12 +230,12 @@ def call_async(cls, topic, request, wait_duration=1.0): # used in the original service clients new_request = client.srv_type.Request() - assert new_request.__slots__ == request.__slots__, f"Message attributes for {topic} do not match!" + assert new_request.__slots__ == request.__slots__, f"Message attributes for '{topic}' do not match!" for attr in request.__slots__: setattr(new_request, attr, getattr(request, attr)) else: - raise TypeError(f"Invalid request type {request.__class__.__name__} " - f"(vs. {client.srv_type.Request.__name__}) for topic {topic}") + raise TypeError(f'Invalid request type {request.__class__.__name__} ' + f'(vs. {client.srv_type.Request.__name__}) for topic {topic}') else: # Same class definition instance as stored new_request = request @@ -278,12 +279,12 @@ def _check_service_available(cls, topic, wait_duration=1): """ client_dict = ProxyServiceCaller._services.get(topic) if client_dict is None: - Logger.error(f"Service client {topic} not yet registered, need to add it first!") + Logger.error(f"Service client '{topic}' not yet registered, need to add it first!") return False client = client_dict['service'] if not isinstance(wait_duration, float): - Logger.localinfo(f"Check for service {topic} requires floating point wait_duration in seconds (change to 0.001)!") + Logger.localinfo(f"Check for service '{topic}' requires floating point wait_duration in seconds (change to 0.001)!") wait_duration = 0.001 warning_sent = False @@ -307,15 +308,15 @@ def _check_service_available(cls, topic, wait_duration=1): warning_sent = True if not available: - Logger.error(f"Service client {topic} not available! (timed out with wait_duration={wait_duration:.3f} seconds)") + Logger.error(f"Service client '{topic}' not available! (timed out with wait_duration={wait_duration:.3f} seconds)") elif warning_sent: - Logger.info("Finally found service %s..." % (topic)) + Logger.info(f"Finally found service '{topic}' ...") return available @classmethod def _print_wait_warning(cls, topic): - Logger.warning("Waiting for service %s..." % (topic)) + Logger.warning(f"Waiting for service '{topic}' ...") @classmethod def remove_client(cls, topic): @@ -352,5 +353,5 @@ def destroy_service(cls, srv, topic): Logger.localwarn(f'Some issue destroying the proxy service caller for {topic}!') del srv except Exception as exc: # pylint: disable=W0703 - Logger.error("Something went wrong destroying service caller" - f" for {topic}!\n {type(exc)} - {str(exc)}") + Logger.error('Something went wrong destroying service caller' + f" for '{topic}'!\n {type(exc)} - {str(exc)}") diff --git a/flexbe_core/flexbe_core/proxy/proxy_subscriber_cached.py b/flexbe_core/flexbe_core/proxy/proxy_subscriber_cached.py index 508e5f6..85acbce 100644 --- a/flexbe_core/flexbe_core/proxy/proxy_subscriber_cached.py +++ b/flexbe_core/flexbe_core/proxy/proxy_subscriber_cached.py @@ -1,4 +1,4 @@ -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -33,9 +33,10 @@ Provides a single point for comminications for all states in behavior """ -from functools import partial from collections import defaultdict, deque +from functools import partial from threading import Lock + from flexbe_core.logger import Logger from flexbe_core.proxy.qos import QOS_DEFAULT @@ -60,19 +61,19 @@ def shutdown(): """Shut down this proxy by unregistering all subscribers.""" try: with ProxySubscriberCached._subscription_lock: - print(f"Shutdown proxy subscriber with {len(ProxySubscriberCached._topics)} topics ...", flush=True) + print(f'Shutdown proxy subscriber with {len(ProxySubscriberCached._topics)} topics ...', flush=True) for topic, topic_dict in ProxySubscriberCached._topics.items(): try: ProxySubscriberCached._topics[topic] = None topic_dict['callbacks'] = {} ProxySubscriberCached._node.destroy_subscription(topic_dict['subscription']) except Exception as exc: # pylint: disable=W0703 - Logger.error(f"Something went wrong during shutdown of proxy subscriber for " - f"{topic}!\n{type(exc)} - {exc}") + Logger.error(f'Something went wrong during shutdown of proxy subscriber for ' + f'{topic}!\n{type(exc)} - {exc}') ProxySubscriberCached._topics.clear() ProxySubscriberCached._persistant_topics.clear() - print("Shutdown proxy subscriber - finished!") + print('Shutdown proxy subscriber - finished!') except Exception as exc: # pylint: disable=W0703 print(f'Something went wrong during shutdown of proxy subscriber !\n{str(exc)}') @@ -131,7 +132,7 @@ def subscribe(self, topic, msg_type, callback=None, buffered=False, qos=None, in 'callbacks': defaultdict(None), 'subscribers': [inst_id]} - Logger.localinfo(f"Created subscription for {topic} with message type {msg_type.__name__}!") + Logger.localinfo(f"Created subscription for '{topic}' with message type '{msg_type.__name__}'!") else: with ProxySubscriberCached._subscription_lock: @@ -142,27 +143,27 @@ def subscribe(self, topic, msg_type, callback=None, buffered=False, qos=None, in # Since we don't throw TypeErrors based on isinstance, and count on Python's duck typing # for callbacks, we will ignore on FlexBE side for subscribers if inst_id not in ProxySubscriberCached._topics[topic]['subscribers']: - Logger.localinfo(f"Add subscriber to existing subscription for {topic}" - " - keep existing subscriber! (" + Logger.localinfo(f"Add subscriber to existing subscription for '{topic}'" + ' - keep existing subscriber! (' f"{len(ProxySubscriberCached._topics[topic]['subscribers'])})") ProxySubscriberCached._topics[topic]['subscribers'].append(inst_id) else: - Logger.localinfo(f"Existing subscription for {topic} with same message type name" - " - keep existing subscriber! " + Logger.localinfo(f"Existing subscription for '{topic}' with same message type name" + ' - keep existing subscriber! ' f"({len(ProxySubscriberCached._topics[topic]['subscribers'])})") else: - Logger.info(f"Mis-matched msg_types ({msg_type.__name__} vs. " + Logger.info(f'Mis-matched msg_types ({msg_type.__name__} vs. ' f"{ProxySubscriberCached._topics[topic]['subscription'].msg_type.__name__})" - f" for {topic} subscription (possibly due to reload of behavior)!") - raise TypeError(f"Trying to replace existing subscription with different msg type for {topic}") + f" for '{topic}' subscription (possibly due to reload of behavior)!") + raise TypeError(f"Trying to replace existing subscription with different msg type for '{topic}'") else: if inst_id not in ProxySubscriberCached._topics[topic]['subscribers']: - Logger.localinfo(f"Add subscriber to existing subscription for {topic}! " + Logger.localinfo(f"Add subscriber to existing subscription for '{topic}'! " f"({len(ProxySubscriberCached._topics[topic]['subscribers'])})") ProxySubscriberCached._topics[topic]['subscribers'].append(inst_id) else: - Logger.localinfo(f"Existing subscription for {topic} with same message type " - "- keep existing subscriber! " + Logger.localinfo(f"Existing subscription for '{topic}' with same message type " + '- keep existing subscriber! ' f"({len(ProxySubscriberCached._topics[topic]['subscribers'])})") # Register the local callback for topic message @@ -181,7 +182,7 @@ def _callback(cls, msg, topic): @param topic: The topic to which this callback belongs. """ if topic not in ProxySubscriberCached._topics: - Logger.localinfo(f"-- invalid topic={topic} for callback!") + Logger.localinfo(f"-- invalid topic='{topic}' for callback!") return with ProxySubscriberCached._subscription_lock: @@ -198,10 +199,10 @@ def _callback(cls, msg, topic): try: callback(msg) except Exception as exc: # pylint: disable=W0703 - Logger.error(f"Exception in callback for {topic}: " - f"{callback.__module__} {callback.__name__} @ {inst_id} \n {exc} ") + Logger.error(f"Exception in callback for '{topic}': " + f'{callback.__module__} {callback.__name__} @ {inst_id} \n {exc} ') except Exception as exc: # pylint: disable=W0703 - Logger.error(f"Exception in main callback for {topic}: \n {type(exc)} - {exc} ") + Logger.error(f"Exception in main callback for '{topic}': \n {type(exc)} - {exc} ") @classmethod def set_callback(cls, topic, callback, inst_id): @@ -219,7 +220,7 @@ def set_callback(cls, topic, callback, inst_id): (presumes no more than one callback per instance) """ if topic not in ProxySubscriberCached._topics: - Logger.localerr(f"-- invalid topic={topic} for set_callback @inst_id={inst_id}!") + Logger.localerr(f"-- invalid topic='{topic}' for set_callback @inst_id={inst_id}!") return if callback is not None: @@ -232,16 +233,16 @@ def __set_callback(cls, topic, callback, inst_id): try: if inst_id not in ProxySubscriberCached._topics[topic]['callbacks']: ProxySubscriberCached._topics[topic]['callbacks'][inst_id] = callback - Logger.localinfo(f" Set local callback {callback.__name__} of " - f"{len(ProxySubscriberCached._topics[topic]['callbacks'])} for {topic}!") + Logger.localinfo(f" Set local callback '{callback.__name__}' of " + f"{len(ProxySubscriberCached._topics[topic]['callbacks'])} for '{topic}'!") else: - Logger.localinfo("Update existing callback " + Logger.localinfo('Update existing callback ' f"{ProxySubscriberCached._topics[topic]['callbacks'][inst_id].__name__} with " f"{callback.__name__} of {len(ProxySubscriberCached._topics[topic]['callbacks'])}" - f" for {topic}!") + f" for '{topic}'!") ProxySubscriberCached._topics[topic]['callbacks'][inst_id] = callback except KeyError: - Logger.localwarn("Error: topic {topic} is not longer available - cannot set callback!") + Logger.localwarn(f"Error: topic '{topic}' is not longer available - cannot set callback!") @classmethod def enable_buffer(cls, topic): @@ -332,7 +333,6 @@ def remove_last_msg(cls, topic, clear_buffer=False): @type topic: boolean @param topic: Set to true if the buffer of the given topic should be cleared as well. """ - with ProxySubscriberCached._subscription_lock: if topic in ProxySubscriberCached._persistant_topics: return @@ -342,7 +342,7 @@ def remove_last_msg(cls, topic, clear_buffer=False): if clear_buffer: ProxySubscriberCached._topics[topic]['msg_queue'] = deque() except KeyError: - Logger.localwarn(f"remove_last_msg: {topic} is not available!") + Logger.localwarn(f"remove_last_msg: '{topic}' is not available!") @classmethod def make_persistant(cls, topic): @@ -375,7 +375,7 @@ def unsubscribe_topic(cls, topic, inst_id=-1): topic_dict = ProxySubscriberCached._topics[topic] if inst_id in topic_dict['subscribers']: topic_dict['subscribers'].remove(inst_id) - Logger.localdebug(f"Unsubscribed {topic} from proxy! " + Logger.localdebug(f"Unsubscribed '{topic}' from proxy! " f"({len(topic_dict['subscribers'])} remaining)") remaining_subscribers = len(topic_dict['subscribers']) @@ -387,23 +387,23 @@ def unsubscribe_topic(cls, topic, inst_id=-1): elif inst_id in topic_dict['callbacks']: # Remove callback in executor thread to avoid changing size during callback ProxySubscriberCached._node.executor.create_task(topic_dict['callbacks'].pop, inst_id) - Logger.localdebug(f"Removed callback from proxy subscription for {topic} " + Logger.localdebug(f"Removed callback from proxy subscription for '{topic}' " f"from proxy! ({len(topic_dict['callbacks'])} remaining)") except Exception as exc: # pylint: disable=W0703 - Logger.error(f'Something went wrong unsubscribing {topic} of proxy subscriber!\n%s', str(exc)) + Logger.error(f"Something went wrong unsubscribing '{topic}' of proxy subscriber!\n{str(exc)}") else: - Logger.localinfo(f"Unsubscribe : {topic} is not subscribed!") + Logger.localinfo(f"Unsubscribe : '{topic}' is not subscribed!") @classmethod def destroy_subscription(cls, sub, topic): """Handle subscription destruction from within the executor threads.""" try: if ProxySubscriberCached._node.destroy_subscription(sub): - Logger.localinfo(f'Destroyed the proxy subscription for {topic}!') + Logger.localinfo(f"Destroyed the proxy subscription for '{topic}'!") else: - Logger.localwarn(f'Some issue destroying the proxy subscription for {topic}!') + Logger.localwarn(f"Some issue destroying the proxy subscription for '{topic}'!") del sub except Exception as exc: # pylint: disable=W0703 - Logger.error("Something went wrong destroying subscription" - f" for {topic}!\n {type(exc)} - {str(exc)}") + Logger.error('Something went wrong destroying subscription' + f" for '{topic}'!\n {type(exc)} - {str(exc)}") diff --git a/flexbe_core/flexbe_core/proxy/proxy_transform_listener.py b/flexbe_core/flexbe_core/proxy/proxy_transform_listener.py index 0d146f3..03b654e 100644 --- a/flexbe_core/flexbe_core/proxy/proxy_transform_listener.py +++ b/flexbe_core/flexbe_core/proxy/proxy_transform_listener.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -31,9 +31,10 @@ """Provide a single tf listener per node for all states that need tf operations.""" -import tf2_ros from flexbe_core.logger import Logger +import tf2_ros + class ProxyTransformListener: """Provide a single tf listener per node for all states that need tf operations.""" @@ -54,7 +55,7 @@ def shutdown(): try: ProxyTransformListener._listener = None ProxyTransformListener._buffer = None - print("Shutdown proxy transform listener - finished!") + print('Shutdown proxy transform listener - finished!') except Exception as exc: # pylint: disable=W0703 print(f'Something went wrong during shutdown of proxy transform listener !\n{str(exc)}') diff --git a/flexbe_core/flexbe_core/proxy/qos.py b/flexbe_core/flexbe_core/proxy/qos.py index 0f80cab..ec30d9b 100644 --- a/flexbe_core/flexbe_core/proxy/qos.py +++ b/flexbe_core/flexbe_core/proxy/qos.py @@ -1,4 +1,4 @@ -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -28,7 +28,7 @@ """Provide default QoS settings for FlexBE.""" -from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSReliabilityPolicy +from rclpy.qos import QoSDurabilityPolicy, QoSProfile, QoSReliabilityPolicy # see https://index.ros.org/doc/ros2/Concepts/About-Quality-of-Service-Settings for details QOS_DEFAULT = QoSProfile(depth=10) # default queue_size setting diff --git a/flexbe_core/flexbe_core/state_logger.py b/flexbe_core/flexbe_core/state_logger.py index 0523ad6..bb33f0c 100644 --- a/flexbe_core/flexbe_core/state_logger.py +++ b/flexbe_core/flexbe_core/state_logger.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -31,21 +31,21 @@ """Logger for active state information.""" -from functools import wraps, partial - import logging import logging.config import os import pickle import time -import yaml +from functools import partial, wraps + +from flexbe_core.core.topics import Topics +from flexbe_core.proxy import ProxyPublisher from rclpy.exceptions import ParameterNotDeclaredException from std_msgs.msg import String -from flexbe_core.core.topics import Topics -from flexbe_core.proxy import ProxyPublisher +import yaml class StateLogger: @@ -64,6 +64,7 @@ class StateLogger: @staticmethod def initialize_ros(node): + """Initialize ROS nodes.""" StateLogger._node = node try: StateLogger._log_folder = node.get_parameter('log_folder') @@ -92,9 +93,10 @@ def initialize_ros(node): @staticmethod def initialize(be_name=None): + """Initialize the state logger.""" log_folder = os.path.expanduser(StateLogger._log_folder.get_parameter_value().string_value) - if log_folder == "" or not StateLogger._log_enabled.get_parameter_value().bool_value: + if log_folder == '' or not StateLogger._log_enabled.get_parameter_value().bool_value: StateLogger.enabled = False return StateLogger.enabled = True @@ -102,9 +104,9 @@ def initialize(be_name=None): if not os.path.exists(log_folder): os.makedirs(log_folder) - name = "states" + name = 'states' if be_name is not None: - name = be_name.replace(" ", "_").replace(",", "_").replace(".", "_").replace("/", "_").lower() + name = be_name.replace(' ', '_').replace(',', '_').replace('.', '_').replace('/', '_').lower() StateLogger._serialize_impl = StateLogger._log_serialize.get_parameter_value().string_value @@ -126,12 +128,13 @@ def initialize(be_name=None): }, 'loggers': {'flexbe': {'level': 'INFO', 'handlers': ['file']}} }, **yaml.safe_load(StateLogger._log_config.get_parameter_value().string_value)) - if ('handlers' in logger_config and 'file' in logger_config['handlers'] + if ('handlers' in logger_config + and 'file' in logger_config['handlers'] and 'filename' in logger_config['handlers']['file']): logger_config['handlers']['file']['filename'] %= { 'log_folder': log_folder, 'behavior': name, - 'timestamp': time.strftime("%Y-%m-%d-%H_%M_%S") + 'timestamp': time.strftime('%Y-%m-%d-%H_%M_%S') } if 'loggers' in logger_config and 'flexbe' in logger_config['loggers']: level = StateLogger._log_level.get_parameter_value().string_value @@ -140,6 +143,7 @@ def initialize(be_name=None): @staticmethod def shutdown(): + """Shutdown state logging.""" if not StateLogger.enabled: return logging.shutdown() @@ -279,6 +283,7 @@ class YamlFormatter(logging.Formatter): """Special yaml formatting class.""" def format(self, record): + """Format yaml with prefix.""" record.msg.update(logger=record.name, loglevel=record.levelname) return '- %s' % super().format(record) @@ -287,10 +292,12 @@ class PublishBehaviorLogMessage(logging.Handler): """publish messages to behavior logs.""" def __init__(self, level=logging.NOTSET, topic=Topics._STATE_LOGGER_TOPIC): + """Initialize instance.""" super().__init__(level) self._topic = topic self._pub = ProxyPublisher({self._topic: String}) def emit(self, record): + """Emit message.""" message = self.format(record) self._pub.publish(self._topic, String(data=message)) diff --git a/flexbe_core/setup.py b/flexbe_core/setup.py index 10ecb70..0ea7655 100644 --- a/flexbe_core/setup.py +++ b/flexbe_core/setup.py @@ -1,6 +1,6 @@ """Setup package for flexbe_core package.""" -from setuptools import setup from setuptools import find_packages +from setuptools import setup PACKAGE_NAME = 'flexbe_core' diff --git a/flexbe_core/test/flexbe_core_test.py b/flexbe_core/test/flexbe_core_test.py index 3427739..960f330 100644 --- a/flexbe_core/test/flexbe_core_test.py +++ b/flexbe_core/test/flexbe_core_test.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -32,14 +32,17 @@ """Test description.""" import os import sys + import launch + import launch_testing.actions + import pytest @pytest.mark.rostest def generate_test_description(): - + """Generate test description for flexbe_core_test.""" path_to_test = os.path.dirname(__file__) test_proc_path = os.path.join(path_to_test, 'test_core.py') diff --git a/flexbe_core/test/flexbe_exceptions_test.py b/flexbe_core/test/flexbe_exceptions_test.py index a745542..07355ee 100644 --- a/flexbe_core/test/flexbe_exceptions_test.py +++ b/flexbe_core/test/flexbe_exceptions_test.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -32,14 +32,17 @@ """Test setup.""" import os import sys + import launch + import launch_testing.actions + import pytest @pytest.mark.rostest def generate_test_description(): - + """Generate test description for flexbe_exceptions_test.""" path_to_test = os.path.dirname(__file__) test_proc_path = os.path.join(path_to_test, 'test_exceptions.py') diff --git a/flexbe_core/test/flexbe_logger_test.py b/flexbe_core/test/flexbe_logger_test.py index e182d2f..69cd4f2 100644 --- a/flexbe_core/test/flexbe_logger_test.py +++ b/flexbe_core/test/flexbe_logger_test.py @@ -32,14 +32,17 @@ """Test description for test proxies.""" import os import sys + import launch + import launch_testing.actions + import pytest @pytest.mark.rostest def generate_test_description(): - + """Generate test descriptoin for flexbe_logger test.""" path_to_test = os.path.dirname(__file__) TEST_PROC_PATH = os.path.join(path_to_test, 'test_logger.py') diff --git a/flexbe_core/test/flexbe_proxies_test.py b/flexbe_core/test/flexbe_proxies_test.py index b415c48..75e18b2 100644 --- a/flexbe_core/test/flexbe_proxies_test.py +++ b/flexbe_core/test/flexbe_proxies_test.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -32,14 +32,17 @@ """Test description for test proxies.""" import os import sys + import launch + import launch_testing.actions + import pytest @pytest.mark.rostest def generate_test_description(): - + """Generate test description for flexbe_proxies_test.""" path_to_test = os.path.dirname(__file__) TEST_PROC_PATH = os.path.join(path_to_test, 'test_proxies.py') diff --git a/flexbe_core/test/test_core.py b/flexbe_core/test/test_core.py index 3953d4c..8d1dd0b 100755 --- a/flexbe_core/test/test_core.py +++ b/flexbe_core/test/test_core.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -33,17 +33,18 @@ import time import unittest -import rclpy -from rclpy.executors import MultiThreadedExecutor - -from std_msgs.msg import Bool, Empty, UInt32, String -from flexbe_msgs.msg import CommandFeedback, OutcomeRequest - -from flexbe_core.proxy import initialize_proxies, shutdown_proxies, ProxySubscriberCached -from flexbe_core import EventState, OperatableStateMachine, ConcurrencyContainer +from flexbe_core import ConcurrencyContainer, EventState, OperatableStateMachine from flexbe_core.core import PreemptableState from flexbe_core.core.exceptions import StateMachineError from flexbe_core.core.topics import Topics +from flexbe_core.proxy import ProxySubscriberCached, initialize_proxies, shutdown_proxies + +from flexbe_msgs.msg import CommandFeedback, OutcomeRequest + +import rclpy +from rclpy.executors import MultiThreadedExecutor + +from std_msgs.msg import Bool, Empty, String, UInt32 class CoreTestState(EventState): @@ -52,6 +53,7 @@ class CoreTestState(EventState): _set_state_id = 256 def __init__(self): + """Initialize CoreTestState instance.""" super().__init__(outcomes=['done', 'error']) self.result = None self.last_events = [] @@ -60,25 +62,32 @@ def __init__(self): self._state_id = CoreTestState._set_state_id def execute(self, userdata): + """Execute.""" self.count += 1 return self.result def on_enter(self, userdata): + """Call on enter.""" self.last_events.append('on_enter') def on_exit(self, userdata): + """Call on exit.""" self.last_events.append('on_exit') def on_start(self): + """Call on start.""" self.last_events.append('on_start') def on_stop(self): + """Call on stop.""" self.last_events.append('on_stop') def on_pause(self): + """Call on pause.""" self.last_events.append('on_pause') def on_resume(self, userdata): + """Call on resume.""" self.last_events.append('on_resume') @@ -89,16 +98,16 @@ def __init__(self): super().__init__() # def execute(self, userdata): # outcome = super().execute(userdata) - # self._node.get_logger().info(" %s - ConcurrencyTestState execute" - # " out=%s count= %d" % (self._name, str(outcome), self.count)) + # self._node.get_logger().info(' %s - ConcurrencyTestState execute' + # ' out=%s count= %d' % (self._name, str(outcome), self.count)) # return outcome # def on_enter(self, userdata): - # self._node.get_logger().info(" %s - ConcurrencyTestState on_enter" % (self._name)) + # self._node.get_logger().info(' %s - ConcurrencyTestState on_enter' % (self._name)) # super().on_enter(userdata) # def on_exit(self, userdata): - # self._node.get_logger().info(" %s - ConcurrencyTestState on_exit count=%d" % (self._name, self.count)) + # self._node.get_logger().info(' %s - ConcurrencyTestState on_exit count=%d' % (self._name, self.count)) # super().on_exit(userdata) @@ -108,17 +117,19 @@ class TestCore(unittest.TestCase): test = 0 def __init__(self, *args, **kwargs): + """Initialize TestCore instance.""" super().__init__(*args, **kwargs) def setUp(self): + """Set up the TestCore test.""" TestCore.test += 1 self.context = rclpy.context.Context() rclpy.init(context=self.context) self.executor = MultiThreadedExecutor(context=self.context) - self.node = rclpy.create_node("core_test_" + str(self.test), context=self.context) - # self.node.get_logger().info(" set up core test %d (%d)... " % (self.test, self.context.ok())) + self.node = rclpy.create_node('core_test_' + str(self.test), context=self.context) + # self.node.get_logger().info(' set up core test %d (%d)... ' % (self.test, self.context.ok())) self.executor.add_node(self.node) initialize_proxies(self.node) @@ -126,16 +137,17 @@ def setUp(self): time.sleep(0.1) def tearDown(self): - self.node.get_logger().info(" shutting down core test %d ... " % (self.test)) + """Tear down the TestCore test.""" + self.node.get_logger().info(' shutting down core test %d ... ' % (self.test)) for _ in range(50): # Allow any lingering pub/sub to clear up rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.01) - self.node.get_logger().info(" shutting down proxies in core test %d ... " % (self.test)) + self.node.get_logger().info(' shutting down proxies in core test %d ... ' % (self.test)) shutdown_proxies() time.sleep(0.1) - self.node.get_logger().info(" destroy node in core test %d ... " % (self.test)) + self.node.get_logger().info(' destroy node in core test %d ... ' % (self.test)) self.node.destroy_node() time.sleep(0.1) @@ -147,6 +159,7 @@ def tearDown(self): time.sleep(0.2) def _create(self): + """Create the test.""" CoreTestState.initialize_ros(self.node) OperatableStateMachine.initialize_ros(self.node) state = CoreTestState() @@ -160,17 +173,19 @@ def _create(self): return state, sm def _execute(self, state): - # self.node.get_logger().info(" execute %s ... " % (str(state.name))) + """Execute the test.""" + # self.node.get_logger().info(' execute %s ... ' % (str(state.name))) rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) state.last_events = [] outcome = state.parent.execute(None) - # self.node.get_logger().info(" outcome = %s ... " % (str(outcome))) + # self.node.get_logger().info(' outcome = %s ... ' % (str(outcome))) rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) return outcome def assertMessage(self, sub, topic, msg, timeout=1): + """Check message.""" for _ in range(int(timeout * 100)): rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) if sub.has_msg(topic): @@ -184,21 +199,22 @@ def assertMessage(self, sub, topic, msg, timeout=1): if topic in Topics._topic_types: self.assertIsInstance(received, Topics.get_type(topic), - f"Failed message type for {topic} - was {type(received)}" - f" not {Topics.get_type(topic).__name__}") + f"Failed message type for '{topic}' - was '{type(received)}'" + f" not '{Topics.get_type(topic).__name__}'") else: - print(f" Unknown topic {topic} - not standard FlexBE topic - skip type test!", flush=True) + print(f" Unknown topic '{topic}' - not standard FlexBE topic - skip type test!", flush=True) for slot in msg.__slots__: expected = getattr(msg, slot) actual = getattr(received, slot) - error = "Mismatch for %s, is %s but expected %s" % (slot, actual, expected) + error = "Mismatch for '%s', is %s but expected %s" % (slot, actual, expected) if isinstance(expected, list): self.assertListEqual(expected, actual, error) else: self.assertEqual(expected, actual, error) def assertNoMessage(self, sub, topic, timeout=1): + """Assert no message received.""" for _ in range(int(timeout * 100)): rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) if sub.has_msg(topic): @@ -210,7 +226,8 @@ def assertNoMessage(self, sub, topic, timeout=1): # Test Cases def test_event_state(self): - self.node.get_logger().info("test_event_state ... ") + """Test event state.""" + self.node.get_logger().info('test_event_state ... ') rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) ProxySubscriberCached.initialize(self.node) @@ -234,7 +251,7 @@ def test_event_state(self): self._execute(state) self.assertListEqual(['on_pause'], state.last_events) - self.assertMessage(sub, fb_topic, CommandFeedback(command="pause")) + self.assertMessage(sub, fb_topic, CommandFeedback(command='pause')) state.result = 'error' outcome = self._execute(state) @@ -244,13 +261,13 @@ def test_event_state(self): state._sub._callback(Bool(data=False), Topics._CMD_PAUSE_TOPIC) self._execute(state) self.assertListEqual(['on_resume'], state.last_events) - self.assertMessage(sub, fb_topic, CommandFeedback(command="resume")) + self.assertMessage(sub, fb_topic, CommandFeedback(command='resume')) # repeat triggers exit and enter again state._sub._callback(Empty(), Topics._CMD_REPEAT_TOPIC) self._execute(state) self.assertListEqual(['on_exit'], state.last_events) - self.assertMessage(sub, fb_topic, CommandFeedback(command="repeat")) + self.assertMessage(sub, fb_topic, CommandFeedback(command='repeat')) self._execute(state) self.assertListEqual(['on_enter'], state.last_events) @@ -263,15 +280,16 @@ def test_event_state(self): self.assertListEqual(['on_exit'], state.last_events) self.assertEqual('done', outcome) - self.node.get_logger().info("test_event_state - OK") + self.node.get_logger().info('test_event_state - OK') def test_operatable_state(self): - self.node.get_logger().info("test_operatable_state ... ") + """Test operatable state.""" + self.node.get_logger().info('test_operatable_state ... ') rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) ProxySubscriberCached.initialize(self.node) state, sm = self._create() - self.node.get_logger().info("test_operatable_state - ProxySubscribe request ...") + self.node.get_logger().info('test_operatable_state - ProxySubscribe request ...') out_topic = Topics._OUTCOME_TOPIC req_topic = Topics._OUTCOME_REQUEST_TOPIC sub = ProxySubscriberCached({out_topic: UInt32, req_topic: OutcomeRequest}, inst_id=id(self)) @@ -282,7 +300,7 @@ def test_operatable_state(self): time.sleep(0.1) # return outcome in full autonomy, no request - self.node.get_logger().info("test_operatable_state - request outcome on full autonomy, no request ...") + self.node.get_logger().info('test_operatable_state - request outcome on full autonomy, no request ...') state.result = 'error' self._execute(state) @@ -290,7 +308,7 @@ def test_operatable_state(self): self.assertMessage(sub, out_topic, UInt32(data=(2 + state._state_id))) # Test presumes outcome = 1 + offset # request outcome on same autonomy and clear request on loopback - self.node.get_logger().info("test_operatable_state - request outcome on autonomy level=2 ...") + self.node.get_logger().info('test_operatable_state - request outcome on autonomy level=2 ...') OperatableStateMachine.autonomy_level = 2 self._execute(state) self.assertNoMessage(sub, out_topic) @@ -301,34 +319,35 @@ def test_operatable_state(self): self.assertMessage(sub, req_topic, OutcomeRequest(outcome=255, target='/subject')) # still return other outcomes - self.node.get_logger().info("test_operatable_state - still return other outcomes") + self.node.get_logger().info('test_operatable_state - still return other outcomes') state.result = 'done' self._execute(state) self.assertNoMessage(sub, req_topic) self.assertMessage(sub, out_topic, UInt32(data=(1 + state._state_id))) # Test presumes outcome = 0 + offset # request outcome on lower autonomy, return outcome after level increase - self.node.get_logger().info("test_operatable_state - lower autonomy level=0") + self.node.get_logger().info('test_operatable_state - lower autonomy level=0') OperatableStateMachine.autonomy_level = 0 self._execute(state) self.assertNoMessage(sub, out_topic) self.assertMessage(sub, req_topic, OutcomeRequest(outcome=0, target='/subject')) OperatableStateMachine.autonomy_level = 3 - self.node.get_logger().info("test_operatable_state -autonomy level=3") + self.node.get_logger().info('test_operatable_state -autonomy level=3') self._execute(state) self.assertMessage(sub, out_topic, UInt32(data=(1 + state._state_id))) # Test presumes outcome = 0 + offset - self.node.get_logger().info("test_operatable_state - OK! ") + self.node.get_logger().info('test_operatable_state - OK! ') def test_preemptable_state(self): - self.node.get_logger().info("test_preemptable_state ... ") + """Test preemptable state.""" + self.node.get_logger().info('test_preemptable_state ... ') rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) ProxySubscriberCached.initialize(self.node) state, sm = self._create() fb_topic = Topics._CMD_FEEDBACK_TOPIC - self.node.get_logger().info("test_preemptable_state - subscribers ...") + self.node.get_logger().info('test_preemptable_state - subscribers ...') sub = ProxySubscriberCached({fb_topic: CommandFeedback}, inst_id=id(self)) # wait for pub/sub @@ -340,18 +359,18 @@ def test_preemptable_state(self): # preempt when trigger variable is set PreemptableState.preempt = True - self.node.get_logger().info("test_preemptable_state - preempt = True ...") + self.node.get_logger().info('test_preemptable_state - preempt = True ...') outcome = self._execute(state) self.assertEqual(outcome, PreemptableState._preempted_name) self.assertRaises(StateMachineError, lambda: state.parent.current_state) - self.node.get_logger().info("test_preemptable_state - preempt = False ...") + self.node.get_logger().info('test_preemptable_state - preempt = False ...') PreemptableState.preempt = False outcome = self._execute(state) self.assertIsNone(outcome) # preempt when command is received - self.node.get_logger().info("test_preemptable_state - preempt on command ...") + self.node.get_logger().info('test_preemptable_state - preempt on command ...') state._sub._callback(Empty(), Topics._CMD_PREEMPT_TOPIC) outcome = self._execute(state) @@ -360,10 +379,11 @@ def test_preemptable_state(self): self.assertMessage(sub, fb_topic, CommandFeedback(command='preempt')) PreemptableState.preempt = False - self.node.get_logger().info("test_preemptable_state - OK! ") + self.node.get_logger().info('test_preemptable_state - OK! ') def test_lockable_state(self): - self.node.get_logger().info("test_lockable_state ... ") + """Test lockable state.""" + self.node.get_logger().info('test_lockable_state ... ') rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) ProxySubscriberCached.initialize(self.node) @@ -373,25 +393,25 @@ def test_lockable_state(self): time.sleep(0.2) # lock and unlock as commanded, return outcome after unlock - self.node.get_logger().info(" test lock on command ... ") + self.node.get_logger().info(' test lock on command ... ') rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) state._sub._callback(String(data='/subject'), Topics._CMD_LOCK_TOPIC) rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) state.result = 'done' - self.node.get_logger().info(" execute state after lock ... ") + self.node.get_logger().info(' execute state after lock ... ') rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) outcome = self._execute(state) rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) - self.node.get_logger().info(" check results of test lock after execute ... ") + self.node.get_logger().info(' check results of test lock after execute ... ') rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) self.assertIsNone(outcome) self.assertTrue(state._locked) self.assertMessage(sub, fb_topic, CommandFeedback(command='lock', args=['/subject', '/subject'])) state.result = None - self.node.get_logger().info(" test unlock on command ... ") + self.node.get_logger().info(' test unlock on command ... ') rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) state._sub._callback(String(data='/subject'), Topics._CMD_UNLOCK_TOPIC) rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) @@ -400,7 +420,7 @@ def test_lockable_state(self): self.assertMessage(sub, fb_topic, CommandFeedback(command='unlock', args=['/subject', '/subject'])) # lock and unlock without target - self.node.get_logger().info(" test lock and unlock without target ... ") + self.node.get_logger().info(' test lock and unlock without target ... ') state._sub._callback(String(data=''), Topics._CMD_LOCK_TOPIC) state.result = 'done' outcome = self._execute(state) @@ -412,20 +432,20 @@ def test_lockable_state(self): self.assertMessage(sub, fb_topic, CommandFeedback(command='unlock', args=['/subject', '/subject'])) # reject invalid lock command - self.node.get_logger().info(" test reject invalid lock command ... ") + self.node.get_logger().info(' test reject invalid lock command ... ') state._sub._callback(String(data='/invalid'), Topics._CMD_LOCK_TOPIC) outcome = self._execute(state) self.assertEqual(outcome, 'done') self.assertMessage(sub, fb_topic, CommandFeedback(command='lock', args=['/invalid', ''])) # reject generic unlock command when not locked - self.node.get_logger().info(" test reject invalid unlock when not locked command ... ") + self.node.get_logger().info(' test reject invalid unlock when not locked command ... ') state._sub._callback(String(data=''), Topics._CMD_UNLOCK_TOPIC) self._execute(state) self.assertMessage(sub, fb_topic, CommandFeedback(command='unlock', args=['', ''])) # do not transition out of locked container - self.node.get_logger().info(" test do not transition out of locked container ... ") + self.node.get_logger().info(' test do not transition out of locked container ... ') state.parent._locked = True outcome = self._execute(state) self.assertIsNone(outcome) @@ -433,10 +453,11 @@ def test_lockable_state(self): state.result = None outcome = self._execute(state) self.assertEqual(outcome, 'done') - self.node.get_logger().info("test_lockable_state - OK! ") + self.node.get_logger().info('test_lockable_state - OK! ') def test_manually_transitionable_state(self): - self.node.get_logger().info("test_manually_transitionable_state ...") + """Test manually transitionable state.""" + self.node.get_logger().info('test_manually_transitionable_state ...') rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) ProxySubscriberCached.initialize(self.node) @@ -456,10 +477,11 @@ def test_manually_transitionable_state(self): outcome = self._execute(state) self.assertIsNone(outcome) self.assertMessage(sub, fb_topic, CommandFeedback(command='transition', args=['invalid', 'subject'])) - self.node.get_logger().info("test_manually_transitionable_state - OK! ") + self.node.get_logger().info('test_manually_transitionable_state - OK! ') def test_cross_combinations(self): - self.node.get_logger().info("test_cross_combinations ...") + """Test cross combinations.""" + self.node.get_logger().info('test_cross_combinations ...') rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) state, sm = self._create() @@ -476,7 +498,7 @@ def test_cross_combinations(self): state.result = None # manual transition blocked by lock - self.node.get_logger().info("test_cross_combinations - check manual transition blocked by lock ... ") + self.node.get_logger().info('test_cross_combinations - check manual transition blocked by lock ... ') state._sub._callback(String(data='/subject'), Topics._CMD_LOCK_TOPIC) outcome = self._execute(state) self.assertIsNone(outcome) @@ -488,7 +510,7 @@ def test_cross_combinations(self): self.assertEqual(outcome, 'error') # preempt works on low autonomy - self.node.get_logger().info("test_cross_combinations - verify preempt works in low autonomy ... ") + self.node.get_logger().info('test_cross_combinations - verify preempt works in low autonomy ... ') OperatableStateMachine.autonomy_level = 0 state.result = 'error' outcome = self._execute(state) @@ -501,7 +523,7 @@ def test_cross_combinations(self): state.result = None # preempt also works when locked - self.node.get_logger().info("test_cross_combinations - verify preempt works when locked ... ") + self.node.get_logger().info('test_cross_combinations - verify preempt works when locked ... ') state._sub._callback(String(data='/subject'), Topics._CMD_LOCK_TOPIC) outcome = self._execute(state) self.assertIsNone(outcome) @@ -512,10 +534,11 @@ def test_cross_combinations(self): state._sub._callback(String(data='/subject'), Topics._CMD_UNLOCK_TOPIC) outcome = self._execute(state) self.assertIsNone(outcome) - self.node.get_logger().info("test_cross_combinations - OK! ") + self.node.get_logger().info('test_cross_combinations - OK! ') def test_concurrency_container(self): - self.node.get_logger().info("test_concurrency_container ... ") + """Test CC.""" + self.node.get_logger().info('test_concurrency_container ... ') rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) ConcurrencyTestState.initialize_ros(self.node) ConcurrencyContainer.initialize_ros(self.node) @@ -538,7 +561,7 @@ def test_concurrency_container(self): transitions={'done': 'done', 'error': 'error'}, autonomy={'done': 1, 'error': 2}) - # self.node.get_logger().info(" after setting up OSM - call execute CC ... ") + # self.node.get_logger().info(' after setting up OSM - call execute CC ... ') rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) # all states are called with their correct rate @@ -547,8 +570,8 @@ def test_concurrency_container(self): try: self.assertAlmostEqual(cc.sleep_duration, .1, places=2) except AssertionError: # pylint: disable=W0703 - self.node.get_logger().warn(f" Caught error with cc.sleep_duration = {cc.sleep_duration:.6f} =/= 0.1 " - f"- Sometimes fails if OS interruption! ... ") + self.node.get_logger().warn(f' Caught error with cc.sleep_duration = {cc.sleep_duration:.6f} =/= 0.1 ' + f'- Sometimes fails if OS interruption! ... ') # Not controlled yet (e.g. as if no UI connected) self.assertFalse(cc['main']._is_controlled) @@ -564,12 +587,12 @@ def test_concurrency_container(self): test_start_time = self.node.get_clock().now() start_time = time.time() elapsed = None - self.node.get_logger().info(" test timing loop ... ") + self.node.get_logger().info(' test timing loop ... ') while (self.node.get_clock().now() - test_start_time).nanoseconds < 1000000001: # 1 second cc_count += 1 rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.0005) outcome = cc.execute(None) - self.node.get_logger().info(" cc_count=%d (%d, %d) outcome=%s duration=%s (%s, %s) last= (%s, %s) now=%s" % + self.node.get_logger().info(' cc_count=%d (%d, %d) outcome=%s duration=%s (%s, %s) last= (%s, %s) now=%s' % (cc_count, cc['main'].count, cc['side'].count, str(outcome), str(cc.sleep_duration), @@ -590,7 +613,7 @@ def test_concurrency_container(self): self.assertIn(cc['side'].count, [9, 10, 11]) self.assertLessEqual(cc_count, 30) # 1 / (1/10 - 1/15) - self.node.get_logger().info(" verify ROS properties ... ") + self.node.get_logger().info(' verify ROS properties ... ') # verify ROS properties cc._enable_ros_control() @@ -601,7 +624,7 @@ def test_concurrency_container(self): cc['main'].set_rate(1.e16) # run every time cc['side'].set_rate(1.e16) # run every time - self.node.get_logger().info(" verify outcomes ... ") + self.node.get_logger().info(' verify outcomes ... ') outcome = cc.execute(None) self.assertIsNone(outcome) @@ -614,7 +637,7 @@ def test_concurrency_container(self): outcome = cc.execute(None) self.assertEqual(outcome, 'error') - self.node.get_logger().info(" verify outcome only if both done (as set by conditions above) ... ") + self.node.get_logger().info(' verify outcome only if both done (as set by conditions above) ... ') cc['main'].result = None cc['side'].result = None outcome = cc.execute(None) @@ -626,18 +649,18 @@ def test_concurrency_container(self): self.assertIsNone(outcome) cc['main'].result = 'done' - self.node.get_logger().info(" verify both done returns outcome(as set by conditions above) ... ") + self.node.get_logger().info(' verify both done returns outcome(as set by conditions above) ... ') outcome = cc.execute(None) self.assertEqual(outcome, 'done') # always call on_exit exactly once when returning an outcome - self.node.get_logger().info(" verify on_exit called once and only once ... ") + self.node.get_logger().info(' verify on_exit called once and only once ... ') cc['main'].result = None cc['side'].result = None outcome = cc.execute(None) self.assertIsNone(outcome) - self.node.get_logger().info(" check last events ... ") + self.node.get_logger().info(' check last events ... ') cc['main'].last_events = [] cc['side'].last_events = [] cc['main'].result = 'error' @@ -646,10 +669,11 @@ def test_concurrency_container(self): self.assertListEqual(cc['main'].last_events, ['on_exit']) self.assertListEqual(cc['side'].last_events, ['on_exit']) - self.node.get_logger().info("test_concurrency_container - OK! ") + self.node.get_logger().info('test_concurrency_container - OK! ') def test_user_data(self): - self.node.get_logger().info("test_user_data ...") + """Test user data.""" + self.node.get_logger().info('test_user_data ...') rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) @@ -731,7 +755,7 @@ def execute(self, userdata): outcome = sm.execute(None) self.assertEqual(sm['final_state'].data, 'last_data') self.assertEqual(outcome, 'done') - self.node.get_logger().info("test_user_data - OK! ") + self.node.get_logger().info('test_user_data - OK! ') if __name__ == '__main__': diff --git a/flexbe_core/test/test_exceptions.py b/flexbe_core/test/test_exceptions.py index 10f145a..d5d27d4 100755 --- a/flexbe_core/test/test_exceptions.py +++ b/flexbe_core/test/test_exceptions.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -33,13 +33,13 @@ import time import unittest -import rclpy - -from rclpy.executors import MultiThreadedExecutor from flexbe_core import EventState, OperatableStateMachine from flexbe_core.core.exceptions import StateError, StateMachineError, UserDataError from flexbe_core.proxy import initialize_proxies, shutdown_proxies +import rclpy +from rclpy.executors import MultiThreadedExecutor + class TestExceptions(unittest.TestCase): """Test FlexBE Exception handling.""" @@ -47,28 +47,31 @@ class TestExceptions(unittest.TestCase): test = 0 def __init__(self, *args, **kwargs): + """Initialize TestExceptions instance.""" super().__init__(*args, **kwargs) def setUp(self): + """Set up the TestExceptions test.""" TestExceptions.test += 1 self.context = rclpy.context.Context() rclpy.init(context=self.context) self.executor = MultiThreadedExecutor(context=self.context) - self.node = rclpy.create_node("exception_test_" + str(self.test), context=self.context) - self.node.get_logger().info(" set up exceptions test %d (%d) ... " % (self.test, self.context.ok())) + self.node = rclpy.create_node('exception_test_' + str(self.test), context=self.context) + self.node.get_logger().info(' set up exceptions test %d (%d) ... ' % (self.test, self.context.ok())) self.executor.add_node(self.node) initialize_proxies(self.node) def tearDown(self): - self.node.get_logger().info(" shutting down exceptions test %d (%d) ... " % (self.test, self.context.ok())) + """Tear down the TestExceptions test.""" + self.node.get_logger().info(' shutting down exceptions test %d (%d) ... ' % (self.test, self.context.ok())) rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) - self.node.get_logger().info(" shutting down proxies in core test %d ... " % (self.test)) + self.node.get_logger().info(' shutting down proxies in core test %d ... ' % (self.test)) shutdown_proxies() time.sleep(0.1) - self.node.get_logger().info(" destroy node in core test %d ... " % (self.test)) + self.node.get_logger().info(' destroy node in core test %d ... ' % (self.test)) self.node.destroy_node() time.sleep(0.1) @@ -80,7 +83,8 @@ def tearDown(self): time.sleep(0.2) def test_invalid_outcome(self): - self.node.get_logger().info("test_invalid_outcome ...") + """Test invalid outcome.""" + self.node.get_logger().info('test_invalid_outcome ...') rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) OperatableStateMachine.initialize_ros(self.node) @@ -103,10 +107,11 @@ def execute(self, userdata): outcome = sm.execute(None) self.assertIsNone(outcome) self.assertIsInstance(sm._last_exception, StateError) - self.node.get_logger().info("test_invalid_outcome - OK! ") + self.node.get_logger().info('test_invalid_outcome - OK! ') def test_invalid_transition(self): - self.node.get_logger().info("test_invalid_transition ...") + """Test invalid transition.""" + self.node.get_logger().info('test_invalid_transition ...') rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) OperatableStateMachine.initialize_ros(self.node) @@ -132,10 +137,11 @@ def execute(self, userdata): outcome = sm.execute(None) self.assertIsNone(outcome) self.assertIsInstance(sm._last_exception, StateMachineError) - self.node.get_logger().info("test_invalid_transition - OK! ") + self.node.get_logger().info('test_invalid_transition - OK! ') def test_invalid_userdata_input(self): - self.node.get_logger().info("test_invalid_userdata ...") + """Test invalid user data.""" + self.node.get_logger().info('test_invalid_userdata ...') rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) OperatableStateMachine.initialize_ros(self.node) @@ -159,10 +165,11 @@ def execute(self, userdata): outcome = sm.execute(None) self.assertIsNone(outcome) self.assertIsInstance(sm._last_exception, UserDataError) - self.node.get_logger().info("test_invalid_userdata - OK! ") + self.node.get_logger().info('test_invalid_userdata - OK! ') def test_invalid_userdata_output(self): - self.node.get_logger().info("test_invalid_userdata_output ...") + """Test invalid userdata output.""" + self.node.get_logger().info('test_invalid_userdata_output ...') rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) OperatableStateMachine.initialize_ros(self.node) @@ -186,10 +193,11 @@ def execute(self, userdata): outcome = sm.execute(None) self.assertIsNone(outcome) self.assertIsInstance(sm._last_exception, UserDataError) - self.node.get_logger().info("test_invalid_userdata_output - OK! ") + self.node.get_logger().info('test_invalid_userdata_output - OK! ') def test_missing_userdata(self): - self.node.get_logger().info("test_missing_userdata ...") + """Test missing userdata.""" + self.node.get_logger().info('test_missing_userdata ...') rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) OperatableStateMachine.initialize_ros(self.node) @@ -213,10 +221,11 @@ def execute(self, userdata): outcome = sm.execute(None) self.assertIsNone(outcome) self.assertIsInstance(sm._last_exception, UserDataError) - self.node.get_logger().info("test_missing_userdata - OK! ") + self.node.get_logger().info('test_missing_userdata - OK! ') def test_modify_input_key(self): - self.node.get_logger().info("test_modify_input_key ...! ") + """Test modify input key.""" + self.node.get_logger().info('test_modify_input_key ...! ') rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) OperatableStateMachine.initialize_ros(self.node) @@ -241,7 +250,7 @@ def execute(self, userdata): outcome = sm.execute(None) self.assertIsNone(outcome) self.assertIsInstance(sm._last_exception, UserDataError) - self.node.get_logger().info("test_modify_input_key - OK! ") + self.node.get_logger().info('test_modify_input_key - OK! ') if __name__ == '__main__': diff --git a/flexbe_core/test/test_logger.py b/flexbe_core/test/test_logger.py index 1e2074c..225cf99 100644 --- a/flexbe_core/test/test_logger.py +++ b/flexbe_core/test/test_logger.py @@ -33,13 +33,12 @@ import time import unittest -import rclpy +from flexbe_core import EventState, OperatableStateMachine, set_node +from flexbe_core.logger import Logger +from flexbe_core.proxy import initialize_proxies, shutdown_proxies +import rclpy from rclpy.executors import MultiThreadedExecutor -from flexbe_core import set_node, EventState, OperatableStateMachine -from flexbe_core.core.exceptions import StateError, StateMachineError, UserDataError -from flexbe_core.proxy import initialize_proxies, shutdown_proxies -from flexbe_core.logger import Logger class TestLogger(unittest.TestCase): @@ -48,28 +47,31 @@ class TestLogger(unittest.TestCase): test = 0 def __init__(self, *args, **kwargs): + """Initialize TestLogger instance.""" super().__init__(*args, **kwargs) def setUp(self): + """Set up the test.""" TestLogger.test += 1 self.context = rclpy.context.Context() rclpy.init(context=self.context) self.executor = MultiThreadedExecutor(context=self.context) - self.node = rclpy.create_node("logger_test_" + str(self.test), context=self.context) - self.node.get_logger().info(" set up logger test %d (%d) ... " % (self.test, self.context.ok())) + self.node = rclpy.create_node('logger_test_' + str(self.test), context=self.context) + self.node.get_logger().info(' set up logger test %d (%d) ... ' % (self.test, self.context.ok())) self.executor.add_node(self.node) initialize_proxies(self.node) def tearDown(self): - self.node.get_logger().info(" shutting down logger test %d (%d) ... " % (self.test, self.context.ok())) + """Tear down the test.""" + self.node.get_logger().info(' shutting down logger test %d (%d) ... ' % (self.test, self.context.ok())) rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) - self.node.get_logger().info(" shutting down proxies in logger test %d ... " % (self.test)) + self.node.get_logger().info(' shutting down proxies in logger test %d ... ' % (self.test)) shutdown_proxies() time.sleep(0.1) - self.node.get_logger().info(" destroy node in core test %d ... " % (self.test)) + self.node.get_logger().info(' destroy node in core test %d ... ' % (self.test)) self.node.destroy_node() time.sleep(0.1) @@ -81,9 +83,10 @@ def tearDown(self): time.sleep(0.2) def test_throttle_logger_one(self): - self.node.get_logger().info("test_throttle_logger_one ...") - self.node.declare_parameter("max_throttle_logging_size", 100) - self.node.declare_parameter("throttle_logging_clear_ratio", 0.25) + """Test throttle logger one.""" + self.node.get_logger().info('test_throttle_logger_one ...') + self.node.declare_parameter('max_throttle_logging_size', 100) + self.node.declare_parameter('throttle_logging_clear_ratio', 0.25) set_node(self.node) # Update the logger node rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) @@ -97,10 +100,10 @@ def __init__(self): self.initialize_ros(node) super().__init__(outcomes=['done']) self._trials = Logger.MAX_LAST_LOGGED_SIZE * 2 - Logger.logerr_throttle(0.0, "test") + Logger.logerr_throttle(0.0, 'test') def execute(self, userdata): - Logger.logerr_throttle(0.0, "test") + Logger.logerr_throttle(0.0, 'test') self._trials -= 1 if self._trials == 0: return 'done' @@ -116,16 +119,17 @@ def execute(self, userdata): while outcome is None: outcome = sm.execute(None) self.assertEqual(len(Logger._last_logged), 1) - self.assertEqual(outcome, "done") + self.assertEqual(outcome, 'done') self.assertEqual(state_instance._trials, 0) self.assertIsNone(sm._last_exception) - self.node.get_logger().info("test_throttle_logger_one - OK! ") + self.node.get_logger().info('test_throttle_logger_one - OK! ') def test_throttle_logger_err_multi(self): - self.node.get_logger().info("test_throttle_logger_err_multi ...") - self.node.declare_parameter("max_throttle_logging_size", 200) - self.node.declare_parameter("throttle_logging_clear_ratio", 0.35) + """Test throttle logger with errrors.""" + self.node.get_logger().info('test_throttle_logger_err_multi ...') + self.node.declare_parameter('max_throttle_logging_size', 200) + self.node.declare_parameter('throttle_logging_clear_ratio', 0.35) set_node(self.node) # Update the logger node rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) @@ -139,10 +143,10 @@ def __init__(self): self.initialize_ros(node) super().__init__(outcomes=['done']) self._trials = Logger.MAX_LAST_LOGGED_SIZE * 2 - Logger.logerr_throttle(0.01, f"0_test") + Logger.logerr_throttle(0.01, '0_test') def execute(self, userdata): - Logger.logerr_throttle(0.01, f"{self._trials}_test") + Logger.logerr_throttle(0.01, f'{self._trials}_test') self._trials -= 1 if self._trials == 0: return 'done' @@ -158,16 +162,17 @@ def execute(self, userdata): while outcome is None: outcome = sm.execute(None) self.assertTrue(1 < len(Logger._last_logged) <= Logger.MAX_LAST_LOGGED_SIZE) - self.assertEqual(outcome, "done") + self.assertEqual(outcome, 'done') self.assertEqual(state_instance._trials, 0) self.assertIsNone(sm._last_exception) - self.node.get_logger().info("test_throttle_logger_err_multi - OK! ") + self.node.get_logger().info('test_throttle_logger_err_multi - OK! ') def test_throttle_logger_multiple_params(self): - self.node.get_logger().info("test_throttle_logger_multiple_params ...") - self.node.declare_parameter("max_throttle_logging_size", 100) - self.node.declare_parameter("throttle_logging_clear_ratio", 0.7) + """Test throttle logger with multiple params.""" + self.node.get_logger().info('test_throttle_logger_multiple_params ...') + self.node.declare_parameter('max_throttle_logging_size', 100) + self.node.declare_parameter('throttle_logging_clear_ratio', 0.7) set_node(self.node) # Update the logger node @@ -182,14 +187,14 @@ def __init__(self): self.initialize_ros(node) super().__init__(outcomes=['done']) self._trials = Logger.MAX_LAST_LOGGED_SIZE * 2 - Logger.logerr_throttle(0.01, f"0_test") + Logger.logerr_throttle(0.01, '0_test') def execute(self, userdata): - Logger.logerr(f"{self._trials}_test") - Logger.logerr_throttle(0.0, f"{self._trials}_test") - Logger.logwarn_throttle(0.0, f"{self._trials}_test") - Logger.loginfo_throttle(0.0, f"{self._trials}_test") - Logger.loghint_throttle(0.0, f"{self._trials}_test") + Logger.logerr(f'{self._trials}_test') + Logger.logerr_throttle(0.0, f'{self._trials}_test') + Logger.logwarn_throttle(0.0, f'{self._trials}_test') + Logger.loginfo_throttle(0.0, f'{self._trials}_test') + Logger.loghint_throttle(0.0, f'{self._trials}_test') self._trials -= 1 if self._trials == 0: return 'done' @@ -206,16 +211,17 @@ def execute(self, userdata): outcome = sm.execute(None) self.assertTrue(1 < len(Logger._last_logged) <= Logger.MAX_LAST_LOGGED_SIZE) rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.001) - self.assertEqual(outcome, "done") + self.assertEqual(outcome, 'done') self.assertEqual(state_instance._trials, 0) self.assertIsNone(sm._last_exception) - self.node.get_logger().info("test_throttle_logger_multiple - OK! ") + self.node.get_logger().info('test_throttle_logger_multiple - OK! ') def test_throttle_logger_multiple(self): - self.node.get_logger().info("test_throttle_logger_multiple_params ...") - self.node.declare_parameter("max_throttle_logging_size", 120) - self.node.declare_parameter("throttle_logging_clear_ratio", 0.22) + """Test throttle logger multiple.""" + self.node.get_logger().info('test_throttle_logger_multiple_params ...') + self.node.declare_parameter('max_throttle_logging_size', 120) + self.node.declare_parameter('throttle_logging_clear_ratio', 0.22) set_node(self.node) # Update the logger node rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) @@ -229,14 +235,14 @@ def __init__(self): self.initialize_ros(node) super().__init__(outcomes=['done']) self._trials = Logger.MAX_LAST_LOGGED_SIZE * 2 - Logger.logerr_throttle(0.01, f"0_test") + Logger.logerr_throttle(0.01, '0_test') def execute(self, userdata): - Logger.logerr(f"{self._trials}_test") - Logger.logerr_throttle(0.0, f"{self._trials}_test") - Logger.logwarn_throttle(0.0, f"{self._trials}_test") - Logger.loginfo_throttle(0.0, f"{self._trials}_test") - Logger.loghint_throttle(0.0, f"{self._trials}_test") + Logger.logerr(f'{self._trials}_test') + Logger.logerr_throttle(0.0, f'{self._trials}_test') + Logger.logwarn_throttle(0.0, f'{self._trials}_test') + Logger.loginfo_throttle(0.0, f'{self._trials}_test') + Logger.loghint_throttle(0.0, f'{self._trials}_test') self._trials -= 1 if self._trials == 0: return 'done' @@ -253,11 +259,11 @@ def execute(self, userdata): outcome = sm.execute(None) self.assertTrue(1 < len(Logger._last_logged) <= Logger.MAX_LAST_LOGGED_SIZE) rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.001) - self.assertEqual(outcome, "done") + self.assertEqual(outcome, 'done') self.assertEqual(state_instance._trials, 0) self.assertIsNone(sm._last_exception) - self.node.get_logger().info("test_throttle_logger_multiple_params - OK! ") + self.node.get_logger().info('test_throttle_logger_multiple_params - OK! ') if __name__ == '__main__': diff --git a/flexbe_core/test/test_proxies.py b/flexbe_core/test/test_proxies.py index 5d809c8..97a7006 100755 --- a/flexbe_core/test/test_proxies.py +++ b/flexbe_core/test/test_proxies.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -34,17 +34,20 @@ import time import unittest +from action_msgs.msg import GoalStatus + +from flexbe_core.proxy import ProxyActionClient, ProxyPublisher, ProxyServiceCaller, ProxySubscriberCached +from flexbe_core.proxy import initialize_proxies, shutdown_proxies + +from flexbe_msgs.action import BehaviorExecution + import rclpy from rclpy.action import ActionServer from rclpy.executors import MultiThreadedExecutor -from action_msgs.msg import GoalStatus from std_msgs.msg import String -from std_srvs.srv import Trigger -from flexbe_msgs.action import BehaviorExecution -from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached, ProxyActionClient, ProxyServiceCaller -from flexbe_core.proxy import initialize_proxies, shutdown_proxies +from std_srvs.srv import Trigger class TestProxies(unittest.TestCase): @@ -53,30 +56,33 @@ class TestProxies(unittest.TestCase): test = 0 def __init__(self, *args, **kwargs): + """Initialize TestProxies instance.""" super().__init__(*args, **kwargs) def setUp(self): + """Set up the test.""" TestProxies.test += 1 self.context = rclpy.context.Context() rclpy.init(context=self.context) self.executor = MultiThreadedExecutor(context=self.context) - self.node = rclpy.create_node("proxy_test" + str(self.test), context=self.context) + self.node = rclpy.create_node('proxy_test' + str(self.test), context=self.context) self.executor.add_node(self.node) - self.node.get_logger().info(" set up proxies test %d (%d) ... " % (self.test, self.context.ok())) + self.node.get_logger().info(' set up proxies test %d (%d) ... ' % (self.test, self.context.ok())) initialize_proxies(self.node) def tearDown(self): - self.node.get_logger().info(" shutting down proxies test %d (%d) ... " % (self.test, self.context.ok())) + """Tear down the test.""" + self.node.get_logger().info(' shutting down proxies test %d (%d) ... ' % (self.test, self.context.ok())) rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) - self.node.get_logger().info(" shutting down proxies in core test %d ... " % (self.test)) + self.node.get_logger().info(' shutting down proxies in core test %d ... ' % (self.test)) shutdown_proxies() time.sleep(0.1) - self.node.get_logger().info(" destroy node in core test %d ... " % (self.test)) + self.node.get_logger().info(' destroy node in core test %d ... ' % (self.test)) self.node.destroy_node() time.sleep(0.1) @@ -88,7 +94,8 @@ def tearDown(self): time.sleep(0.5) def test_publish_subscribe(self): - self.node.get_logger().info("test_publish_subscribe ...") + """Test publish and subscribe.""" + self.node.get_logger().info('test_publish_subscribe ...') rclpy.spin_once(self.node, executor=self.executor, timeout_sec=1) ProxyPublisher.initialize(self.node) @@ -97,13 +104,13 @@ def test_publish_subscribe(self): topic1 = '/pubsub_1' topic2 = '/pubsub_2' - self.node.get_logger().info("test_publish_subscribe - define publishers ...") + self.node.get_logger().info('test_publish_subscribe - define publishers ...') pub = ProxyPublisher({topic1: String}) rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) pub = ProxyPublisher({topic2: String}) rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) - self.node.get_logger().info(" subscribe topic1 only ...") + self.node.get_logger().info(' subscribe topic1 only ...') sub = ProxySubscriberCached({topic1: String}, inst_id=id(self)) @@ -120,11 +127,11 @@ def test_publish_subscribe(self): self.assertTrue(pub.number_of_subscribers(topic1) > 0) self.assertFalse(pub.number_of_subscribers(topic2) > 0) - self.node.get_logger().info(" both available ...") + self.node.get_logger().info(' both available ...') self.assertTrue(pub.is_available(topic1)) self.assertTrue(pub.is_available(topic2)) - self.node.get_logger().info(" subscribe topic2 ...") + self.node.get_logger().info(' subscribe topic2 ...') sub = ProxySubscriberCached({topic2: String}, inst_id=id(self)) for _ in range(50): rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.01) @@ -133,11 +140,11 @@ def test_publish_subscribe(self): self.assertTrue(pub.number_of_subscribers(topic1) > 0) self.assertTrue(pub.number_of_subscribers(topic2) > 0) - self.node.get_logger().info(" found both subscribers ...") + self.node.get_logger().info(' found both subscribers ...') self.assertTrue(pub.is_available(topic1)) self.assertTrue(pub.is_available(topic2)) - self.node.get_logger().info(" publish two messages ...") + self.node.get_logger().info(' publish two messages ...') msg1 = String() msg1.data = '1' msg2 = String() @@ -147,7 +154,7 @@ def test_publish_subscribe(self): pub.publish(topic2, msg2) # Make sure messages are sent before checking subscription - self.node.get_logger().info(" listen for two messages ...") + self.node.get_logger().info(' listen for two messages ...') end_time = time.time() + 5 while time.time() < end_time: rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) @@ -155,18 +162,19 @@ def test_publish_subscribe(self): self.assertTrue(sub.has_msg(topic1)) self.assertEqual(sub.get_last_msg(topic1).data, '1') sub.remove_last_msg(topic1) - self.node.get_logger().info(" received on topic1...") + self.node.get_logger().info(' received on topic1...') self.assertFalse(sub.has_msg(topic1)) self.assertIsNone(sub.get_last_msg(topic1)) - self.node.get_logger().info(" check for topic2 ...") + self.node.get_logger().info(' check for topic2 ...') self.assertTrue(sub.has_msg(topic2)) self.assertEqual(sub.get_last_msg(topic2).data, '2') - self.node.get_logger().info("test_publish_subscribe - OK!") + self.node.get_logger().info('test_publish_subscribe - OK!') def test_subscribe_buffer(self): - self.node.get_logger().info("test_subscribe_buffer ...") + """Test subscribe buffer.""" + self.node.get_logger().info('test_subscribe_buffer ...') rclpy.spin_once(self.node, executor=self.executor, timeout_sec=2) ProxyPublisher.initialize(self.node) @@ -213,10 +221,11 @@ def test_subscribe_buffer(self): self.assertEqual(sub.get_from_buffer(topic1).data, '3') self.assertIsNone(sub.get_from_buffer(topic1)) self.assertFalse(sub.has_buffered(topic1)) - self.node.get_logger().info("test_subscribe_buffer - OK! ") + self.node.get_logger().info('test_subscribe_buffer - OK! ') def test_service_caller(self): - self.node.get_logger().info("test_service_caller ...") + """Test service caller.""" + self.node.get_logger().info('test_service_caller ...') rclpy.spin_once(self.node, executor=self.executor, timeout_sec=2) ProxyServiceCaller.initialize(self.node) @@ -225,7 +234,7 @@ def test_service_caller(self): def server_callback(request, response): response.success = True - response.message = "ok" + response.message = 'ok' return response self.node.create_service(Trigger, topic1, server_callback) @@ -246,10 +255,11 @@ def server_callback(request, response): self.assertFalse(srv.is_available('/not_there')) srv = ProxyServiceCaller({'/invalid': Trigger}, wait_duration=.1) self.assertFalse(srv.is_available('/invalid')) - self.node.get_logger().info("test_service_caller - OK! ") + self.node.get_logger().info('test_service_caller - OK! ') def test_action_client(self): - self.node.get_logger().info("test_action_client ...") + """Test action client.""" + self.node.get_logger().info('test_action_client ...') rclpy.spin_once(self.node, executor=self.executor, timeout_sec=2) topic1 = '/action_1' @@ -271,66 +281,66 @@ def execute_cb(goal_handle): client = ProxyActionClient({topic1: BehaviorExecution}, wait_duration=1.0) self.assertFalse(client.has_result(topic1)) status = client.get_state(topic1) - self.node.get_logger().info(f"validate action client - check state before sending = {status} ") + self.node.get_logger().info(f'validate action client - check state before sending = {status} ') client.send_goal(topic1, BehaviorExecution.Goal(), wait_duration=1.0) status = client.get_state(topic1) - self.node.get_logger().info(f"validate action client - check state after goal sent = {status} ") + self.node.get_logger().info(f'validate action client - check state after goal sent = {status} ') end_time = time.time() + 10 while time.time() < end_time and not client.has_result(topic1): status = client.get_state(topic1) - # self.node.get_logger().info(f" get state = {status} ") + # self.node.get_logger().info(f' get state = {status} ') rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) self.assertTrue(client.is_active(topic1) or client.has_result(topic1)) self.assertTrue(client.has_result(topic1)) status = client.get_state(topic1) - self.node.get_logger().info(f" check state = {status} ") + self.node.get_logger().info(f' check state = {status} ') - self.node.get_logger().info("validate action client result 1 ... ") + self.node.get_logger().info('validate action client result 1 ... ') result = client.get_result(topic1) self.assertEqual(result.outcome, 'ok') status = client.get_state(topic1) self.assertEqual(status, GoalStatus.STATUS_SUCCEEDED) - self.node.get_logger().info("validate action client succeeded - OK! ") + self.node.get_logger().info('validate action client succeeded - OK! ') status = client.get_state(topic1) - self.node.get_logger().info(f" check state before send 2 = {status} ") + self.node.get_logger().info(f' check state before send 2 = {status} ') client.send_goal(topic1, BehaviorExecution.Goal(), wait_duration=1.0) status = client.get_state(topic1) - self.node.get_logger().info(f" check state after sending goal 2 = {status} ") + self.node.get_logger().info(f' check state after sending goal 2 = {status} ') # end_time = time.time() + 2 while not client.has_result(topic1): rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) self.assertTrue(client.is_active(topic1) or client.has_result(topic1)) status = client.get_state(topic1) - # self.node.get_logger().info(f" check state = {status} ") + # self.node.get_logger().info(f' check state = {status} ') self.assertFalse(client.is_active(topic1)) - self.node.get_logger().info("validate action client result 2 ... ") + self.node.get_logger().info('validate action client result 2 ... ') result = client.get_result(topic1) self.assertEqual(result.outcome, 'ok') status = client.get_state(topic1) self.assertEqual(status, GoalStatus.STATUS_SUCCEEDED) - self.node.get_logger().info("validate action client succeeded 2 - OK! ") + self.node.get_logger().info('validate action client succeeded 2 - OK! ') self.assertTrue(client.has_result(topic1)) client.remove_result(topic1) self.assertIsNone(client._result.get(topic1)) self.assertIsNone(client._result_status.get(topic1)) self.assertFalse(client.has_result(topic1)) - self.node.get_logger().info("validated remove_result! ") + self.node.get_logger().info('validated remove_result! ') self.assertFalse(client.is_available('/not_there')) client = ProxyActionClient({'/invalid': BehaviorExecution}, wait_duration=.1) self.assertFalse(client.is_available('/invalid')) - self.node.get_logger().info("test_action_client - OK! ") + self.node.get_logger().info('test_action_client - OK! ') del server # Through with instance, and explicitly calling del() to avoid ununsed warning diff --git a/flexbe_mirror/flexbe_mirror/__init__.py b/flexbe_mirror/flexbe_mirror/__init__.py index e69de29..d49e8df 100644 --- a/flexbe_mirror/flexbe_mirror/__init__.py +++ b/flexbe_mirror/flexbe_mirror/__init__.py @@ -0,0 +1,31 @@ +#!/usr/bin/env python + +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the Philipp Schillinger, Team ViGIR, Christopher Newport University nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""Initialize flexbe_mirror package.""" diff --git a/flexbe_mirror/flexbe_mirror/behavior_mirror_sm.py b/flexbe_mirror/flexbe_mirror/behavior_mirror_sm.py index 5c4cee1..b8f6140 100644 --- a/flexbe_mirror/flexbe_mirror/behavior_mirror_sm.py +++ b/flexbe_mirror/flexbe_mirror/behavior_mirror_sm.py @@ -1,4 +1,4 @@ -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -29,11 +29,13 @@ """Main function to start up the FlexBE mirror of onboard statemachine.""" from datetime import datetime -import rclpy from flexbe_core.proxy import shutdown_proxies + from flexbe_mirror.flexbe_mirror import FlexbeMirror +import rclpy + def main(args=None): """Run main function to start up the FlexBE mirror of onboard statemachine.""" @@ -45,54 +47,54 @@ def main(args=None): executor = rclpy.executors.SingleThreadedExecutor() executor.add_node(mirror) - mirror.get_logger().info("Begin behavior mirror processing ...") + mirror.get_logger().info('Begin behavior mirror processing ...') # Wait for ctrl-c to stop the application try: executor.spin() except KeyboardInterrupt: - print(f"Keyboard interrupt at {datetime.now()} ! Shut the behavior mirror down!", flush=True) + print(f'Keyboard interrupt at {datetime.now()} ! Shut the behavior mirror down!', flush=True) except Exception as exc: - print(f"Exception in mirror executor! {type(exc)}\n {exc}", flush=True) + print(f"Exception in mirror executor! '{type(exc)}'\n {exc}", flush=True) import traceback print(f"{traceback.format_exc().replace('%', '%%')}", flush=True) try: - print(f"Request behavior mirror shutdown at {datetime.now()} ...", flush=True) + print(f'Request behavior mirror shutdown at {datetime.now()} ...', flush=True) if mirror.shutdown_mirror(): - print(f"Mirror shutdown at {datetime.now()} ...", flush=True) + print(f'Mirror shutdown at {datetime.now()} ...', flush=True) else: # Last call for clean up of any stray communications and try again for _ in range(100): executor.spin_once(timeout_sec=0.001) # allow behavior to cleanup after itself if mirror.shutdown_mirror(): - print(f"Mirror shutdown at {datetime.now()} ...", flush=True) + print(f'Mirror shutdown at {datetime.now()} ...', flush=True) except Exception as exc: - print(f"Exception in behavior mirror node shutdown! {type(exc)}\n {exc}", flush=True) + print(f"Exception in behavior mirror node shutdown! '{type(exc)}'\n {exc}", flush=True) import traceback print(f"{traceback.format_exc().replace('%', '%%')}", flush=True) try: - print(f"Shutdown proxies requested at {datetime.now()} ...", flush=True) + print(f'Shutdown proxies requested at {datetime.now()} ...', flush=True) shutdown_proxies() # Last call for clean up of any stray communications for _ in range(100): executor.spin_once(timeout_sec=0.001) # allow behavior to cleanup after itself - print(f"Mirror proxy shutdown completed at {datetime.now()} ...", flush=True) + print(f'Mirror proxy shutdown completed at {datetime.now()} ...', flush=True) mirror.destroy_node() except Exception as exc: - print(f"Exception in behavior mirror node shutdown! {type(exc)}\n {exc}", flush=True) + print(f"Exception in behavior mirror node shutdown! '{type(exc)}'\n {exc}", flush=True) import traceback print(f"{traceback.format_exc().replace('%', '%%')}", flush=True) - print(f"Done with behavior mirror at {datetime.now()}!", flush=True) + print(f'Done with behavior mirror at {datetime.now()}!', flush=True) try: rclpy.try_shutdown() except Exception as exc: # pylint: disable=W0703 - print(f"Exception from rclpy.try_shutdown for behavior mirror: {type(exc)}\n{exc}", flush=True) + print(f"Exception from rclpy.try_shutdown for behavior mirror: '{type(exc)}'\n{exc}", flush=True) print(f"{traceback.format_exc().replace('%', '%%')}", flush=True) diff --git a/flexbe_mirror/flexbe_mirror/flexbe_mirror.py b/flexbe_mirror/flexbe_mirror/flexbe_mirror.py index f700591..1998f90 100644 --- a/flexbe_mirror/flexbe_mirror/flexbe_mirror.py +++ b/flexbe_mirror/flexbe_mirror/flexbe_mirror.py @@ -1,4 +1,4 @@ -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -33,36 +33,37 @@ from prctl import set_name as set_thread_name except Exception: def set_thread_name(name): - print("Python thread names are not visible in ps/top unless you install prctl") + """Set thread name dummy function if prctl not defined.""" + print('Python thread names are not visible in ps/top unless you install prctl') import threading import time import traceback -from rclpy.clock import Clock -from rclpy.node import Node -from rclpy.qos import QoSProfile, QoSDurabilityPolicy - -from std_msgs.msg import Empty, String, Int32, UInt32 - from flexbe_core import Logger, MIN_UI_VERSION from flexbe_core.core import LockableStateMachine, OperatableStateMachine from flexbe_core.core import PreemptableState, PreemptableStateMachine, StateMap from flexbe_core.core.topics import Topics from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached -from flexbe_msgs.msg import ContainerStructure, BehaviorSync, BEStatus +from flexbe_msgs.msg import BEStatus, BehaviorSync, ContainerStructure + +from rclpy.clock import Clock +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy, QoSProfile + +from std_msgs.msg import Empty, Int32, String, UInt32 +from .mirror_concurrency_container import MirrorConcurrencyContainer from .mirror_state import MirrorState from .mirror_state_machine import MirrorStateMachine -from .mirror_concurrency_container import MirrorConcurrencyContainer class FlexbeMirror(Node): """Class to handle the FlexBE mirror of onboard statemachine.""" def __init__(self): - # Initiate the Node class's constructor and give it a name + """Initiate the Node class's constructor and give it a name.""" super().__init__('flexbe_mirror') self._sm = None @@ -134,6 +135,7 @@ def _version_callback(self, msg): @staticmethod def _parse_version(v): + """Extract version information.""" result = 0 offset = 1 for n in reversed(v.split('.')): @@ -145,7 +147,7 @@ def get_elapsed_str(self, start_time): """Return a truncated time string for debugging.""" elapsed = self._system_clock.now() - start_time sec, nsec = start_time.seconds_nanoseconds() - return f"started at {sec & 0xFFFF}.{nsec//1000:06d} s (elapsed={elapsed.nanoseconds/1e9} s)" + return f'started at {sec & 0xFFFF}.{nsec//1000:06d} s (elapsed={elapsed.nanoseconds/1e9} s)' def heartbeat_timer_callback(self): """ @@ -195,21 +197,21 @@ def shutdown_mirror(self): return False # No active behavior except Exception as exc: # pylint: disable=W0703 - print(f"Exception shutting down behavior mirror {type(exc)}\n {exc}", flush=True) + print(f"Exception shutting down behavior mirror '{type(exc)}'\n {exc}", flush=True) import traceback - print(traceback.format_exc().replace("%", "%%"), flush=True) + print(traceback.format_exc().replace('%', '%%'), flush=True) def _mirror_structure_callback(self, msg): start_time = self._system_clock.now() Logger.loginfo(f'--> Mirror - received updated structure with checksum id = {msg.behavior_id}') Logger.localinfo(f' at {start_time.nanoseconds} ns') thread = threading.Thread(target=self._activate_mirror, args=[msg, start_time], - name=f"activate_mirror_{msg.behavior_id}_{start_time.nanoseconds}") + name=f'activate_mirror_{msg.behavior_id}_{start_time.nanoseconds}') thread.daemon = True thread.start() def _activate_mirror(self, struct_msg, start_time): - set_thread_name("act" + f"{start_time.nanoseconds}"[-12:]) # only 15 chars allowed + set_thread_name('act' + f'{start_time.nanoseconds}'[-12:]) # only 15 chars allowed with self._sync_lock: Logger.loginfo(f'Activate mirror for behavior id = {struct_msg.behavior_id} ...') @@ -233,7 +235,7 @@ def _activate_mirror(self, struct_msg, start_time): self._mirror_state_machine(struct_msg) if self._sm: Logger.localinfo(f'Mirror built for behavior id = {self._active_id}.') - self._sm.set_name("root") + self._sm.set_name('root') else: Logger.localwarn(f'Error processing mirror structure for behavior checksum id = {self._active_id} ...') Logger.logwarn('Requesting a new mirror structure from onboard ...') @@ -245,7 +247,7 @@ def _activate_mirror(self, struct_msg, start_time): # Release sync lock and execute the mirror Logger.localinfo(f"Mirror - begin execution of active mirror behavior id = '{self._sm.id}' " - f"{self.get_elapsed_str(start_time)}") + f'{self.get_elapsed_str(start_time)}') try: self._execute_mirror(start_time) @@ -254,8 +256,8 @@ def _activate_mirror(self, struct_msg, start_time): Logger.localerr(f"{traceback.format_exc().replace('%', '%%')}") self._running = False # normally set false in execute_mirror (but not if exeception) - Logger.localwarn(f"Done executing mirror {self._active_id} from activation " - f"{self.get_elapsed_str(start_time)}") + Logger.localwarn(f'Done executing mirror {self._active_id} from activation ' + f'{self.get_elapsed_str(start_time)}') def _status_callback(self, msg): start_time = self._system_clock.now() @@ -263,51 +265,51 @@ def _status_callback(self, msg): Logger.localinfo(f'Mirror - received BEstate={msg.code} - start mirror') # f'start mirror with behavior id = {msg.behavior_id} started at {start_time.nanoseconds} ns') thread = threading.Thread(target=self._start_mirror, args=[msg, start_time], - name=f"start_mirror_{msg.behavior_id}_{start_time.nanoseconds}") + name=f'start_mirror_{msg.behavior_id}_{start_time.nanoseconds}') thread.daemon = True thread.start() elif self._sm and msg.code in (BEStatus.ERROR, BEStatus.FAILED): Logger.localinfo(f'Mirror - received BEstate={msg.code} - issue ocurred onboard - stop current mirror!') thread = threading.Thread(target=self._stop_mirror, args=[msg, start_time], - name=f"stop_mirror_{msg.behavior_id}_{start_time.nanoseconds}") + name=f'stop_mirror_{msg.behavior_id}_{start_time.nanoseconds}') thread.daemon = True thread.start() Logger.logerr(f'Onboard error - stopped behavior mirror (code={msg.code})') elif self._sm and not self._stopping and msg.code == BEStatus.FINISHED: Logger.localinfo(f'Mirror - received BEstate={msg.code} - stop current mirror') thread = threading.Thread(target=self._stop_mirror, args=[msg, start_time], - name=f"stop_mirror_{msg.behavior_id}_{start_time.nanoseconds}") + name=f'stop_mirror_{msg.behavior_id}_{start_time.nanoseconds}') thread.daemon = True thread.start() def _start_mirror(self, msg, start_time): - set_thread_name("str" + f"{start_time.nanoseconds}"[-12:]) # only 15 chars allowed + set_thread_name('str' + f'{start_time.nanoseconds}'[-12:]) # only 15 chars allowed with self._sync_lock: self._wait_stopping(start_time) if self._running: if self._active_id != msg.behavior_id: - Logger.loginfo(f"Tried to start mirror for id={msg.behavior_id} while" - f" mirror for id={self._active_id} is already running - will ignore start request!") - Logger.localwarn(f" (active thread started {self._active_thread_start}) " - f" {self.get_elapsed_str(start_time)}") + Logger.loginfo(f'Tried to start mirror for id={msg.behavior_id} while' + f' mirror for id={self._active_id} is already running - will ignore start request!') + Logger.localwarn(f' (active thread started {self._active_thread_start}) ' + f' {self.get_elapsed_str(start_time)}') else: - Logger.localinfo(f" Received start request for already active {self._active_id} " - f"(active thread started {self._active_thread_start}) " - f" {self.get_elapsed_str(start_time)}") + Logger.localinfo(f' Received start request for already active {self._active_id} ' + f'(active thread started {self._active_thread_start}) ' + f' {self.get_elapsed_str(start_time)}') return - Logger.localinfo(f" Start request mirror for {msg.behavior_id} in thread {self.get_elapsed_str(start_time)}") + Logger.localinfo(f' Start request mirror for {msg.behavior_id} in thread {self.get_elapsed_str(start_time)}') if len(msg.args) > 0: - self._starting_path = "/" + msg.args[0][1:].replace("/", "_mirror/") + "_mirror" + self._starting_path = '/' + msg.args[0][1:].replace('/', '_mirror/') + '_mirror' self._active_id = msg.behavior_id - assert self._sm is None, ("No SM should be active with start command here " - f"(id = {self._active_id}, {self._sm.id}) {self.get_elapsed_str(start_time)}") + assert self._sm is None, ('No SM should be active with start command here ' + f'(id = {self._active_id}, {self._sm.id}) {self.get_elapsed_str(start_time)}') if len(self._struct_buffer) > 0: - # Logger.localinfo(f"Building mirror structure for checksum id={msg.behavior_id} " - # f"current len(struct_buffer)={len(self._struct_buffer)} ...") + # Logger.localinfo(f'Building mirror structure for checksum id={msg.behavior_id} ' + # f'current len(struct_buffer)={len(self._struct_buffer)} ...') while self._sm is None and len(self._struct_buffer) > 0: struct = self._struct_buffer[0] self._struct_buffer = self._struct_buffer[1:] @@ -319,8 +321,8 @@ def _start_mirror(self, msg, start_time): if self._sm is None: Logger.localwarn(f"Missing correct mirror structure for starting behavior checksum id ='{msg.behavior_id}'") - Logger.localwarn(f" Canceling start request for behavior checksum id ={msg.behavior_id} " - f"{self.get_elapsed_str(start_time)}") + Logger.localwarn(f' Canceling start request for behavior checksum id ={msg.behavior_id} ' + f'{self.get_elapsed_str(start_time)}') Logger.logwarn('Requesting mirror structure from onboard ...') self._request_struct_pub.publish(Int32(data=msg.behavior_id)) self._active_id = msg.behavior_id @@ -338,7 +340,7 @@ def _start_mirror(self, msg, start_time): try: Logger.localinfo(f"Begin mirror execution for checksum '{self._active_id}' " - f"{self.get_elapsed_str(start_time)}") + f'{self.get_elapsed_str(start_time)}') self._execute_mirror(start_time) except Exception as exc: # pylint: disable=W0703 Logger.logerr(f'Exception in start_mirror: {type(exc)} ...\n {exc}') @@ -346,18 +348,18 @@ def _start_mirror(self, msg, start_time): self._running = False # normally set false in execute_mirror (but not if exception) Logger.localwarn(f"Mirror execution for '{self._active_id}' is finished " - f"{self.get_elapsed_str(start_time)}") + f'{self.get_elapsed_str(start_time)}') def _stop_mirror(self, msg, start_time): # self.get_logger().info('--> Mirror - request to stop mirror for ' # f'checksum id={msg.behavior_id if isinstance(msg, BEStatus) else None} ' # f'- waiting for sync lock ...') - set_thread_name("stp" + f"{start_time.nanoseconds}"[-12:]) # only 15 chars allowed + set_thread_name('stp' + f'{start_time.nanoseconds}'[-12:]) # only 15 chars allowed with self._sync_lock: Logger.localinfo(f"Mirror '{self._active_id}' - stopping mirror " f"for checksum id={msg.behavior_id if msg is not None else 'None'} " - f" {self.get_elapsed_str(start_time)}") + f' {self.get_elapsed_str(start_time)}') self._stopping = True if self._sm is not None and self._running: if msg is None: @@ -390,23 +392,23 @@ def _stop_mirror(self, msg, start_time): if msg is not None and msg.code != BEStatus.SWITCHING: Logger.loginfo('\033[92m--- Behavior Mirror ready! ---\033[0m') - # Logger.localinfo("Mirror - stopped mirror for " - # f"behavior id={msg.behavior_id if msg is not None else BehaviorSync.INVALID} " - # f"- ready to release sync lock in stopping thread {self.get_elapsed_str(start_time)} s ...") + # Logger.localinfo('Mirror - stopped mirror for ' + # f'behavior id={msg.behavior_id if msg is not None else BehaviorSync.INVALID} ' + # f'- ready to release sync lock in stopping thread {self.get_elapsed_str(start_time)} s ...') self._stopping = False def _sync_callback(self, msg): start_time = self._system_clock.now() if msg.behavior_id == self._active_id: - Logger.logwarn(f"--> Mirror - sync request for behavior id={msg.behavior_id} - restart mirror") + Logger.logwarn(f'--> Mirror - sync request for behavior id={msg.behavior_id} - restart mirror') thread = threading.Thread(target=self._restart_mirror, args=[msg, start_time]) thread.daemon = True thread.start() else: Logger.error('--> Mirror - Cannot synchronize!\n Different behavior is running onboard, ' 'please stop execution while we reset the mirror!') - Logger.localwarn(f"Cannot synchronize! onboard checksum id={msg.behavior_id} active={self._active_id} " - f"{self.get_elapsed_str(start_time)}") + Logger.localwarn(f'Cannot synchronize! onboard checksum id={msg.behavior_id} active={self._active_id} ' + f'{self.get_elapsed_str(start_time)}') thread = threading.Thread(target=self._stop_mirror, args=[None, start_time]) thread.daemon = True thread.start() @@ -421,15 +423,15 @@ def _onboard_heartbeat_callback(self, msg): if self._sm is not None: mirror_status = self._sm.get_latest_status() if mirror_status.behavior_id != self._active_id: - Logger.localwarn(f"mirror_status.behavior_id ({mirror_status.behavior_id}) != " - f" self._active_id ({self._active_id})") + Logger.localwarn(f'mirror_status.behavior_id ({mirror_status.behavior_id}) != ' + f' self._active_id ({self._active_id})') mirror_status.behavior_id = self._active_id if msg.current_state_checksums != mirror_status.current_state_checksums: if self._sync_heartbeat_mismatch_counter > 0: # Two consecutive out of sync heartbeats - onboard_state_path = "Unknown" + onboard_state_path = 'Unknown' if len(msg.current_state_checksums) > 0: # Use deepest state as the best path estimate ob_state_id, ob_out = StateMap.unhash(msg.current_state_checksums[-1]) @@ -450,9 +452,9 @@ def _onboard_heartbeat_callback(self, msg): ob_state_id, ob_out = StateMap.unhash(state_hash) ob_state = self._state_map[ob_state_id] Logger.localinfo(f" onboard {ob_state_id} : '{ob_state.name}' " - f"out={ob_out} - {ob_state.path}") + f'out={ob_out} - {ob_state.path}') except Exception as exc: # pylint: disable=W0703 - Logger.localinfo(f" error for onboard state hash {state_hash} - {type(exc)} - {exc}") + Logger.localinfo(f' error for onboard state hash {state_hash} - {type(exc)} - {exc}') for state_hash in mirror_status.current_state_checksums: try: mr_state_id, mr_out = StateMap.unhash(state_hash) @@ -460,8 +462,8 @@ def _onboard_heartbeat_callback(self, msg): Logger.localinfo(f" mirror {mr_state_id} : '{mr_state.name.replace('_mirror', '')}' " f" out={mr_out} - {mr_state.path.replace('_mirror', '')}") except Exception as exc: # pylint: disable=W0703 - Logger.localinfo(f" error for mirror state hash {state_hash} - {type(exc)} - {exc}") - Logger.localinfo(30 * "=") + Logger.localinfo(f' error for mirror state hash {state_hash} - {type(exc)} - {exc}') + Logger.localinfo(30 * '=') else: # Start counting mismatches @@ -483,12 +485,12 @@ def _onboard_heartbeat_callback(self, msg): self._sync_heartbeat_mismatch_counter += 1 if self._sync_heartbeat_mismatch_counter % 10 == 1: Logger.warning(f"Mismatched behavior ids ('{msg.behavior_id}', '{self._active_id}')- " - f"please restart behavior! {self._sync_heartbeat_mismatch_counter}") + f'please restart behavior! {self._sync_heartbeat_mismatch_counter}') else: - Logger.localinfo(f"Heartbeat: mirror is stopping - waiting for {self._active_id} to stop ...") + Logger.localinfo(f'Heartbeat: mirror is stopping - waiting for {self._active_id} to stop ...') except Exception as exc: - Logger.localinfo(f"Exception in heartbeat callback {type(exc)} - {exc}") + Logger.localinfo(f'Exception in heartbeat callback {type(exc)} - {exc}') Logger.localinfo(f"{traceback.format_exc().replace('%', '%%')}") def _wait_stop_running(self, start_time): @@ -499,25 +501,25 @@ def _wait_stop_running(self, start_time): while self._running: if running_cnt % 2000 == 0: try: - Logger.localinfo(f"Waiting for another mirror (start thread {self._active_thread_start}) to stop " + Logger.localinfo(f'Waiting for another mirror (start thread {self._active_thread_start}) to stop ' f"with id = '{self._active_id}' " - f"(this {self.get_elapsed_str(start_time)}) (running cnt={running_cnt}) " - f"preempt={PreemptableState.preempt}") + f'(this {self.get_elapsed_str(start_time)}) (running cnt={running_cnt}) ' + f'preempt={PreemptableState.preempt}') except Exception: # pylint: disable=W703 # Likely during shutdown - print(f"Waiting for another mirror (start thread {self._active_thread_start}) to stop " + print(f'Waiting for another mirror (start thread {self._active_thread_start}) to stop ' f"with id = '{self._active_id}' " - f"(this {self.get_elapsed_str(start_time)}) (running cnt={running_cnt})", flush=True) + f'(this {self.get_elapsed_str(start_time)}) (running cnt={running_cnt})', flush=True) running_cnt += 1 if running_cnt > 100000: - Logger.logerr(f"Timeout waiting for another mirror ({self._active_thread_start}) to stop running " - f" with {self._active_id} (this {self.get_elapsed_str(start_time)})") + Logger.logerr(f'Timeout waiting for another mirror ({self._active_thread_start}) to stop running ' + f' with {self._active_id} (this {self.get_elapsed_str(start_time)})') return timing_event.wait(0.00002) # Use system time for polling check, never sim_time - Logger.localinfo(f"Mirror for active id {self._active_id} stopped running (start thread {self._active_thread_start}) " - f" ({running_cnt}) (this {self.get_elapsed_str(start_time)})") - Logger.loginfo("Mirror stopped running!") + Logger.localinfo(f'Mirror for active id {self._active_id} stopped running (start thread {self._active_thread_start}) ' + f' ({running_cnt}) (this {self.get_elapsed_str(start_time)})') + Logger.loginfo('Mirror stopped running!') def _wait_stopping(self, start_time): if self._stopping: @@ -526,23 +528,23 @@ def _wait_stopping(self, start_time): while self._stopping: if stopping_cnt % 5000 == 0: try: - Logger.localinfo(f"Waiting for another mirror (start thread {self._active_thread_start}) " + Logger.localinfo(f'Waiting for another mirror (start thread {self._active_thread_start}) ' f" to finish stopping with id='{self._active_id}' " - f"(this {self.get_elapsed_str(start_time)}) (stopping {stopping_cnt})... ") + f'(this {self.get_elapsed_str(start_time)}) (stopping {stopping_cnt})... ') except Exception: # pylint: disable=W0703 - print(f"Waiting for another mirror (start thread {self._active_thread_start}) " + print(f'Waiting for another mirror (start thread {self._active_thread_start}) ' f" to finish stopping with id='{self._active_id}' " - f"(this {self.get_elapsed_str(start_time)}) (stopping {stopping_cnt})... ", flush=True) + f'(this {self.get_elapsed_str(start_time)}) (stopping {stopping_cnt})... ', flush=True) stopping_cnt += 1 if stopping_cnt > 100000: Logger.logerr(f"Timeout waiting for another mirror to finish stopping with '{self._active_id}'" - f" (this {self.get_elapsed_str(start_time)})") + f' (this {self.get_elapsed_str(start_time)})') return timing_event.wait(0.00002) # use wall clock not sim time - Logger.localinfo(f"Mirror completed stopping for active id {self._active_id} " - f" (this {self.get_elapsed_str(start_time)}) ({stopping_cnt})!") - Logger.loginfo("Mirror stopped.") + Logger.localinfo(f'Mirror completed stopping for active id {self._active_id} ' + f' (this {self.get_elapsed_str(start_time)}) ({stopping_cnt})!') + Logger.loginfo('Mirror stopped.') def _reinitialize_state_machine(self, state_machine): """Reinitialize existing SM when restarting mirror during sync (faster than rebuilding current structure).""" @@ -554,13 +556,13 @@ def _reinitialize_state_machine(self, state_machine): self._reinitialize_state_machine(state) def _restart_mirror(self, msg, restart_time): - set_thread_name("rsm" + f"{restart_time.nanoseconds}"[-12:]) # only 15 chars allowed + set_thread_name('rsm' + f'{restart_time.nanoseconds}'[-12:]) # only 15 chars allowed with self._sync_lock: if self._sm is not None and self._running: self._wait_stop_running(restart_time) - Logger.localinfo(f"Restarting mirror for synchronization of behavior checksum id ={msg.behavior_id} " - f"in thread {self.get_elapsed_str(restart_time)} ...") + Logger.localinfo(f'Restarting mirror for synchronization of behavior checksum id ={msg.behavior_id} ' + f'in thread {self.get_elapsed_str(restart_time)} ...') # Clear existing outcome messages self._outcome_sub.remove_last_msg(Topics._OUTCOME_TOPIC, clear_buffer=True) @@ -575,17 +577,17 @@ def _restart_mirror(self, msg, restart_time): PreemptableState.preempt = False # Reset preempt flag before restarting self._reinitialize_state_machine(self._sm) end = time.time() - Logger.localinfo(f"Done reinitializing the existing state machine with matching " + Logger.localinfo(f'Done reinitializing the existing state machine with matching ' f"behavior id='{msg.behavior_id}' in thread " - f"{self.get_elapsed_str(restart_time)} (reinitialized in {end-start} seconds) ") + f'{self.get_elapsed_str(restart_time)} (reinitialized in {end-start} seconds) ') else: # Reconstruct the state machine from structure self._mirror_state_machine(self._current_struct) if self._sm is None: - Logger.localwarn(f"Missing correct mirror structure for restarting " - f"behavior checksum id ={msg.behavior_id}\n" - f" Canceling restart request for behavior checksum id ={msg.behavior_id} " - f"in thread {self.get_elapsed_str(restart_time)} ") + Logger.localwarn(f'Missing correct mirror structure for restarting ' + f'behavior checksum id ={msg.behavior_id}\n' + f' Canceling restart request for behavior checksum id ={msg.behavior_id} ' + f'in thread {self.get_elapsed_str(restart_time)} ') Logger.logwarn('Requesting mirror structure from onboard ...') self._pub.publish(Topics._REQUEST_STRUCTURE_TOPIC, Int32(data=msg.behavior_id)) self._active_id = msg.behavior_id @@ -617,52 +619,52 @@ def _restart_mirror(self, msg, restart_time): elif isinstance(parent, MirrorStateMachine): parent._current_state = state else: - Logger.logerr(f" Sync: Unexpected parent reference {parent.name} ({type(parent)}) " - f"from {state.name} in {self._sm.name}") + Logger.logerr(f" Sync: Unexpected parent reference '{parent.name}' ({type(parent)}) " + f"from '{state.name}' in '{self._sm.name}'") state = parent parent = parent.parent else: - Logger.logerr(f" Unknown state from {state_id} in {self._sm.name} from restart " - f"in thread {self.get_elapsed_str(restart_time)}!") + Logger.logerr(f" Unknown state from {state_id} in '{self._sm.name}' from restart " + f'in thread {self.get_elapsed_str(restart_time)}!') curst = self._sm._current_state self._sm._last_deep_states_list = self._sm.get_deep_states() Logger.localwarn(f" Restart SM with current top-level state = {curst.name if curst is not None else 'None'} " - f"starting path={self._starting_path}") - Logger.localinfo(f" active states = {self._sm.get_latest_status()}") + f'starting path={self._starting_path}') + Logger.localinfo(f' active states = {self._sm.get_latest_status()}') self._running = True # set running while we have sync lock except (AttributeError, RuntimeError) as exc: Logger.loginfo(f'Stopping synchronization because behavior{msg.behavior_id} has stopped.') - Logger.localinfo(f"{type(exc)} - {exc}") + Logger.localinfo(f"'{type(exc)}' - {exc}") return try: - Logger.localinfo("Execute mirror after sync lock of restart mirror" + Logger.localinfo('Execute mirror after sync lock of restart mirror' f" for synchronization of behavior '{msg.behavior_id}' from restart " - f"in thread {self.get_elapsed_str(restart_time)} ...") + f'in thread {self.get_elapsed_str(restart_time)} ...') self._execute_mirror(restart_time) except Exception as exc: # pylint: disable=W0703 Logger.logerr(f'Exception in restart_mirror in thread {self.get_elapsed_str(restart_time)}:\n {type(exc)} - {exc}') Logger.localerr(f"{traceback.format_exc().replace('%', '%%')}") self._running = False - Logger.localwarn(f"Finished execution of restart request for behavior checksum id ={msg.behavior_id} " - f"in thread {self.get_elapsed_str(restart_time)}.") + Logger.localwarn(f'Finished execution of restart request for behavior checksum id ={msg.behavior_id} ' + f'in thread {self.get_elapsed_str(restart_time)}.') def _execute_mirror(self, start_time): if self._active_thread_start is not None: - Logger.localwarn(f"Trying to start execution for {start_time.nanoseconds} but " - f"older {self._active_thread_start} is still active!") - Logger.logerr("Mirror issue - shutdown all threads - need to reattach!") + Logger.localwarn(f'Trying to start execution for {start_time.nanoseconds} but ' + f'older {self._active_thread_start} is still active!') + Logger.logerr('Mirror issue - shutdown all threads - need to reattach!') PreemptableState.preempt = True return self._running = True self._active_thread_start = start_time.nanoseconds # Track starting time - Logger.loginfo("Executing mirror ...") - Logger.localinfo(f" in thread {self.get_elapsed_str(start_time)} s ") + Logger.loginfo('Executing mirror ...') + Logger.localinfo(f' in thread {self.get_elapsed_str(start_time)} s ') if self._starting_path is not None: LockableStateMachine.path_for_switch = self._starting_path - Logger.loginfo("Starting mirror in state " + self._starting_path) + Logger.loginfo('Starting mirror in state ' + self._starting_path) self._starting_path = None result = PreemptableStateMachine._preempted_name @@ -673,12 +675,12 @@ def _execute_mirror(self, start_time): except Exception as exc: try: Logger.logerr('\n(_execute_mirror Traceback): Caught exception on preempt:\n%s' % str(exc)) - Logger.localerr(traceback.format_exc().replace("%", "%%")) + Logger.localerr(traceback.format_exc().replace('%', '%%')) except Exception: # pylint: disable=W0703 # Likely the loggers are dead if we ctrl-C'd during active behavior # so just try a simple print print('\n(_execute_mirror Traceback): Caught exception on preempt:\n%s' % str(exc), flush=True) - print(traceback.format_exc().replace("%", "%%"), flush=True) + print(traceback.format_exc().replace('%', '%%'), flush=True) result = PreemptableStateMachine._preempted_name self._active_thread_start = None @@ -695,7 +697,7 @@ def _mirror_state_machine(self, msg): root = con_msg.path break - # self.get_logger().info(f"Constructing top-level mirror for {root} ...") + # self.get_logger().info(f'Constructing top-level mirror for {root} ...') self._add_node(msg, root) if self._sm: @@ -707,17 +709,17 @@ def _mirror_state_machine(self, msg): hash_val = self._state_map.get_path_hash(con_msg.path) if hash_val: state = self._state_map[hash_val] - for path_seg in con_msg.path.split("/"): - assert path_seg in state.path, (f"Mismatched state id={state.state_id} for {state.path} vs. " - f"({hash_val}) {con_msg.path}") + for path_seg in con_msg.path.split('/'): + assert path_seg in state.path, (f'Mismatched state id={state.state_id} for {state.path} vs. ' + f'({hash_val}) {con_msg.path}') end = time.time() Logger.localinfo(f"Constructed mirror for behavior id ='{self._sm.id}' in {end - start} seconds !") return # success here else: - Logger.logerr(f"Failed to construct mirror SM for {root}!") + Logger.logerr(f'Failed to construct mirror SM for {root}!') except Exception as exc: - self.get_logger().warn(f"_mirror_statemachine Exception: {type(exc)} - {exc}") + self.get_logger().warn(f"_mirror_statemachine Exception: '{type(exc)}' - {exc}") self.get_logger().warn(f"{traceback.format_exc().replace('%', '%%')}") self._sm = None @@ -750,8 +752,8 @@ def _add_node(self, msg, path): sm = MirrorStateMachine(container_name, path, outcomes=sm_outcomes) self._state_map.add_state(path, sm) - assert sm.state_id == container.state_id, ("Failed to validate container state_id " - f"= {sm.state_id} vs. {container.state_id}") + assert sm.state_id == container.state_id, ('Failed to validate container state_id ' + f'= {sm.state_id} vs. {container.state_id}') with sm: for child in container.children: @@ -770,12 +772,12 @@ def _add_node(self, msg, path): else: # Basic state - # self.get_logger().info(f" Adding mirror state {container_name} at {path} ...") - assert container.type == 0, f"{container_name} - Non-containers should have type 0 not {container.type}!" + # self.get_logger().info(f" Adding mirror state '{container_name}' at {path} ...") + assert container.type == 0, f"'{container_name}' - Non-containers should have type 0 not {container.type}!" mrst = MirrorState(container_name, path, container.outcomes, container.autonomy) self._state_map.add_state(path, mrst) - assert mrst.state_id == container.state_id, ("Failed to validate container state_id " - f"= {mrst.state_id} vs. {container.state_id}") + assert mrst.state_id == container.state_id, ('Failed to validate container state_id ' + f'= {mrst.state_id} vs. {container.state_id}') MirrorStateMachine.add(container_name + '_mirror', mrst, transitions=transitions) def _preempt_callback(self, msg): diff --git a/flexbe_mirror/flexbe_mirror/mirror_concurrency_container.py b/flexbe_mirror/flexbe_mirror/mirror_concurrency_container.py index 95a6d6a..8d3b580 100644 --- a/flexbe_mirror/flexbe_mirror/mirror_concurrency_container.py +++ b/flexbe_mirror/flexbe_mirror/mirror_concurrency_container.py @@ -1,4 +1,4 @@ -# Copyright 2023 Christopher Newport University +# Copyright 2024 Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -40,10 +40,12 @@ class MirrorConcurrencyContainer(MirrorStateMachine): """Manage updates of ConcurrencyContainer in the FlexBE mirror in response to changes.""" def __init__(self, target_name, target_path, *args, **kwargs): + """Initialize MirrorConcurrrencyContainer instance.""" MirrorStateMachine.__init__(self, target_name, target_path, *args, **kwargs) self._returned_outcomes = {} def on_enter_mirror(self, userdata): + """Enter mirror concurrency container state.""" self.assert_consistent_transitions() self._current_state = self._states[:] self._userdata = None # Mirror does not use user data @@ -83,8 +85,8 @@ def execute_mirror(self, userdata): # Handle outcome of this internal SM if self._last_outcome is not None: Logger.localwarn(f"Mirror SM concurrency execute for '{self.name}' ({self._state_id}) : " - f"Already processed outcome={self._last_outcome} for " - f"outcome index={MirrorState._last_state_outcome} - reprocessing anyway") + f"Already processed outcome='{self._last_outcome}' for " + f'outcome index={MirrorState._last_state_outcome} - reprocessing anyway') MirrorState._last_state_id = None # Flag that the message was handled if MirrorState._last_state_outcome is not None: @@ -92,7 +94,8 @@ def execute_mirror(self, userdata): MirrorState._last_state_outcome = None # Flag that the message was handled return self.on_exit_mirror(userdata, desired_outcome, states=[s for s in self._states if (s.name not in self._returned_outcomes - or self._returned_outcomes[s.name] is None)]) + or self._returned_outcomes[s.name] is None) + ]) return self._execute_current_state_mirror(userdata) @@ -131,8 +134,8 @@ def get_deep_states(self): deep_states.append(state) elif self._current_state is not None: Logger.localerr(f"MirrorConcurrentContainer.get_deep_states '{self.name}' ({self._current_state._state_id})\n" - f" - current state is NOT a list! Error type={type(self._current_state)}") + f" - current state is NOT a list! Error type='{type(self._current_state)}'") Logger.localerr(f" '{self._current_state.name}' ({self._current_state._state_id})") - raise TypeError(f"MirrorConcurrentContainer.get_deep_states {self.name} - " - f"current state is NOT a list! Errror type={type(self._current_state)}") + raise TypeError(f"MirrorConcurrentContainer.get_deep_states '{self.name}' - " + f"current state is NOT a list! Errror type='{type(self._current_state)}'") return deep_states diff --git a/flexbe_mirror/flexbe_mirror/mirror_state.py b/flexbe_mirror/flexbe_mirror/mirror_state.py index a8c2a7f..bd7dfa4 100644 --- a/flexbe_mirror/flexbe_mirror/mirror_state.py +++ b/flexbe_mirror/flexbe_mirror/mirror_state.py @@ -1,4 +1,4 @@ -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -29,13 +29,13 @@ """Simplified state for use with FlexBE UI State machine mirror.""" -from std_msgs.msg import String - from flexbe_core import EventState from flexbe_core import Logger from flexbe_core.core.preemptable_state import PreemptableState from flexbe_core.core.topics import Topics -from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached +from flexbe_core.proxy import ProxyPublisher + +from std_msgs.msg import String class MirrorState(EventState): @@ -54,11 +54,12 @@ class MirrorState(EventState): _pub = None def __init__(self, target_name, target_path, given_outcomes, outcome_autonomy): + """Initialize MirrorState instance.""" # pylint: disable=unused-argument super().__init__(outcomes=given_outcomes) self._outcomes.append(PreemptableState._preempted_name) # Add preempted to outcomes list (for -1 outcome) self._target_name = target_name - self._target_path = "/" + "/".join(target_path.split("/")[1:]) # Drop top-level name + self._target_path = '/' + '/'.join(target_path.split('/')[1:]) # Drop top-level name MirrorState._last_target_path = None # reset any time that we build a new state machine if MirrorState._pub is None: @@ -73,15 +74,16 @@ def publish_update(cls, target_path): MirrorState._pub.publish(Topics._BEHAVIOR_UPDATE_TOPIC, String(data=target_path)) def execute_mirror(self, userdata): + """Execute the mirror state.""" if self._entering: self.on_enter_mirror(userdata) if MirrorState._last_state_id == self.state_id: # Only process if relevant to this state if self._last_outcome is not None: - Logger.localwarn(f"Already processed outcome={self._last_outcome} for " + Logger.localwarn(f"Already processed outcome='{self._last_outcome}' for " f" state '{self.name}' ({self.state_id}) given new " - f"outcome index={MirrorState._last_state_outcome} - reprocessing anyway") + f"outcome index='{MirrorState._last_state_outcome}' - reprocessing anyway") MirrorState._last_state_id = None # Flag that the message was handled if MirrorState._last_state_outcome is not None: @@ -91,17 +93,19 @@ def execute_mirror(self, userdata): return None def on_enter_mirror(self, userdata): + """Enter the mirror state.""" self._entering = False self._last_outcome = None MirrorState.publish_update(self._target_path) def on_exit_mirror(self, userdata, desired_outcome): + """Exit mirror state.""" try: self._last_outcome = self.outcomes[desired_outcome] return self._last_outcome except Exception as exc: # pylint: disable=W0703 - Logger.localerr(f"Error: MirrorState execute for {self.name}: " - f"outcome index {desired_outcome} is not relevant ({len(self.outcomes)}) ") + Logger.localerr(f"Error: MirrorState execute for '{self.name}': " + f"outcome index '{desired_outcome}' is not relevant ({len(self.outcomes)}) ") import traceback - Logger.localerr(f"{type(exc)} - {exc}\n{traceback.format_exc().replace('%', '%%')}") + Logger.localerr(f"Error: '{type(exc)}' - {exc}\n{traceback.format_exc().replace('%', '%%')}") return None diff --git a/flexbe_mirror/flexbe_mirror/mirror_state_machine.py b/flexbe_mirror/flexbe_mirror/mirror_state_machine.py index b92a2db..b27ae58 100644 --- a/flexbe_mirror/flexbe_mirror/mirror_state_machine.py +++ b/flexbe_mirror/flexbe_mirror/mirror_state_machine.py @@ -1,4 +1,4 @@ -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -31,10 +31,6 @@ from threading import Event -import rclpy - -from std_msgs.msg import UInt32 - from flexbe_core import Logger from flexbe_core.core import PreemptableState, PreemptableStateMachine from flexbe_core.core import StateMachine @@ -43,9 +39,12 @@ from flexbe_core.core.topics import Topics from flexbe_core.proxy import ProxySubscriberCached -from flexbe_msgs.msg import BehaviorSync from flexbe_mirror.mirror_state import MirrorState +from flexbe_msgs.msg import BehaviorSync + +import rclpy + class MirrorStateMachine(PreemptableStateMachine): """Manage updates of the FlexBE mirror in response to changes.""" @@ -53,16 +52,17 @@ class MirrorStateMachine(PreemptableStateMachine): _execute_flag = True # on change, we should execute cycle def __init__(self, target_name, target_path, *args, **kwargs): + """Initialize MirrorStateMachine instance.""" super().__init__(*args, **kwargs) self.id = None self._entering = True self._target_name = target_name - self._target_path = "/" + "/".join(target_path.split("/")[1:]) # Drop top-level name + self._target_path = '/' + '/'.join(target_path.split('/')[1:]) # Drop top-level name def spin(self, start_time, userdata=None): """Spin the execute in loop for Mirror.""" Logger.localinfo(f"Mirror: begin spinning for '{self.name}' ({self.id}) " - f" in thread with start time = {start_time.nanoseconds} ns") + f' in thread with start time = {start_time.nanoseconds} ns') timing_event = Event() @@ -99,19 +99,19 @@ def spin(self, start_time, userdata=None): if MirrorState._last_state_id == self.state_id: # Handle this top-level outcome if self._last_outcome is not None: - Logger.localwarn(f"Mirror SM top-level spin for {self.name} : " - f"Already processed outcome={self._last_outcome} for " + Logger.localwarn(f"Mirror SM top-level spin for '{self.name}' : " + f"Already processed outcome='{self._last_outcome}' for " f" state '{self.name}' ({self.state_id}) given new " - f"outcome index={MirrorState._last_state_outcome} - " - f"reprocessing anyway in thread started at {start_time.nanoseconds}") + f'outcome index={MirrorState._last_state_outcome} - ' + f'reprocessing anyway in thread started at {start_time.nanoseconds}') MirrorState._last_state_id = None # Flag that the message was handled if MirrorState._last_state_outcome is not None: outcome = self.on_exit_mirror(userdata, MirrorState._last_state_outcome) MirrorState.publish_update(self._target_path) # Notify back at top-level before exit MirrorState._last_state_outcome = None # Flag that the message was handled - Logger.localinfo(f" top-level outcome {outcome} for {state_id} " - f"in thread started at {start_time.nanoseconds}") + Logger.localinfo(f' top-level outcome {outcome} for {state_id} ' + f'in thread started at {start_time.nanoseconds}') break # Outcome at the top-level # Some change to process @@ -126,8 +126,8 @@ def spin(self, start_time, userdata=None): if MirrorState._last_state_id is not None or MirrorState._last_state_outcome is not None: # This should not happen unless system is out of sync Logger.logwarn(f"MirrorStateMachine '{self.name}' ({self._state_id}) spin() - " - f"no state handled outcome from {MirrorState._last_state_id} " - f"outcome index={MirrorState._last_state_outcome}") + f'no state handled outcome from {MirrorState._last_state_id} ' + f'outcome index={MirrorState._last_state_outcome}') # Store the information for safely passing to heartbeat thread deep_states = self.get_deep_states() @@ -143,33 +143,35 @@ def spin(self, start_time, userdata=None): if outcome is not None: Logger.localinfo(f"MirrorStateMachine '{self.name}' ({self._state_id}) spin() - outcome = {outcome}" - f" - wait for confirming top-level outcome message!") + ' - wait for confirming top-level outcome message!') else: # Process fast independent of simulation time in order to keep up with onboard if loop_count > 50000: loop_count = 0 # periodic spam for updates Logger.localinfo(f" SM spinner -'{self.name}' ({self.id}) - " - f"after {self._total_loop_count} spins in thread started at {start_time.nanoseconds}") + f'after {self._total_loop_count} spins in thread started at {start_time.nanoseconds}') timing_event.wait(0.0002) # minor wait for next message if we didn't process anything previous loop except Exception as exc: # pylint: disable=W0703 Logger.logerr(f" Exception in mirror spinner -'{self.state_id}' ({self.id})") - Logger.localerr(f"{type(exc)} - {exc}") + Logger.localerr(f'{type(exc)} - {exc}') import traceback Logger.localinfo(f"{traceback.format_exc().replace('%', '%%')}") break Logger.localinfo(f"Mirror: done spinning for '{self.name}' ({self.id}) with outcome = '{outcome}' " - f" after {self._total_loop_count} spins" - f" in thread started at {start_time.nanoseconds}") + f' after {self._total_loop_count} spins' + f' in thread started at {start_time.nanoseconds}') return outcome def destroy(self): + """Destroy state machine.""" Logger.localinfo(f'Destroy mirror state machine {self.name} ...') self._notify_stop() def _notify_stop(self): + """Notify states to stop.""" for state in self._states: if isinstance(state, MirrorState): state.on_stop() @@ -197,9 +199,9 @@ def _execute_current_state_mirror(self, userdata): except KeyError as exc: err_msg = f"Returned outcome '{outcome}' is not registered as a transition from '{self._current_state}'" Logger.localerr(f"Mirror SM execute for '{self.name}' ({self._state_id}): {err_msg}") - Logger.localinfo(f" {self.name} ({self._state_id}) - labels={self._labels}") - Logger.localinfo(f" {self.name} ({self._state_id}) - transitions={self._transitions}") - Logger.localinfo(f" {self.name} ({self._state_id}) - outcomes={self._outcomes}") + Logger.localinfo(f' {self.name} ({self._state_id}) - labels={self._labels}') + Logger.localinfo(f' {self.name} ({self._state_id}) - transitions={self._transitions}') + Logger.localinfo(f' {self.name} ({self._state_id}) - outcomes={self._outcomes}') raise StateError(err_msg) from exc # we handle internal SM transitions using mirror outcome messages @@ -214,8 +216,8 @@ def execute_mirror(self, userdata): # Handle outcome of this internal SM if self._last_outcome is not None: Logger.localwarn(f"Mirror SM execute for '{self.name}' ({self._state_id}) : " - f"Already processed outcome={self._last_outcome} for " - f"outcome index={MirrorState._last_state_outcome} - reprocessing anyway") + f'Already processed outcome={self._last_outcome} for ' + f'outcome index={MirrorState._last_state_outcome} - reprocessing anyway') MirrorState._last_state_id = None # Flag that the message was handled if MirrorState._last_state_outcome is not None: @@ -257,10 +259,11 @@ def get_latest_status(self): msg.current_state_checksums.append(StateMap.hash(active, outcome_index)) else: - Logger.localinfo(f" Mirror get_latest_status: No active states for {msg.behavior_id}!") + Logger.localinfo(f' Mirror get_latest_status: No active states for {msg.behavior_id}!') return msg def on_enter_mirror(self, userdata): + """Enter mirror statemachine.""" self._entering = False self._last_outcome = None self.assert_consistent_transitions() @@ -269,6 +272,7 @@ def on_enter_mirror(self, userdata): MirrorState.publish_update(self._target_path) def on_exit_mirror(self, userdata, desired_outcome=-1): + """Exit mirror statemachine.""" try: if self._current_state is not None: self._current_state._entering = True @@ -278,6 +282,6 @@ def on_exit_mirror(self, userdata, desired_outcome=-1): self._entering = True return self._last_outcome except Exception: # pylint: disable=W0703 - Logger.localerr(f"Error: MirrorStateMachine execute for {self.name}: " - f"outcome index {desired_outcome} is not relevant ({len(self.outcomes)}) ") + Logger.localerr(f"Error: MirrorStateMachine execute for '{self.name}': " + f'outcome index {desired_outcome} is not relevant ({len(self.outcomes)}) ') return None diff --git a/flexbe_mirror/setup.py b/flexbe_mirror/setup.py index 28b14fc..6958c79 100644 --- a/flexbe_mirror/setup.py +++ b/flexbe_mirror/setup.py @@ -1,7 +1,7 @@ """Setup script for flexbe_mirror package.""" -from setuptools import setup from setuptools import find_packages +from setuptools import setup PACKAGE_NAME = 'flexbe_mirror' diff --git a/flexbe_onboard/flexbe_onboard/__init__.py b/flexbe_onboard/flexbe_onboard/__init__.py index 9a1c325..9d48c94 100644 --- a/flexbe_onboard/flexbe_onboard/__init__.py +++ b/flexbe_onboard/flexbe_onboard/__init__.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: diff --git a/flexbe_onboard/flexbe_onboard/flexbe_onboard.py b/flexbe_onboard/flexbe_onboard/flexbe_onboard.py index 0464232..b884f28 100644 --- a/flexbe_onboard/flexbe_onboard/flexbe_onboard.py +++ b/flexbe_onboard/flexbe_onboard/flexbe_onboard.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -32,9 +32,7 @@ """Class defining state machine executive for onboard control.""" -from ast import literal_eval as cast import contextlib -from datetime import datetime import inspect import os import sys @@ -42,26 +40,29 @@ import threading import time import zlib +from ast import literal_eval as cast +from datetime import datetime try: from prctl import set_name as set_thread_name except Exception: def set_thread_name(name): - print("Python thread names are not visible in ps/top unless you install prctl") - -import rclpy -from rclpy.exceptions import ParameterNotDeclaredException -from rclpy.node import Node -from rclpy.qos import QoSProfile, QoSDurabilityPolicy + """Set thread name if prctl is not available.""" + print('Python thread names are not visible in ps/top unless you install prctl') from flexbe_core import BehaviorLibrary, Logger, MIN_UI_VERSION from flexbe_core.core.state_machine import StateMachine from flexbe_core.core.topics import Topics from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached -from flexbe_msgs.msg import BehaviorSelection, BehaviorSync, BEStatus, CommandFeedback, UserdataInfo +from flexbe_msgs.msg import BEStatus, BehaviorSelection, BehaviorSync, CommandFeedback, UserdataInfo from flexbe_msgs.srv import GetUserdata +import rclpy +from rclpy.exceptions import ParameterNotDeclaredException +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy, QoSProfile + from std_msgs.msg import String @@ -69,7 +70,7 @@ class FlexbeOnboard(Node): """Control the execution of robot behaviors.""" def __init__(self): - # Initiate the Node class's constructor and give it a name + """Initiate the Node class's constructor and give it a name.""" super().__init__('flexbe_onboard') ProxyPublisher.initialize(self) @@ -122,7 +123,7 @@ def __init__(self): time.sleep(0.5) # wait for publishers etc to really be set up # Will also re-publish BEStatus.READY every 10 seconds until first behavior received - Logger.localinfo("Set up the heartbeat timer ...") + Logger.localinfo('Set up the heartbeat timer ...') self._trigger_ready = False self._ready_counter = 0 self._heartbeat = self.create_timer(1.0, self._heartbeat_worker) @@ -131,6 +132,7 @@ def __init__(self): self._status_pub.publish(BEStatus(stamp=self.get_clock().now().to_msg(), code=BEStatus.READY)) def _version_callback(self, msg): + """Check required version of UI.""" vui = FlexbeOnboard._parse_version(msg.data) vex = FlexbeOnboard._parse_version(MIN_UI_VERSION) if vui < vex: @@ -141,6 +143,7 @@ def _version_callback(self, msg): @staticmethod def _parse_version(v): + """Parse the UI version string.""" result = 0 offset = 1 for n in reversed(v.split('.')): @@ -158,25 +161,26 @@ def _behavior_callback(self, beh_sel_msg): def behavior_shutdown(self): """Destroy any active behavior state machines to force proper shutdown.""" try: - print(f" Shutting down onboard behavior engine at {datetime.now()} ...", flush=True) + print(f' Shutting down onboard behavior engine at {datetime.now()} ...', flush=True) with self._switch_lock: if self._running: - assert self.be is not None, "Must have an active behavior here!" + assert self.be is not None, 'Must have an active behavior here!' self._switching = True self.be.preempt() - print(" Waiting for existing behavior to terminate ...", flush=True) + print(' Waiting for existing behavior to terminate ...', flush=True) return True # Active behavior needs to quit return False # No active behavior except Exception as exc: # pylint: disable=W0703 - print(f"Exception shutting down onboard behaviors {type(exc)}\n {exc}", flush=True) + print(f"Exception shutting down onboard behaviors '{type(exc)}'\n {exc}", flush=True) import traceback - print(traceback.format_exc().replace("%", "%%")) + print(traceback.format_exc().replace('%', '%%'), flush=True) def onboard_shutdown(self): - print("Shutting down the onboard behavior executive ...", flush=True) + """Shutdown the onboard software.""" + print('Shutting down the onboard behavior executive ...', flush=True) self.destroy_timer(self._heartbeat) self._proxy_pub.remove_publisher(Topics._CMD_FEEDBACK_TOPIC) for _ in range(50): @@ -186,9 +190,9 @@ def verify_no_active_behaviors(self, timeout=0.5): """Verify no active behaviors.""" run_locked = self._run_lock.acquire(timeout=timeout) if run_locked: - assert self.be is None, "Run lock with old behavior active?" + assert self.be is None, 'Run lock with old behavior active?' self._run_lock.release() - print(f" All onboard behaviors are stopped at {datetime.now()}!", flush=True) + print(f' All onboard behaviors are stopped at {datetime.now()}!', flush=True) return True else: return False @@ -199,21 +203,21 @@ def verify_no_active_behaviors(self, timeout=0.5): def _behavior_execution(self, beh_sel_msg): # sending a behavior while one is already running is considered as switching - set_thread_name("beh" + f"{beh_sel_msg.behavior_id}") + set_thread_name(f'beh{beh_sel_msg.behavior_id}') if not rclpy.ok(): self._cleanup_tempdir() if self._running: Logger.loginfo('--> Initiating behavior switch...') - self._proxy_pub.publish(Topics._CMD_FEEDBACK_TOPIC, CommandFeedback(command="switch", args=['received'])) + self._proxy_pub.publish(Topics._CMD_FEEDBACK_TOPIC, CommandFeedback(command='switch', args=['received'])) # construct the behavior that should be executed - Logger.localinfo(f"Prepare behavior id={beh_sel_msg.behavior_key} ({beh_sel_msg.behavior_id}) ...") + Logger.localinfo(f'Prepare behavior id={beh_sel_msg.behavior_key} ({beh_sel_msg.behavior_id}) ...') be = self._prepare_behavior(beh_sel_msg) if be is None: Logger.logerr('Dropped behavior start request because preparation failed.') if self._running: - self._proxy_pub.publish(Topics._CMD_FEEDBACK_TOPIC, CommandFeedback(command="switch", args=['failed'])) + self._proxy_pub.publish(Topics._CMD_FEEDBACK_TOPIC, CommandFeedback(command='switch', args=['failed'])) else: # self._status_pub.publish(BEStatus(stamp=self.get_clock().now().to_msg(), code=BEStatus.READY)) Logger.localinfo('\033[92m--- Behavior Engine ready to try again! ---\033[0m') @@ -224,22 +228,22 @@ def _behavior_execution(self, beh_sel_msg): # Logger.localinfo("Behavior Engine - get switch lock to start behavior id " # f"key={beh_sel_msg.behavior_key} ({beh_sel_msg.behavior_id})...") with self._switch_lock: - Logger.localinfo("Behavior Engine - got switch lock to start behavior new id " - f"key={beh_sel_msg.behavior_key} ({beh_sel_msg.behavior_id})...") + Logger.localinfo('Behavior Engine - got switch lock to start behavior new id ' + f'key={beh_sel_msg.behavior_key} ({beh_sel_msg.behavior_id})...') if self._running: - assert self.be is not None, "Must have an active behavior here!" + assert self.be is not None, 'Must have an active behavior here!' self._switching = True - Logger.localinfo("Behavior Engine - prepare to switch current running behavior" - f" {self.be.name}: id={self.be.beh_id}...") - self._proxy_pub.publish(Topics._CMD_FEEDBACK_TOPIC, CommandFeedback(command="switch", args=['start'])) + Logger.localinfo('Behavior Engine - prepare to switch current running behavior' + f" '{self.be.name}': id={self.be.beh_id}...") + self._proxy_pub.publish(Topics._CMD_FEEDBACK_TOPIC, CommandFeedback(command='switch', args=['start'])) # ensure that switching is possible if not self._is_switchable(be): - Logger.logerr("Dropped behavior start request for " - f"key={beh_sel_msg.behavior_key} (id={beh_sel_msg.behavior_id}) " - " because switching is not possible.") + Logger.logerr('Dropped behavior start request for ' + f'key={beh_sel_msg.behavior_key} (id={beh_sel_msg.behavior_id}) ' + ' because switching is not possible.') self._proxy_pub.publish(Topics._CMD_FEEDBACK_TOPIC, - CommandFeedback(command="switch", args=['not_switchable'])) + CommandFeedback(command='switch', args=['not_switchable'])) return self._status_pub.publish(BEStatus(stamp=self.get_clock().now().to_msg(), @@ -257,40 +261,40 @@ def _behavior_execution(self, beh_sel_msg): # extract the active state if any if active_states is not None: - Logger.localinfo(f"Behavior Engine - {self.be.name}: {self.be.beh_id} " - f"switching behaviors from active state {[acst.name for acst in active_states]} ...") + Logger.localinfo(f"Behavior Engine - '{self.be.name}': {self.be.beh_id} " + f'switching behaviors from active state {[acst.name for acst in active_states]} ...') try: if len(active_states) > 1: - Logger.localinfo(f"\n\nBehavior Engine - {self.be.name}: {self.be.beh_id} " - f"- switching only top level state!\n") - Logger.logwarn("Switch multiple active states?") + Logger.localinfo(f"\n\nBehavior Engine - '{self.be.name}': {self.be.beh_id} " + f'- switching only top level state!\n') + Logger.logwarn('Switch multiple active states?') be.prepare_for_switch(active_states[0]) self._proxy_pub.publish(Topics._CMD_FEEDBACK_TOPIC, - CommandFeedback(command="switch", args=['prepared'])) + CommandFeedback(command='switch', args=['prepared'])) except Exception as exc: Logger.logerr('Failed to prepare behavior switch:\n%s' % str(exc)) self._proxy_pub.publish(Topics._CMD_FEEDBACK_TOPIC, - CommandFeedback(command="switch", args=['failed'])) + CommandFeedback(command='switch', args=['failed'])) # Let us know that old behavior is still running self._status_pub.publish(BEStatus(stamp=self.get_clock().now().to_msg(), behavior_id=self.be.beh_id, code=BEStatus.RUNNING)) return # stop the rest - Logger.localinfo(f"Behavior Engine - {self.be.name}: {self.be.beh_id} - " - f"preempt active state {active_states[0].name} ...") + Logger.localinfo(f"Behavior Engine - '{self.be.name}': {self.be.beh_id} - " + f"preempt active state '{active_states[0].name}' ...") self.be.preempt() else: - Logger.localinfo(f"Behavior Engine - {self.be.name}: {self.be.beh_id} " - f"no active state to preempt (but Running={self._running}?)!") + Logger.localinfo(f"Behavior Engine - '{self.be.name}': {self.be.beh_id} " + f'no active state to preempt (but Running={self._running}?)!') # execute the behavior Logger.localinfo('Waiting on prior behavior to shutdown ...') with self._run_lock: - Logger.localinfo("Behavior Engine - got run lock to start behavior id " - f"key={beh_sel_msg.behavior_key}={be.beh_id} ({beh_sel_msg.behavior_id}) ...") - assert self.be is None, "Run lock with old behavior active?" + Logger.localinfo('Behavior Engine - got run lock to start behavior id ' + f'key={beh_sel_msg.behavior_key}={be.beh_id} ({beh_sel_msg.behavior_id}) ...') + assert self.be is None, 'Run lock with old behavior active?' self._running = True self.be = be @@ -322,8 +326,8 @@ def _behavior_execution(self, beh_sel_msg): code=BEStatus.FAILED)) Logger.logerr(f'Behavior execution for {self.be.name}: {self.be.beh_id} failed!\n%s' % str(exc)) import traceback - Logger.localinfo(f'''{traceback.format_exc().replace("%", "%%")}''') # Avoid single % in string - result = result or "exception" # only set result if not executed + Logger.localinfo(f"{traceback.format_exc().replace('%', '%%')}") # Avoid single % in string + result = result or 'exception' # only set result if not executed # done, remove left-overs like the temporary behavior file try: @@ -333,12 +337,12 @@ def _behavior_execution(self, beh_sel_msg): self._clear_imports() self._cleanup_behavior(beh_sel_msg.behavior_id) except Exception as exc: - self.get_logger().error(f"Failed to clean up behavior {self.be.name}: " - f"{self.be.beh_id}:\n {str(exc)}") + self.get_logger().error(f"Failed to clean up behavior '{self.be.name}': " + f'{self.be.beh_id}:\n {str(exc)}') if not self._switching: - Logger.localinfo(f"Behavior execution finished for {self.be.name}: {self.be.beh_id}" - f" with result {str(result)}") + Logger.localinfo(f"Behavior execution finished for '{self.be.name}': {self.be.beh_id}" + f" with result '{str(result)}'") self._status_pub.publish(BEStatus(stamp=self.get_clock().now().to_msg(), code=BEStatus.READY)) Logger.localinfo('\033[92m--- Behavior Engine finished - ready for more! ---\033[0m') @@ -366,9 +370,9 @@ def _userdata_callback(self, request, response): if len(userdata) > 0: # also print in terminal (better readability for complex message types) - self.get_logger().info(f"GetUserdata Service: Found {len(userdata)} " + self.get_logger().info(f'GetUserdata Service: Found {len(userdata)} ' f"occurrences of key='{request.userdata_key}' " - f"from be={self.be._state_machine._name}") + f"from be='{self.be._state_machine._name}'") for ud in userdata: self.get_logger().info(f"\tuser data key={ud.key}:\n{ud.data}\n{10*'-'}") self.get_logger().info(f"{10*'='} End get user data {10*'='}") @@ -396,11 +400,11 @@ def _prepare_behavior(self, beh_sel_msg): raise ValueError(beh_sel_msg.behavior_key) be_filepath = self._behavior_lib.get_sourcecode_filepath(beh_sel_msg.behavior_key, add_tmp=True) if os.path.isfile(be_filepath): - self.get_logger().warn("Found a tmp version of the referred behavior! Assuming local test run.") + self.get_logger().warn('Found a tmp version of the referred behavior! Assuming local test run.') else: be_filepath = self._behavior_lib.get_sourcecode_filepath(beh_sel_msg.behavior_key) - with open(be_filepath, "r") as be_file: + with open(be_filepath, 'r') as be_file: be_content = be_file.read() except Exception as exc: # pylint: disable=W0703 @@ -412,20 +416,20 @@ def _prepare_behavior(self, beh_sel_msg): # apply modifications if any try: - file_content = "" + file_content = '' last_index = 0 for mod in beh_sel_msg.modifications: file_content += be_content[last_index:mod.index_begin] + mod.new_content last_index = mod.index_end file_content += be_content[last_index:] if zlib.adler32(file_content.encode()) & 0x7fffffff != beh_sel_msg.behavior_id: - mismatch_msg = ("Checksum mismatch of behavior versions! \n" - "Attempted to load behavior: %s\n" - "Make sure that all computers are on the same version a.\n" - "Also try: ros2 run flexbe_widget clear_cache" % str(be_filepath)) + mismatch_msg = ('Checksum mismatch of behavior versions! \n' + 'Attempted to load behavior: %s\n' + 'Make sure that all computers are on the same version a.\n' + 'Also try: ros2 run flexbe_widget clear_cache' % str(be_filepath)) raise Exception(mismatch_msg) else: - self.get_logger().info("Successfully applied %d modifications." % len(beh_sel_msg.modifications)) + self.get_logger().info('Successfully applied %d modifications.' % len(beh_sel_msg.modifications)) except Exception as exc: Logger.logerr('Failed to apply behavior modifications:\n%s' % str(exc)) self._status_pub.publish(BEStatus(stamp=self.get_clock().now().to_msg(), @@ -436,7 +440,7 @@ def _prepare_behavior(self, beh_sel_msg): # create temp file for behavior class try: file_path = os.path.join(self._tmp_folder, f'tmp_{beh_sel_msg.behavior_id}.py') - with open(file_path, "w") as sc_file: + with open(file_path, 'w') as sc_file: sc_file.write(file_content) except Exception as exc: Logger.logerr('Failed to create temporary file for behavior class:\n%s' % str(exc)) @@ -448,8 +452,8 @@ def _prepare_behavior(self, beh_sel_msg): # import temp class file and initialize behavior try: with self._track_imports(): - package = __import__("tmp_%d" % beh_sel_msg.behavior_id, - fromlist=["tmp_%d" % beh_sel_msg.behavior_id]) + package = __import__('tmp_%d' % beh_sel_msg.behavior_id, + fromlist=['tmp_%d' % beh_sel_msg.behavior_id]) clsmembers = inspect.getmembers(package, lambda member: (inspect.isclass(member) and member.__module__ == package.__name__)) beclass = clsmembers[0][1] @@ -459,7 +463,7 @@ def _prepare_behavior(self, beh_sel_msg): Logger.logerr('Exception caught in behavior definition:\n%s\n' 'See onboard terminal for more information.' % str(exc)) import traceback - Logger.localinfo(f'''{traceback.format_exc().replace("%", "%%")}''') # Avoid single % in string + Logger.localinfo(f"{traceback.format_exc().replace('%', '%%')}") # Avoid single % in string self._status_pub.publish(BEStatus(stamp=self.get_clock().now().to_msg(), behavior_id=beh_sel_msg.behavior_id, code=BEStatus.ERROR)) @@ -486,8 +490,8 @@ def _prepare_behavior(self, beh_sel_msg): self.get_logger().warn(f"Parameter '{beh_sel_msg.arg_keys[i]}' " f"(set to '{beh_sel_msg.arg_values[i]}') not defined") except Exception as exc: - Logger.logerr(f"Failed to initialize parameters for " - f"behavior key={beh_sel_msg.behavior_key}:\n {type(exc)} - {exc}") + Logger.logerr(f'Failed to initialize parameters for ' + f"behavior key='{beh_sel_msg.behavior_key}':\n {type(exc)} - {exc}") self._status_pub.publish(BEStatus(stamp=self.get_clock().now().to_msg(), behavior_id=beh_sel_msg.behavior_id, code=BEStatus.ERROR)) @@ -495,8 +499,8 @@ def _prepare_behavior(self, beh_sel_msg): # build state machine try: - self.get_logger().info(f"Building state machine {beh_sel_msg.behavior_id} with " - f"autonomy level={beh_sel_msg.autonomy_level}.") + self.get_logger().info(f'Building state machine {beh_sel_msg.behavior_id} with ' + f'autonomy level={beh_sel_msg.autonomy_level}.') be.set_up(beh_id=beh_sel_msg.behavior_id, autonomy_level=beh_sel_msg.autonomy_level, debug=False) be.prepare_for_execution(self._convert_input_data(beh_sel_msg.input_keys, beh_sel_msg.input_values)) self.get_logger().info('State machine built.') @@ -504,7 +508,7 @@ def _prepare_behavior(self, beh_sel_msg): Logger.logerr('Behavior construction failed!\n%s\n' 'See onboard terminal for more information.' % str(exc)) import traceback - Logger.localinfo(f'''{traceback.format_exc().replace("%", "%%")}''') # Avoid single % in string + Logger.localinfo(f"{traceback.format_exc().replace('%', '%%')}") # Avoid single % in string self._status_pub.publish(BEStatus(stamp=self.get_clock().now().to_msg(), behavior_id=beh_sel_msg.behavior_id, code=BEStatus.ERROR)) @@ -512,7 +516,7 @@ def _prepare_behavior(self, beh_sel_msg): self._clear_imports() return None - Logger.localinfo(f"Finished behavior preparation for id={be.beh_id}!") + Logger.localinfo(f'Finished behavior preparation for id={be.beh_id}!') return be # ================ # @@ -564,8 +568,7 @@ def _convert_input_data(self, keys, values): # unquoted strings will raise a ValueError, so leave it as string in this case result[k] = str(v) except SyntaxError as se: - Logger.loginfo('Unable to parse input value for key "%s", assuming string:\n%s\n%s' % - (k, str(v), str(se))) + Logger.loginfo(f"Unable to parse input value for key '{k}', assuming string:\n{str(v)}\n{str(se)}") result[k] = str(v) return result @@ -578,8 +581,8 @@ def _heartbeat_worker(self): else: heartbeat = BehaviorSync() - # Logger.localinfo(f"Heartbeat: {heartbeat.behavior_id}: {heartbeat.current_state_checksum } " - # f"- running {self._running} switching {self._switching} ") + # Logger.localinfo(f'Heartbeat: {heartbeat.behavior_id}: {heartbeat.current_state_checksum } ' + # f'- running {self._running} switching {self._switching} ') self._heartbeat_pub.publish(heartbeat) if not self._running and not self._switching: diff --git a/flexbe_onboard/flexbe_onboard/start_behavior.py b/flexbe_onboard/flexbe_onboard/start_behavior.py index 29db871..550be72 100644 --- a/flexbe_onboard/flexbe_onboard/start_behavior.py +++ b/flexbe_onboard/flexbe_onboard/start_behavior.py @@ -1,4 +1,4 @@ -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -31,11 +31,13 @@ """Script for starting the onboard behavior engine.""" from datetime import datetime -import rclpy from flexbe_core.proxy import shutdown_proxies + from flexbe_onboard.flexbe_onboard import FlexbeOnboard +import rclpy + def main(args=None): """Script for starting the onboard behavior engine.""" @@ -47,21 +49,21 @@ def main(args=None): executor = rclpy.executors.SingleThreadedExecutor() executor.add_node(onboard) - onboard.get_logger().info("Begin processing onboard behavior engine ...") + onboard.get_logger().info('Begin processing onboard behavior engine ...') # Wait for ctrl-c to stop the application try: executor.spin() except KeyboardInterrupt: - print(f"Keyboard interrupt request at {datetime.now()} - ! Shut the onboard behavior executive down!", flush=True) + print(f'Keyboard interrupt request at {datetime.now()} - ! Shut the onboard behavior executive down!', flush=True) except Exception as exc: - print(f"Exception in executor at {datetime.now()} - ! {type(exc)}\n {exc}", flush=True) + print(f'Exception in executor at {datetime.now()} - ! {type(exc)}\n {exc}', flush=True) import traceback print(f"{traceback.format_exc().replace('%', '%%')}", flush=True) # Attempt to do a clean shutdown of any active behavior then the behavior engine itself try: - print(f"Request onboard behavior shutdown at {datetime.now()} ...", flush=True) + print(f'Request onboard behavior shutdown at {datetime.now()} ...', flush=True) if onboard.behavior_shutdown(): for i in range(5): for _ in range(100 * i): @@ -69,50 +71,50 @@ def main(args=None): if onboard.verify_no_active_behaviors(timeout=0.1): break else: - print(f" Active behavior still running onboard={onboard._running} at {datetime.now()}!", flush=True) + print(f' Active behavior still running onboard={onboard._running} at {datetime.now()}!', flush=True) else: - print(f" All onboard behaviors are stopped at {datetime.now()}!", flush=True) + print(f' All onboard behaviors are stopped at {datetime.now()}!', flush=True) # Last call for clean up of any stray communications for _ in range(50): executor.spin_once(timeout_sec=0.001) # allow behavior to cleanup after itself - print(f"{datetime.now()} - onboard shutdown requested ...", flush=True) + print(f'{datetime.now()} - onboard shutdown requested ...', flush=True) onboard.onboard_shutdown() for _ in range(30): executor.spin_once(timeout_sec=0.001) # allow onboard system to cleanup after itself except Exception as exc: - print(f"{datetime.now()} - Exception in onboard shutdown! {type(exc)}\n {exc}", flush=True) + print(f"{datetime.now()} - Exception in onboard shutdown! '{type(exc)}'\n {exc}", flush=True) import traceback print(f"{traceback.format_exc().replace('%', '%%')}", flush=True) try: - print(f"Shutdown proxies requested at {datetime.now()} ...", flush=True) + print(f'Shutdown proxies requested at {datetime.now()} ...', flush=True) shutdown_proxies() # Last call for clean up of any stray communications for _ in range(50): executor.spin_once(timeout_sec=0.001) # allow behavior to cleanup after itself - print(f"Proxy shutdown completed at {datetime.now()} ...", flush=True) + print(f'Proxy shutdown completed at {datetime.now()} ...', flush=True) onboard.destroy_node() # Last call for clean up of any stray communications for _ in range(50): executor.spin_once(timeout_sec=0.001) # allow behavior to cleanup after itself - print(f"Node destruction completed at {datetime.now()} ...", flush=True) + print(f'Node destruction completed at {datetime.now()} ...', flush=True) except Exception as exc: - print(f"{datetime.now()} - Exception in onboard proxy and node shutdown! {type(exc)}\n {exc}", flush=True) + print(f"{datetime.now()} - Exception in onboard proxy and node shutdown! '{type(exc)}'\n {exc}", flush=True) import traceback print(f"{traceback.format_exc().replace('%', '%%')}", flush=True) - print(f"Done with behavior executive at {datetime.now()}!", flush=True) + print(f'Done with behavior executive at {datetime.now()}!', flush=True) try: rclpy.try_shutdown() except Exception as exc: # pylint: disable=W0703 - print(f"{datetime.now()} - Exception from rclpy.shutdown for start onboard behavior: {type(exc)}\n{exc}", flush=True) + print(f"{datetime.now()} - Exception from rclpy.shutdown for start onboard behavior: '{type(exc)}'\n{exc}", flush=True) print(f"{traceback.format_exc().replace('%', '%%')}", flush=True) diff --git a/flexbe_onboard/launch/behavior_onboard.launch.py b/flexbe_onboard/launch/behavior_onboard.launch.py index 8995fa3..b792f06 100644 --- a/flexbe_onboard/launch/behavior_onboard.launch.py +++ b/flexbe_onboard/launch/behavior_onboard.launch.py @@ -1,4 +1,4 @@ -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -26,29 +26,31 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +"""Launch file for FlexBE Onboard Behavior Executive.""" from launch import LaunchDescription -from launch_ros.actions import Node from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + def generate_launch_description(): - + """Generate launch description.""" return LaunchDescription([ - DeclareLaunchArgument("log_enabled", default_value="False"), - DeclareLaunchArgument("log_folder", default_value="~/.flexbe_logs"), - DeclareLaunchArgument("log_serialize", default_value="yaml"), - DeclareLaunchArgument("log_level", default_value="INFO"), - DeclareLaunchArgument("use_sim_time", default_value="False"), - DeclareLaunchArgument("enable_clear_imports", default_value="False", - description="Delete behavior-specific module imports after execution."), + DeclareLaunchArgument('log_enabled', default_value='False'), + DeclareLaunchArgument('log_folder', default_value='~/.flexbe_logs'), + DeclareLaunchArgument('log_serialize', default_value='yaml'), + DeclareLaunchArgument('log_level', default_value='INFO'), + DeclareLaunchArgument('use_sim_time', default_value='False'), + DeclareLaunchArgument('enable_clear_imports', default_value='False', + description='Delete behavior-specific module imports after execution.'), Node( - name="behavior", package="flexbe_onboard", executable="start_behavior", output="screen", - parameters=[{"log_enabled": LaunchConfiguration("log_enabled"), - "log_folder": LaunchConfiguration("log_folder"), - "log_serialize": LaunchConfiguration("log_serialize"), - "log_level": LaunchConfiguration("log_level"), - "enable_clear_imports": LaunchConfiguration("enable_clear_imports"), - "use_sim_time": LaunchConfiguration("use_sim_time")}]) + name='behavior', package='flexbe_onboard', executable='start_behavior', output='screen', + parameters=[{'log_enabled': LaunchConfiguration('log_enabled'), + 'log_folder': LaunchConfiguration('log_folder'), + 'log_serialize': LaunchConfiguration('log_serialize'), + 'log_level': LaunchConfiguration('log_level'), + 'enable_clear_imports': LaunchConfiguration('enable_clear_imports'), + 'use_sim_time': LaunchConfiguration('use_sim_time')}]) ]) diff --git a/flexbe_onboard/setup.py b/flexbe_onboard/setup.py index 63464ae..6ff401b 100644 --- a/flexbe_onboard/setup.py +++ b/flexbe_onboard/setup.py @@ -2,7 +2,8 @@ import os from glob import glob -from setuptools import setup, find_packages + +from setuptools import find_packages, setup package_name = 'flexbe_onboard' @@ -17,9 +18,9 @@ ('share/' + package_name, ['package.xml']), (os.path.join('share', package_name), glob('tests/*.py')), (os.path.join('share', package_name), glob('tests/flexbe_onboard_test_data/*.py')), - (os.path.join('share', package_name, "tests", "flexbe_onboard_test_data"), + (os.path.join('share', package_name, 'tests', 'flexbe_onboard_test_data'), glob('tests/flexbe_onboard_test_data/*.py')), # No * here due to __pycache__ folder - (os.path.join('share', package_name, "tests", "flexbe_onboard_test_data"), + (os.path.join('share', package_name, 'tests', 'flexbe_onboard_test_data'), glob('tests/flexbe_onboard_test_data/*.xml')), ], install_requires=['setuptools'], diff --git a/flexbe_onboard/tests/flexbe_onboard_test.py b/flexbe_onboard/tests/flexbe_onboard_test.py index aba1097..7f71ef3 100644 --- a/flexbe_onboard/tests/flexbe_onboard_test.py +++ b/flexbe_onboard/tests/flexbe_onboard_test.py @@ -1,4 +1,4 @@ -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -31,14 +31,17 @@ import os import sys + import launch + import launch_testing.actions + import pytest @pytest.mark.rostest def generate_test_description(): - + """Generate test description for flexbe_onboard_test.""" path_to_test = os.path.dirname(__file__) TEST_PROC_PATH = os.path.join(path_to_test, 'test_onboard.py') diff --git a/flexbe_onboard/tests/flexbe_onboard_test_data/__init__.py b/flexbe_onboard/tests/flexbe_onboard_test_data/__init__.py index e69de29..3803871 100644 --- a/flexbe_onboard/tests/flexbe_onboard_test_data/__init__.py +++ b/flexbe_onboard/tests/flexbe_onboard_test_data/__init__.py @@ -0,0 +1,31 @@ +#!/usr/bin/env python + +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the Philipp Schillinger, Team ViGIR, Christopher Newport University nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""Initialize package for flexbe_onboard_test_data.""" diff --git a/flexbe_onboard/tests/flexbe_onboard_test_data/complex_behavior_test_sm.py b/flexbe_onboard/tests/flexbe_onboard_test_data/complex_behavior_test_sm.py index 6e0b3f8..51dffd9 100644 --- a/flexbe_onboard/tests/flexbe_onboard_test_data/complex_behavior_test_sm.py +++ b/flexbe_onboard/tests/flexbe_onboard_test_data/complex_behavior_test_sm.py @@ -1,7 +1,7 @@ #!/usr/bin/env python # -*- coding: utf-8 -*- -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -39,14 +39,16 @@ __test__ = False # Do not pytest this class (it is the test!) -from flexbe_INVALID import Behavior, Autonomy, OperatableStateMachine -from flexbe_states.wait_state import WaitState +from flexbe_INVALID import Autonomy, Behavior, OperatableStateMachine + +from flexbe_states.calculation_state import CalculationState from flexbe_states.decision_state import DecisionState from flexbe_states.log_state import LogState as flexbe_states__LogState -from flexbe_states.calculation_state import CalculationState +from flexbe_states.wait_state import WaitState + # Additional imports can be added inside the following tags # [MANUAL_IMPORT] -raise ValueError("TODO: Remove!") +raise ValueError('TODO: Remove!') # [/MANUAL_IMPORT] @@ -68,6 +70,7 @@ class ComplexBehaviorTestSM(Behavior): __test__ = False # Do not pytest this class (it is the test!) def __init__(self, node): + """Initialize ComplexBehaviorTestSM instance.""" super(ComplexBehaviorTestSM, self).__init__() self.name = 'Complex Behavior Test' self.node = node @@ -92,6 +95,7 @@ def __init__(self, node): # Behavior comments: def create(self): + """Create SM instance.""" # x:67 y:463, x:336 y:160 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['data']) _state_machine.userdata.data = 0 diff --git a/flexbe_onboard/tests/flexbe_onboard_test_data/log_behavior_test_sm.py b/flexbe_onboard/tests/flexbe_onboard_test_data/log_behavior_test_sm.py index 2c718b1..062ec19 100644 --- a/flexbe_onboard/tests/flexbe_onboard_test_data/log_behavior_test_sm.py +++ b/flexbe_onboard/tests/flexbe_onboard_test_data/log_behavior_test_sm.py @@ -1,7 +1,7 @@ #!/usr/bin/env python # -*- coding: utf-8 -*- -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -37,7 +37,8 @@ # Only code inside the [MANUAL] tags will be kept. # ########################################################### -from flexbe_core import Behavior, Autonomy, OperatableStateMachine +from flexbe_core import Autonomy, Behavior, OperatableStateMachine + from flexbe_states.log_state import LogState as flexbe_states__LogState from flexbe_states.wait_state import WaitState as flexbe_states__WaitState @@ -61,6 +62,7 @@ class LogBehaviorTestSM(Behavior): __test__ = False # Do not pytest this class (it is the test!) def __init__(self, node): + """Initialize LogBehaviorTestSM instance.""" super(LogBehaviorTestSM, self).__init__() self.name = 'Log Behavior Test' self.node = node @@ -80,6 +82,7 @@ def __init__(self, node): # Behavior comments: def create(self): + """Create state machine for test.""" # x:30 y:365, x:130 y:365 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) @@ -91,7 +94,7 @@ def create(self): with _state_machine: # x:30 y:40 OperatableStateMachine.add('Log', - flexbe_states__LogState(text="Test data", severity=2), + flexbe_states__LogState(text='Test data', severity=2), transitions={'done': 'Wait'}, autonomy={'done': Autonomy.Off}) # x:30 y:90 diff --git a/flexbe_onboard/tests/test_onboard.py b/flexbe_onboard/tests/test_onboard.py index 737d034..6ae9743 100755 --- a/flexbe_onboard/tests/test_onboard.py +++ b/flexbe_onboard/tests/test_onboard.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -31,20 +31,22 @@ """Test for onboard behaviors.""" -import sys import multiprocessing import os +import sys +import time import unittest import zlib -import time -import rclpy -from rclpy.executors import MultiThreadedExecutor -from flexbe_onboard import FlexbeOnboard from flexbe_core.core.topics import Topics from flexbe_core.proxy import ProxySubscriberCached -from flexbe_msgs.msg import BehaviorSelection, BEStatus, BehaviorLog, BehaviorModification +from flexbe_msgs.msg import BEStatus, BehaviorLog, BehaviorModification, BehaviorSelection + +from flexbe_onboard import FlexbeOnboard + +import rclpy +from rclpy.executors import MultiThreadedExecutor class TestOnboard(unittest.TestCase): @@ -52,6 +54,7 @@ class TestOnboard(unittest.TestCase): @classmethod def setUp(self): + """Set up the onboard test.""" self.context = rclpy.context.Context() rclpy.init(context=self.context) num_threads = max(2, multiprocessing.cpu_count() - 1) @@ -78,12 +81,13 @@ def setUp(self): @classmethod def tearDown(self): + """Tear down the onboard test.""" self.node.destroy_node() self.onboard.destroy_node() self.executor.shutdown() rclpy.shutdown(context=self.context) - def assertStatus(self, expected, timeout, message=""): + def assertStatus(self, expected, timeout, message=''): """Assert that the expected onboard status is received before the timeout.""" self.executor.spin_once(timeout_sec=0.01) for i in range(int(timeout * 100)): @@ -97,21 +101,23 @@ def assertStatus(self, expected, timeout, message=""): raise AssertionError('Did not receive a status as required.') msg = self.sub.get_last_msg(Topics._ONBOARD_STATUS_TOPIC) self.sub.remove_last_msg(Topics._ONBOARD_STATUS_TOPIC) - self.node.get_logger().info(f"assertStatus: msg= {str(msg)} expected={expected} - {message}?") + self.node.get_logger().info(f'assertStatus: msg= {str(msg)} expected={expected} - {message}?') self.assertEqual(msg.code, expected, msg=message) return msg def clear_extra_heartbeat_ready_messages(self): + """Clear heartbeat ready messages.""" while self.sub.has_msg(Topics._ONBOARD_STATUS_TOPIC): msg = self.sub.get_last_msg(Topics._ONBOARD_STATUS_TOPIC) if msg.code == BEStatus.READY: - self.node.get_logger().info(f"clearing READY msg={str(msg)}") + self.node.get_logger().info(f'clearing READY msg={str(msg)}') self.sub.remove_last_msg(Topics._ONBOARD_STATUS_TOPIC) # Clear any heartbeat start up messages else: break self.executor.spin_once(timeout_sec=0.01) def test_onboard_behaviors(self): + """Test onboard behaviors.""" self.executor.spin_once(timeout_sec=1) behavior_pub = self.node.create_publisher(BehaviorSelection, Topics._START_BEHAVIOR_TOPIC, 1) @@ -121,22 +127,22 @@ def test_onboard_behaviors(self): self.executor.spin_once(timeout_sec=0.1) # wait for the initial status message - self.assertStatus(BEStatus.READY, 1, "BE is ready") + self.assertStatus(BEStatus.READY, 1, 'BE is ready') # send simple behavior request without checksum - be_key, _ = self.lib.find_behavior("Log Behavior Test") + be_key, _ = self.lib.find_behavior('Log Behavior Test') request = BehaviorSelection() request.behavior_key = be_key request.autonomy_level = 255 self.clear_extra_heartbeat_ready_messages() - self.node.get_logger().info("Publish request ...") + self.node.get_logger().info('Publish request ...') behavior_pub.publish(request) self.executor.spin_once(timeout_sec=0.1) - self.node.get_logger().info("Check for expected error ...") - self.assertStatus(BEStatus.ERROR, 2, "Error - checksum test") + self.node.get_logger().info('Check for expected error ...') + self.assertStatus(BEStatus.ERROR, 2, 'Error - checksum test') # send valid simple behavior request with open(self.lib.get_sourcecode_filepath(be_key)) as f: @@ -145,12 +151,12 @@ def test_onboard_behaviors(self): self.clear_extra_heartbeat_ready_messages() - self.node.get_logger().info("Publish with checksum ...") + self.node.get_logger().info('Publish with checksum ...') behavior_pub.publish(request) self.executor.spin_once(timeout_sec=0.1) - self.assertStatus(BEStatus.STARTED, 1, "Started simple log behavior") - self.assertStatus(BEStatus.FINISHED, 3, "Finished simple log behavior") + self.assertStatus(BEStatus.STARTED, 1, 'Started simple log behavior') + self.assertStatus(BEStatus.FINISHED, 3, 'Finished simple log behavior') behavior_logs = [] while self.sub.has_buffered(Topics._BEHAVIOR_LOGGING_TOPIC): behavior_logs.append(self.sub.get_from_buffer(Topics._BEHAVIOR_LOGGING_TOPIC).text) @@ -158,8 +164,8 @@ def test_onboard_behaviors(self): self.assertIn('Test data', behavior_logs) # send valid complex behavior request - self.node.get_logger().info("Request to find (INVALID) complex behavior ...") - be_key, _ = self.lib.find_behavior("Complex Behavior Test") + self.node.get_logger().info('Request to find (INVALID) complex behavior ...') + be_key, _ = self.lib.find_behavior('Complex Behavior Test') request = BehaviorSelection() request.behavior_key = be_key request.autonomy_level = 255 @@ -170,26 +176,26 @@ def test_onboard_behaviors(self): with open(self.lib.get_sourcecode_filepath(be_key)) as f: content = f.read() - self.node.get_logger().info("Request behavior modification of (INVALID) complex behavior ...") - modifications = [('INVALID', 'core'), ('raise ValueError("TODO: Remove!")', '')] + self.node.get_logger().info('Request behavior modification of (INVALID) complex behavior ...') + modifications = [('INVALID', 'core'), ("raise ValueError('TODO: Remove!')", '')] for replace, by in modifications: index = content.index(replace) request.modifications.append(BehaviorModification(index_begin=index, index_end=index + len(replace), new_content=by)) for replace, by in modifications: content = content.replace(replace, by) - self.node.get_logger().info("Modified modified behavior ...") + self.node.get_logger().info('Modified modified behavior ...') self.node.get_logger().info(content) - self.node.get_logger().info(30 * "=" + "\n\n") + self.node.get_logger().info(30 * '=' + '\n\n') request.behavior_id = zlib.adler32(content.encode()) & 0x7fffffff self.clear_extra_heartbeat_ready_messages() - self.node.get_logger().info("Publish modified behavior ...") + self.node.get_logger().info('Publish modified behavior ...') behavior_pub.publish(request) self.executor.spin_once(timeout_sec=0.1) - self.assertStatus(BEStatus.STARTED, 1, "Started modified") - result = self.assertStatus(BEStatus.FINISHED, 3, "Finished modified") + self.assertStatus(BEStatus.STARTED, 1, 'Started modified') + result = self.assertStatus(BEStatus.FINISHED, 3, 'Finished modified') self.assertEqual(result.args[0], 'finished') behavior_logs = [] @@ -205,15 +211,15 @@ def test_onboard_behaviors(self): self.clear_extra_heartbeat_ready_messages() # send the same behavior with different parameters - self.node.get_logger().info("Republish modified behavior ...") + self.node.get_logger().info('Republish modified behavior ...') request.arg_keys = ['param', 'invalid'] request.arg_values = ['value_1', 'should be ignored'] request.input_keys = [] request.input_values = [] behavior_pub.publish(request) self.executor.spin_once(timeout_sec=0.1) - self.assertStatus(BEStatus.STARTED, 1, "Started modified parameters") - result = self.assertStatus(BEStatus.FINISHED, 3, "Finished modified parameters") + self.assertStatus(BEStatus.STARTED, 1, 'Started modified parameters') + result = self.assertStatus(BEStatus.FINISHED, 3, 'Finished modified parameters') self.assertEqual(result.args[0], 'failed') behavior_logs = [] @@ -226,7 +232,7 @@ def test_onboard_behaviors(self): behavior_logs.append(self.sub.get_from_buffer(Topics._BEHAVIOR_LOGGING_TOPIC).text) self.executor.spin_once(timeout_sec=0.1) self.assertIn('value_1', behavior_logs) - self.node.get_logger().info("Done onboard testing!") + self.node.get_logger().info('Done onboard testing!') self.executor.spin_once(timeout_sec=0.1) diff --git a/flexbe_states/flexbe_states/__init__.py b/flexbe_states/flexbe_states/__init__.py index 13fee50..ed481e2 100644 --- a/flexbe_states/flexbe_states/__init__.py +++ b/flexbe_states/flexbe_states/__init__.py @@ -1,4 +1,4 @@ -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -26,5 +26,4 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. - -# import roslib; roslib.load_manifest('flexbe_states') +"""Initialize the flexbe_states package.""" diff --git a/flexbe_states/flexbe_states/calculation_state.py b/flexbe_states/flexbe_states/calculation_state.py index f68f366..5baa806 100644 --- a/flexbe_states/flexbe_states/calculation_state.py +++ b/flexbe_states/flexbe_states/calculation_state.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -28,7 +28,7 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. - +"""CalculationState.""" from flexbe_core import EventState, Logger @@ -39,18 +39,19 @@ class CalculationState(EventState): calculation is a function which takes exactly one parameter, input_value from userdata, and its return value is stored in output_value after leaving the state. - -- calculation function The function that performs the desired calculation. + -- calculation function The function that performs the desired calculation. It could be a private function (self.foo) manually defined in a behavior's source code or a lambda function (e.g., lambda x: x^2, where x will be the input_value). - ># input_value object Input to the calculation function. + ># input_value object Input to the calculation function. - #> output_value object The result of the calculation. + #> output_value object The result of the calculation. - <= done Indicates completion of the calculation. + <= done Indicates completion of the calculation. """ def __init__(self, calculation): + """Initialize the CalculationState instance.""" super(CalculationState, self).__init__(outcomes=['done'], input_keys=['input_value'], output_keys=['output_value']) @@ -58,11 +59,13 @@ def __init__(self, calculation): self._calculation_result = None def execute(self, userdata): + """Set userdata with calculation result and return done.""" userdata.output_value = self._calculation_result # nothing to check return 'done' def on_enter(self, userdata): + """Call calculation on entering state.""" if self._calculation is not None: try: self._calculation_result = self._calculation(userdata.input_value) diff --git a/flexbe_states/flexbe_states/check_condition_state.py b/flexbe_states/flexbe_states/check_condition_state.py index 1f46ed1..c5d818d 100644 --- a/flexbe_states/flexbe_states/check_condition_state.py +++ b/flexbe_states/flexbe_states/check_condition_state.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -28,7 +28,7 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. - +"""CheckConditionState.""" from flexbe_core import EventState, Logger @@ -48,27 +48,30 @@ class CheckConditionState(EventState): """ def __init__(self, predicate): + """Initialize state.""" super(CheckConditionState, self).__init__(outcomes=['true', 'false'], input_keys=['input_value']) self._predicate = None if callable(predicate): self._predicate = predicate elif isinstance(predicate, str): - if "__" in predicate: + if '__' in predicate: Logger.logwarn(f"potentially unsafe code in predicate '{predicate}' - use caution if executing!") try: self._predicate = eval(predicate) # Assumes behavior is from a trusted source! except Exception as exc: # pylint: disable=W0703 - Logger.logwarn(f"Failed to convert predicate to callable function!\n {str(exc)}") + Logger.logwarn(f'Failed to convert predicate to callable function!\n {str(exc)}') self._outcome = 'false' def execute(self, userdata): + """Execute the condition outcome.""" return self._outcome def on_enter(self, userdata): + """Evaluate condition on entering state.""" try: self._outcome = 'true' if self._predicate(userdata.input_value) else 'false' except Exception as exc: # pylint: disable=W0703 - Logger.logwarn(f"Failed to execute condition function!\n {str(exc)}") + Logger.logwarn(f'Failed to execute condition function!\n {str(exc)}') self._outcome = 'false' diff --git a/flexbe_states/flexbe_states/decision_state.py b/flexbe_states/flexbe_states/decision_state.py index 14ab127..d6731f8 100644 --- a/flexbe_states/flexbe_states/decision_state.py +++ b/flexbe_states/flexbe_states/decision_state.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -29,6 +29,7 @@ # POSSIBILITY OF SUCH DAMAGE. +"""DecisionState.""" from flexbe_core import EventState, Logger @@ -38,11 +39,11 @@ class DecisionState(EventState): This state can be used if the further control flow of the behavior depends on an advanced condition. - -- outcomes string[] A list containing all possible outcomes of this state - -- conditions function Implements the condition check and returns one of the available outcomes. + -- outcomes string[] A list containing all possible outcomes of this state + -- conditions function Implements the condition check and returns one of the available outcomes. Has to expect one parameter which will be set to input_value. - ># input_value object Input to the condition function. + ># input_value object Input to the condition function. """ def __init__(self, outcomes, conditions): @@ -52,6 +53,7 @@ def __init__(self, outcomes, conditions): self._conditions = conditions def execute(self, userdata): + """Execute state and check conditions and return associated outcome.""" if self._conditions is not None: outcome = None try: diff --git a/flexbe_states/flexbe_states/flexible_calculation_state.py b/flexbe_states/flexbe_states/flexible_calculation_state.py index 4b5b581..2079ac7 100644 --- a/flexbe_states/flexbe_states/flexible_calculation_state.py +++ b/flexbe_states/flexbe_states/flexible_calculation_state.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -29,6 +29,7 @@ # POSSIBILITY OF SUCH DAMAGE. +"""FlexibleCalculationState.""" from flexbe_core import EventState, Logger @@ -59,11 +60,13 @@ def __init__(self, calculation, input_keys): self._calculation_result = None def execute(self, userdata): + """Set userdata with calculation result and return done.""" userdata.output_value = self._calculation_result # nothing to check return 'done' def on_enter(self, userdata): + """Do calculation on entering state.""" if self._calculation is not None: try: self._calculation_result = self._calculation(**{key: userdata[key] for key in self._input_keys}) diff --git a/flexbe_states/flexbe_states/flexible_check_condition_state.py b/flexbe_states/flexbe_states/flexible_check_condition_state.py index 35b770a..6065fff 100644 --- a/flexbe_states/flexbe_states/flexible_check_condition_state.py +++ b/flexbe_states/flexbe_states/flexible_check_condition_state.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -62,23 +62,25 @@ def __init__(self, predicate, input_keys): if callable(predicate): self._predicate = predicate elif isinstance(predicate, str): - if "__" in predicate: + if '__' in predicate: Logger.logwarn(f"potentially unsafe code in predicate '{predicate}' - use caution if executing!") try: self._predicate = eval(predicate) # Assumes behavior is from a trusted source! except Exception as exc: # pylint: disable=W0703 - Logger.logwarn(f"Failed to convert predicate to callable function!\n {str(exc)}") + Logger.logwarn(f'Failed to convert predicate to callable function!\n {str(exc)}') self._outcome = 'false' def execute(self, userdata): + """Execute the state.""" return self._outcome def on_enter(self, userdata): + """Call on entering state.""" if self._predicate is not None: try: - self._outcome = "true" if self._predicate([userdata[key] for key in self._input_keys]) else 'false' + self._outcome = 'true' if self._predicate([userdata[key] for key in self._input_keys]) else 'false' except Exception as exc: # pylint: disable=W0703 - Logger.logwarn(f"failed to execute condition function!\n {str(exc)}") + Logger.logwarn(f'failed to execute condition function!\n {str(exc)}') else: Logger.logwarn('Passed no predicate!') diff --git a/flexbe_states/flexbe_states/input_state.py b/flexbe_states/flexbe_states/input_state.py index a7a47f6..1f547ee 100644 --- a/flexbe_states/flexbe_states/input_state.py +++ b/flexbe_states/flexbe_states/input_state.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -28,13 +28,15 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +"""InputState.""" import ast import pickle from flexbe_core import EventState, Logger -from flexbe_msgs.action import BehaviorInput from flexbe_core.proxy import ProxyActionClient +from flexbe_msgs.action import BehaviorInput + class InputState(EventState): """ @@ -75,15 +77,18 @@ def __init__(self, request, message, timeout=1.0, action_topic='flexbe/behavior_ self._received = False def on_start(self): + """Set up proxy action client for behavior input server on behavior start.""" self._client = ProxyActionClient({self._action_topic: BehaviorInput}, wait_duration=0.0) self._connected = True def on_stop(self): + """Stop client when behavior stops.""" ProxyActionClient.remove_client(self._action_topic) self._client = None self._connected = False def execute(self, userdata): + """Execute state waiting for action result.""" if not self._connected: return 'no_connection' if self._received: @@ -105,7 +110,7 @@ def execute(self, userdata): # Never unpickle data received from an untrusted or unauthenticated source. response_data = pickle.loads(input_data) - Logger.localinfo(f" InputState returned {type(response_data)} : {response_data}") + Logger.localinfo(f' InputState returned {type(response_data)} : {response_data}') userdata.data = response_data except Exception as exc: # pylint: disable=W0703 Logger.logwarn(f"Was unable to load provided data:\n '{result.data}'\n {str(exc)}") @@ -118,6 +123,7 @@ def execute(self, userdata): return None def on_enter(self, userdata): + """Send goal to action server on entering state.""" self._received = False # Retrive the goal for the BehaviorInput Action. diff --git a/flexbe_states/flexbe_states/log_key_state.py b/flexbe_states/flexbe_states/log_key_state.py index 1688553..106566d 100644 --- a/flexbe_states/flexbe_states/log_key_state.py +++ b/flexbe_states/flexbe_states/log_key_state.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -28,7 +28,7 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. - +"""LogKeyState.""" from flexbe_core import EventState, Logger @@ -38,21 +38,23 @@ class LogKeyState(EventState): Can be used to precisely inform the operator about what happened to the behavior. - -- text string The message to be logged to the terminal Example: 'Counter value: {}'. - -- severity uint8 Type of logging (Logger.REPORT_INFO / WARN / HINT / ERROR) + -- text string The message to be logged to the terminal Example: 'Counter value: {}'. + -- severity uint8 Type of logging (Logger.REPORT_INFO / WARN / HINT / ERROR) - #> data object The data provided to be printed in the message. The exact type depends on the request. + #> data object The data provided to be printed in the message. The exact type depends on the request. - <= done Indicates that the message has been logged. + <= done Indicates that the message has been logged. """ def __init__(self, text, severity=Logger.REPORT_HINT): + """Initialize LogKeyState.""" super(LogKeyState, self).__init__(outcomes=['done'], input_keys=['data']) self._text = text self._severity = severity def execute(self, userdata): + """Return done on first execution.""" # Already logged. No need to wait for anything. return 'done' diff --git a/flexbe_states/flexbe_states/log_state.py b/flexbe_states/flexbe_states/log_state.py index 5aed4f3..5601905 100644 --- a/flexbe_states/flexbe_states/log_state.py +++ b/flexbe_states/flexbe_states/log_state.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -28,7 +28,7 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. - +"""LogState.""" from flexbe_core import EventState, Logger @@ -38,10 +38,10 @@ class LogState(EventState): Can be used to precisely inform the operator about what happened to the behavior. - -- text string The message to be logged to the terminal. - -- severity uint8 Type of logging (Logger.REPORT_INFO / WARN / HINT / ERROR) + -- text string The message to be logged to the terminal. + -- severity uint8 Type of logging (Logger.REPORT_INFO / WARN / HINT / ERROR) - <= done Indicates that the message has been logged. + <= done Indicates that the message has been logged. """ def __init__(self, text, severity=Logger.REPORT_HINT): @@ -50,6 +50,7 @@ def __init__(self, text, severity=Logger.REPORT_HINT): self._severity = severity def execute(self, userdata): + """Execute LogState.""" # Already logged. No need to wait for anything. return 'done' diff --git a/flexbe_states/flexbe_states/operator_decision_state.py b/flexbe_states/flexbe_states/operator_decision_state.py index 499c426..539f4b8 100644 --- a/flexbe_states/flexbe_states/operator_decision_state.py +++ b/flexbe_states/flexbe_states/operator_decision_state.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -28,7 +28,7 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. - +"""OperatorDecisionState.""" from flexbe_core import EventState, Logger @@ -49,16 +49,19 @@ class OperatorDecisionState(EventState): """ def __init__(self, outcomes, hint=None, suggestion=None): + """Initialize OperatorDecisionState instance.""" super(OperatorDecisionState, self).__init__(outcomes=outcomes) self._hint = hint self._suggestion = suggestion def execute(self, userdata): + """Execute until operator defines outcome.""" if self._suggestion is not None and self._suggestion in self._outcomes: return self._suggestion return None def on_enter(self, userdata): + """Call on entering state, and published suggested hint.""" if self._hint is not None: Logger.loghint(self._hint) diff --git a/flexbe_states/flexbe_states/publisher_bool_state.py b/flexbe_states/flexbe_states/publisher_bool_state.py index b161ab2..ac33059 100644 --- a/flexbe_states/flexbe_states/publisher_bool_state.py +++ b/flexbe_states/flexbe_states/publisher_bool_state.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -28,9 +28,10 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. - +"""PublisherBoolState.""" from flexbe_core import EventState from flexbe_core.proxy import ProxyPublisher + from std_msgs.msg import Bool @@ -51,9 +52,11 @@ def __init__(self, topic): self._pub = ProxyPublisher({self._topic: Bool}) def execute(self, userdata): + """Return done.""" return 'done' def on_enter(self, userdata): + """Publish boolean value from userdata on entering state.""" val = Bool() val.data = userdata.value self._pub.publish(self._topic, val) diff --git a/flexbe_states/flexbe_states/publisher_empty_state.py b/flexbe_states/flexbe_states/publisher_empty_state.py index ff84eb9..303249d 100644 --- a/flexbe_states/flexbe_states/publisher_empty_state.py +++ b/flexbe_states/flexbe_states/publisher_empty_state.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -28,9 +28,10 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. - +"""PublisherEmptyState.""" from flexbe_core import EventState from flexbe_core.proxy import ProxyPublisher + from std_msgs.msg import Empty @@ -44,12 +45,15 @@ class PublisherEmptyState(EventState): """ def __init__(self, topic): + """Initialize PublisherEmptyState instance.""" super(PublisherEmptyState, self).__init__(outcomes=['done']) self._topic = topic self._pub = ProxyPublisher({self._topic: Empty}) def execute(self, userdata): + """Execute the state.""" return 'done' def on_enter(self, userdata): + """Publish on entering the state.""" self._pub.publish(self._topic, Empty()) diff --git a/flexbe_states/flexbe_states/publisher_string_state.py b/flexbe_states/flexbe_states/publisher_string_state.py index 6581729..30e0b74 100644 --- a/flexbe_states/flexbe_states/publisher_string_state.py +++ b/flexbe_states/flexbe_states/publisher_string_state.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -28,9 +28,10 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. - +"""PublisherStringState.""" from flexbe_core import EventState from flexbe_core.proxy import ProxyPublisher + from std_msgs.msg import String @@ -38,22 +39,25 @@ class PublisherStringState(EventState): """ Publishes a string (std_msgs/String) message on a given topic name. - -- topic string The topic on which should be published. + -- topic string The topic on which should be published. - >= value Value of string. + >= value Value of string. - <= done Done publishing. + <= done Done publishing. """ def __init__(self, topic): + """Initialize the instance.""" super(PublisherStringState, self).__init__(outcomes=['done'], input_keys=['value']) self._topic = topic self._pub = ProxyPublisher({self._topic: String}) def execute(self, userdata): + """Return done on first execution.""" return 'done' def on_enter(self, userdata): + """Publish the string on entering state.""" val = String() val.data = userdata.value self._pub.publish(self._topic, val) diff --git a/flexbe_states/flexbe_states/subscriber_state.py b/flexbe_states/flexbe_states/subscriber_state.py index fa84eff..9b81d2f 100644 --- a/flexbe_states/flexbe_states/subscriber_state.py +++ b/flexbe_states/flexbe_states/subscriber_state.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -28,9 +28,9 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +"""SubscriberState.""" from flexbe_core import EventState, Logger - from flexbe_core.proxy import ProxySubscriberCached from flexbe_core.proxy.qos import QOS_DEFAULT @@ -52,7 +52,8 @@ class SubscriberState(EventState): <= unavailable The topic is not available when this state becomes actives. """ - def __init__(self, topic, msg_type="", blocking=True, clear=False, qos=QOS_DEFAULT): + def __init__(self, topic, msg_type='', blocking=True, clear=False, qos=QOS_DEFAULT): + """Initialize the SubscriberState instance.""" super(SubscriberState, self).__init__(outcomes=['received', 'unavailable'], output_keys=['message']) self._topic = topic @@ -63,15 +64,18 @@ def __init__(self, topic, msg_type="", blocking=True, clear=False, qos=QOS_DEFAU self._connected = False self._sub = None if not self._connect(): + # If topic is not available, we will retry on_enter Logger.logwarn('Topic %s for state %s not yet available.\n' 'Will try again when entering the state...' % (self._topic, self.name)) def on_stop(self): + """Unsubscribe topic when behavior stops.""" if self._connected: ProxySubscriberCached.unsubscribe_topic(self._topic) self._connected = False def execute(self, userdata): + """Execute and update user data with the last message received.""" if not self._connected: userdata.message = None return 'unavailable' @@ -84,6 +88,7 @@ def execute(self, userdata): return None def on_enter(self, userdata): + """Subscribe to topic if not connected, and clear old messages.""" if not self._connected: if self._connect(): Logger.loginfo('Successfully subscribed to previously unavailable topic %s' % self._topic) @@ -94,6 +99,7 @@ def on_enter(self, userdata): self._sub.remove_last_msg(self._topic) def _connect(self): + """Connect to publisher.""" try: self._sub = ProxySubscriberCached({self._topic: self._msg_type}, qos=self._qos, inst_id=id(self)) self._connected = True diff --git a/flexbe_states/flexbe_states/wait_state.py b/flexbe_states/flexbe_states/wait_state.py index bbfc786..17f5948 100644 --- a/flexbe_states/flexbe_states/wait_state.py +++ b/flexbe_states/flexbe_states/wait_state.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -30,7 +30,6 @@ """WaitState.""" - from flexbe_core import EventState @@ -38,16 +37,18 @@ class WaitState(EventState): """ Implements a state that can be used to wait on timed process. - -- wait_time float Amount of time to wait in seconds. + -- wait_time float Amount of time to wait in seconds. - <= done Indicates that the wait time has elapsed. + <= done Indicates that the wait time has elapsed. """ def __init__(self, wait_time): + """Initialize the WaitState.""" super(WaitState, self).__init__(outcomes=['done']) self._wait = wait_time def execute(self, userdata): + """Execute the WaitState and return after time elapses.""" elapsed = WaitState._node.get_clock().now() - self._start_time if elapsed.nanoseconds * 10 ** -9 > self._wait: return 'done' @@ -55,5 +56,5 @@ def execute(self, userdata): return None def on_enter(self, userdata): - """Upon entering the state, save the current time and start waiting.""" + """Upon entering the WaitState, save the current time.""" self._start_time = WaitState._node.get_clock().now() diff --git a/flexbe_states/setup.py b/flexbe_states/setup.py index 45a2ce4..2bc4ba0 100644 --- a/flexbe_states/setup.py +++ b/flexbe_states/setup.py @@ -2,8 +2,8 @@ from glob import glob -from setuptools import setup from setuptools import find_packages +from setuptools import setup PACKAGE_NAME = 'flexbe_states' @@ -14,9 +14,9 @@ data_files=[ ('share/ament_index/resource_index/packages', ['resource/' + PACKAGE_NAME]), ('share/' + PACKAGE_NAME, ['package.xml']), - ('share/' + PACKAGE_NAME + "/tests", glob('tests/*.test')), - ('share/' + PACKAGE_NAME + "/launch", glob('tests/*.launch.py')), - # ros2 bag issues - ('share/' + PACKAGE_NAME + "/tests/bags", glob('tests/bags/*.bag')), + ('share/' + PACKAGE_NAME + '/tests', glob('tests/*.test')), + ('share/' + PACKAGE_NAME + '/launch', glob('tests/*.launch.py')), + # ros2 bag issues - ('share/' + PACKAGE_NAME + '/tests/bags', glob('tests/bags/*.bag')), ], install_requires=['setuptools'], zip_safe=True, diff --git a/flexbe_states/tests/__init__.py b/flexbe_states/tests/__init__.py index e69de29..7f12e2d 100644 --- a/flexbe_states/tests/__init__.py +++ b/flexbe_states/tests/__init__.py @@ -0,0 +1,31 @@ +#!/usr/bin/env python + +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the Philipp Schillinger, Team ViGIR, Christopher Newport University nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""Initialize flexbe_states.tests package.""" diff --git a/flexbe_states/tests/pub.launch.py b/flexbe_states/tests/pub.launch.py index a3abe24..547e104 100644 --- a/flexbe_states/tests/pub.launch.py +++ b/flexbe_states/tests/pub.launch.py @@ -1,4 +1,4 @@ -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -31,7 +31,7 @@ # ROS 1 style # -# +# # from launch import LaunchDescription @@ -39,6 +39,7 @@ def generate_launch_description(): + """Generate launch description.""" return LaunchDescription([ ExecuteProcess(cmd=['ros2', 'topic', 'pub', '/test', 'geometry_msgs/Pose', '{position: {x: 6.4}}']) ]) diff --git a/flexbe_states/tests/run_colcon_test.py b/flexbe_states/tests/run_colcon_test.py index 444eccd..b376dba 100644 --- a/flexbe_states/tests/run_colcon_test.py +++ b/flexbe_states/tests/run_colcon_test.py @@ -1,4 +1,4 @@ -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -28,8 +28,6 @@ """Pytest testing for flexbe_states.""" - - from flexbe_testing.py_tester import PyTester @@ -42,63 +40,63 @@ def __init__(self, *args, **kwargs): @classmethod def setUpClass(cls): - - PyTester._package = "flexbe_states" - PyTester._tests_folder = "tests" + """Set up TestFlexBEStates class.""" + PyTester._package = 'flexbe_states' + PyTester._tests_folder = 'tests' super().setUpClass() # Do this last after setting package and tests folder # The tests def test_calculation_state_simple(self): """Run FlexBE unit test given .test file.""" - self.run_test("calculation_state_simple") + self.run_test('calculation_state_simple') def test_calculation_state_none(self): """Run FlexBE unit test given .test file.""" - self.run_test("calculation_state_none") + self.run_test('calculation_state_none') def test_check_condition_state_true(self): """Run FlexBE unit test given .test file.""" - self.run_test("check_condition_state_true") + self.run_test('check_condition_state_true') def test_check_condition_state_invalid(self): """Run FlexBE unit test given .test file.""" - self.run_test("check_condition_state_invalid") + self.run_test('check_condition_state_invalid') def test_decision_state_simple(self): """Run FlexBE unit test given .test file.""" - self.run_test("decision_state_simple") + self.run_test('decision_state_simple') def test_flexible_calculation_state_simple(self): """Run FlexBE unit test given .test file.""" - self.run_test("flexible_calculation_state_simple") + self.run_test('flexible_calculation_state_simple') def test_input_state_import(self): """Run FlexBE unit test given .test file.""" - self.run_test("input_state_import") + self.run_test('input_state_import') def test_log_state_string(self): """Run FlexBE unit test given .test file.""" - self.run_test("log_state_string") + self.run_test('log_state_string') def test_log_state_int(self): """Run FlexBE unit test given .test file.""" - self.run_test("log_state_int") + self.run_test('log_state_int') def test_operator_decision_state_suggested(self): """Run FlexBE unit test given .test file.""" - self.run_test("operator_decision_state_suggested") + self.run_test('operator_decision_state_suggested') def test_subscriber_state_unavailable(self): """Run FlexBE unit test given .test file.""" - self.run_test("subscriber_state_unavailable") + self.run_test('subscriber_state_unavailable') # # #### issues with pub/yaml loading Pose - subscriber_state_pose # # ##### Not launching and handling properly - skip for now # def test_subscriber_state_pose(self): # """Run FlexBE unit test given .test file.""" # # This test requires longer wait than normal - # self.run_test("subscriber_state_pose", timeout_sec=1.5, max_cnt=5000) + # self.run_test('subscriber_state_pose', timeout_sec=1.5, max_cnt=5000) def test_wait_state_short(self): """ @@ -106,7 +104,7 @@ def test_wait_state_short(self): This test requires longer wait than normal """ - self.run_test("wait_state_short", timeout_sec=1.5, max_cnt=5000) + self.run_test('wait_state_short', timeout_sec=1.5, max_cnt=5000) # Tests with issues # # ### ros2 bag issues - log_state_msg diff --git a/flexbe_states/tests/run_tests.launch.py b/flexbe_states/tests/run_tests.launch.py index 40183d1..97f27a8 100644 --- a/flexbe_states/tests/run_tests.launch.py +++ b/flexbe_states/tests/run_tests.launch.py @@ -1,4 +1,4 @@ -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -30,11 +30,12 @@ """Flexbe_states testing.""" from os.path import join +from ament_index_python.packages import get_package_share_directory + from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.substitutions import LaunchConfiguration from launch.launch_description_sources import PythonLaunchDescriptionSource -from ament_index_python.packages import get_package_share_directory +from launch.substitutions import LaunchConfiguration def generate_launch_description(): @@ -42,34 +43,34 @@ def generate_launch_description(): flexbe_testing_dir = get_package_share_directory('flexbe_testing') flexbe_states_test_dir = get_package_share_directory('flexbe_states') - path = join(flexbe_states_test_dir, "tests") + path = join(flexbe_states_test_dir, 'tests') - testcases = "" - testcases += join(path, "calculation_state_simple.test") + "\n" - testcases += join(path, "calculation_state_none.test") + "\n" - testcases += join(path, "check_condition_state_true.test") + "\n" - testcases += join(path, "check_condition_state_invalid.test") + "\n" - testcases += join(path, "decision_state_simple.test") + "\n" - testcases += join(path, "flexible_calculation_state_simple.test") + "\n" - testcases += join(path, "input_state_import.test") + "\n" - testcases += join(path, "log_state_string.test") + "\n" - testcases += join(path, "log_state_int.test") + "\n" - # ### ros2 bag issues - testcases += join(path, "log_state_msg.test") + "\n" - testcases += join(path, "operator_decision_state_suggested.test") + "\n" - testcases += join(path, "subscriber_state_unavailable.test") + "\n" - # #### issues with pub/yaml in test testcases += join(path, "subscriber_state_pose.test") + "\n" - testcases += join(path, "wait_state_short.test") + "\n" + testcases = '' + testcases += join(path, 'calculation_state_simple.test') + '\n' + testcases += join(path, 'calculation_state_none.test') + '\n' + testcases += join(path, 'check_condition_state_true.test') + '\n' + testcases += join(path, 'check_condition_state_invalid.test') + '\n' + testcases += join(path, 'decision_state_simple.test') + '\n' + testcases += join(path, 'flexible_calculation_state_simple.test') + '\n' + testcases += join(path, 'input_state_import.test') + '\n' + testcases += join(path, 'log_state_string.test') + '\n' + testcases += join(path, 'log_state_int.test') + '\n' + # ### ros2 bag issues - testcases += join(path, 'log_state_msg.test') + '\n' + testcases += join(path, 'operator_decision_state_suggested.test') + '\n' + testcases += join(path, 'subscriber_state_unavailable.test') + '\n' + # #### issues with pub/yaml in test testcases += join(path, 'subscriber_state_pose.test') + '\n' + testcases += join(path, 'wait_state_short.test') + '\n' return LaunchDescription([ - DeclareLaunchArgument("pkg", default_value="flexbe_states"), # flexbe_testing"), - DeclareLaunchArgument("testcases", default_value=testcases), - DeclareLaunchArgument("compact_format", default_value='true'), + DeclareLaunchArgument('pkg', default_value='flexbe_states'), + DeclareLaunchArgument('testcases', default_value=testcases), + DeclareLaunchArgument('compact_format', default_value='true'), IncludeLaunchDescription( - PythonLaunchDescriptionSource(join(flexbe_testing_dir, "launch", "flexbe_testing.launch.py")), + PythonLaunchDescriptionSource(join(flexbe_testing_dir, 'launch', 'flexbe_testing.launch.py')), launch_arguments={ - 'package': LaunchConfiguration("pkg"), - 'compact_format': LaunchConfiguration("compact_format"), - 'testcases': LaunchConfiguration("testcases"), + 'package': LaunchConfiguration('pkg'), + 'compact_format': LaunchConfiguration('compact_format'), + 'testcases': LaunchConfiguration('testcases'), }.items() ) ]) diff --git a/flexbe_testing/flexbe_testing/__init__.py b/flexbe_testing/flexbe_testing/__init__.py index d701eea..edb5f18 100644 --- a/flexbe_testing/flexbe_testing/__init__.py +++ b/flexbe_testing/flexbe_testing/__init__.py @@ -1,4 +1,4 @@ -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -26,5 +26,6 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +"""Initialize flexbe_testing package.""" from .tester import Tester # noqa: F401 diff --git a/flexbe_testing/flexbe_testing/data_provider.py b/flexbe_testing/flexbe_testing/data_provider.py index 01413d0..de1d316 100644 --- a/flexbe_testing/flexbe_testing/data_provider.py +++ b/flexbe_testing/flexbe_testing/data_provider.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -32,8 +32,9 @@ """Provides an interface for required test case data.""" import os -# import rosbag + from ament_index_python.packages import get_package_share_directory + from .logger import Logger @@ -43,6 +44,7 @@ class DataProvider: __test__ = False # Do not pytest this class (it is the test!) def __init__(self, node, bagfile=None): + """Initialize DataProvider instance.""" self.node = node Logger.initialize(node) self._bag = None @@ -58,7 +60,7 @@ def __init__(self, node, bagfile=None): bagpath = os.path.join(pkgpath, '/'.join(bagfile.split('/')[1:])) # storage_options, converter_options = get_rosbag_options(bagpath) - raise NotImplementedError("ROS bag loading not implemented in ROS 2 - TODO!") + raise NotImplementedError('ROS bag loading not implemented in ROS 2 - TODO!') # self._bag = rosbag2_py.SequentialReader() # self._bag.open(storage_options, converter_options) @@ -89,7 +91,7 @@ def parse(self, value): elif isinstance(value, str) and len(value) > 1 and value[0] == '/' and value[1] == '/': result = value[1:] except Exception as e: - Logger.print_error('unable to parse value "%s" (will be considered as string):\n\t%s' % ( + Logger.print_error("unable to parse value '%s' (will be considered as string):\n\t%s" % ( str(value), str(e) )) result = str(value) diff --git a/flexbe_testing/flexbe_testing/logger.py b/flexbe_testing/flexbe_testing/logger.py index 4f4e672..c355ab6 100644 --- a/flexbe_testing/flexbe_testing/logger.py +++ b/flexbe_testing/flexbe_testing/logger.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -32,6 +32,7 @@ """Bundles static methods for test case logging.""" import traceback + from rclpy.node import Node @@ -44,10 +45,12 @@ class Logger: @staticmethod def initialize(node: Node): + """Initialize node.""" Logger._node = node @classmethod def _param_positive(cls): + """Check print debug positive.""" print_debug_positive = False try: print_debug_positive = Logger._node.get_parameter('~print_debug_positive').get_parameter_value().bool_value @@ -59,6 +62,7 @@ def _param_positive(cls): @classmethod def _param_negative(cls): + """Check print debug negative.""" print_debug_negative = False try: print_debug_negative = Logger._node.get_parameter('~print_debug_negative').get_parameter_value().bool_value @@ -161,4 +165,4 @@ def print_error(cls, text): def __init__(self): """DO NOT USE: use class print methods instead.""" - raise NotImplementedError("use static methods and attributes") + raise NotImplementedError('use static methods and attributes') diff --git a/flexbe_testing/flexbe_testing/py_tester.py b/flexbe_testing/flexbe_testing/py_tester.py index 5fd3170..9014682 100644 --- a/flexbe_testing/flexbe_testing/py_tester.py +++ b/flexbe_testing/flexbe_testing/py_tester.py @@ -1,4 +1,4 @@ -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -31,16 +31,18 @@ import time import unittest -import yaml - from os.path import join from ament_index_python.packages import get_package_share_directory +from flexbe_core.proxy import initialize_proxies, shutdown_proxies + +from flexbe_testing.tester import Tester + import rclpy from rclpy.executors import MultiThreadedExecutor -from flexbe_testing.tester import Tester -from flexbe_core.proxy import initialize_proxies, shutdown_proxies + +import yaml class PyTester(unittest.TestCase): @@ -68,28 +70,29 @@ def __init__(self, *args, **kwargs): @classmethod def setUpClass(cls): - """Must override to define package and tests folder for each test.""" + """Override to define package and tests folder for each test.""" super().setUpClass() package_dir = get_package_share_directory(PyTester._package) PyTester._file_path = join(package_dir, PyTester._tests_folder) def setUp(self): - assert PyTester._package is not None, "Must define setUpClass() for particular test" - assert PyTester._file_path is not None, "Must define setUpClass() for particular test" + """Set up the tester class.""" + assert PyTester._package is not None, 'Must define setUpClass() for particular test' + assert PyTester._file_path is not None, 'Must define setUpClass() for particular test' - print("setUp test and initialize ROS ...", flush=True) + print('setUp test and initialize ROS ...', flush=True) self.context = rclpy.context.Context() rclpy.init(context=self.context) # Set up ROS self.executor = MultiThreadedExecutor(context=self.context) - self.node = rclpy.create_node(f"flexbe_states_test_{self._test}", context=self.context) + self.node = rclpy.create_node(f'flexbe_states_test_{self._test}', context=self.context) self.executor.add_node(self.node) - self.node.declare_parameter("~print_debug_positive", True) - self.node.declare_parameter("~print_debug_negative", True) - self.node.declare_parameter("~compact_format", False) - self.node.get_logger().info(f" setUp - rclpy.ok={rclpy.ok(context=self.context)} context={self.context.ok()}") + self.node.declare_parameter('~print_debug_positive', True) + self.node.declare_parameter('~print_debug_negative', True) + self.node.declare_parameter('~compact_format', False) + self.node.get_logger().info(f' setUp - rclpy.ok={rclpy.ok(context=self.context)} context={self.context.ok()}') initialize_proxies(self.node) @@ -98,16 +101,17 @@ def setUp(self): rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.01) def tearDown(self): - self.node.get_logger().info(f" shutting down {PyTester._package} test {self._test} ...") + """Tear down the the tester instance.""" + self.node.get_logger().info(f' shutting down {PyTester._package} test {self._test} ...') PyTester._test += 1 # increment for next test for i in range(10): # Allow any lingering pub/sub to clear up rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.01) - # self.node.get_logger().info(" shutting down proxies in core test %d ... " % (self._test)) + # self.node.get_logger().info(' shutting down proxies in core test %d ... ' % (self._test)) shutdown_proxies() - # self.node.get_logger().info(" destroy node in core test %d ... " % (self._test)) + # self.node.get_logger().info(' destroy node in core test %d ... ' % (self._test)) self.node.destroy_node() self.executor.shutdown() @@ -116,31 +120,32 @@ def tearDown(self): rclpy.shutdown(context=self.context) def run_test(self, test_name, timeout_sec=None, max_cnt=50): + """Run test.""" assert PyTester._file_path is not None - filename = join(PyTester._file_path, test_name + ".test") + filename = join(PyTester._file_path, test_name + '.test') try: - self.node.get_logger().info(f" Loading {test_name} from -->{filename}") - self.node.get_logger().info(f" run_test import rclpy.ok={rclpy.ok(context=self.context)} " - f"context={self.node.context.ok()}") + self.node.get_logger().info(f" Loading '{test_name}' from --> '{filename}'") + self.node.get_logger().info(f' run_test import rclpy.ok={rclpy.ok(context=self.context)} ' + f'context={self.node.context.ok()}') with open(filename, 'r') as f: config = getattr(yaml, 'unsafe_load', yaml.load)(f) except IOError as io_error: - self.node.get_logger().error(f" Exception in {test_name} : {str(io_error)}") + self.node.get_logger().error(f" Exception in '{test_name}' : {str(io_error)}") self.fail(str(io_error)) # force failure except Exception as exc: - self.node.get_logger().error(f" Exception in {test_name} : {type(exc)} - {str(exc)}") + self.node.get_logger().error(f" Exception in '{test_name}' : {type(exc)} - {str(exc)}") self.fail(str(exc)) # force failure - self.node.get_logger().error(f"Running test for {test_name}\n config: {config} ...") + self.node.get_logger().error(f"Running test for '{test_name}'\n config: {config} ...") tester = Tester(self.node, self.executor) try: - self.node.get_logger().info(f" run_pytest rclpy.ok={rclpy.ok(context=self.context)} " - f"context={self.node.context.ok()}") + self.node.get_logger().info(f' run_pytest rclpy.ok={rclpy.ok(context=self.context)} ' + f'context={self.node.context.ok()}') success = tester.run_pytest(test_name, config, timeout_sec=timeout_sec, max_cnt=max_cnt) - self.node.get_logger().error(f" test for {test_name} success={success}") + self.node.get_logger().error(f" test for '{test_name}' success={success}") except Exception as exc: # pylint: disable=W0703 - self.node.get_logger().error(f"Exception in test for {test_name}\n {exc}") + self.node.get_logger().error(f"Exception in test for '{test_name}'\n {exc}") self.fail(str(exc)) # force failure - self.assertEqual(success, 1, msg=f"{test_name} failed !") + self.assertEqual(success, 1, msg=f"Test '{test_name}' failed !") diff --git a/flexbe_testing/flexbe_testing/test/__init__.py b/flexbe_testing/flexbe_testing/test/__init__.py index e69de29..40f454a 100644 --- a/flexbe_testing/flexbe_testing/test/__init__.py +++ b/flexbe_testing/flexbe_testing/test/__init__.py @@ -0,0 +1,29 @@ +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the Philipp Schillinger, Team ViGIR, Christopher Newport University nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""Initialize flexbe_testing.test package.""" diff --git a/flexbe_testing/flexbe_testing/test/import_only_state.py b/flexbe_testing/flexbe_testing/test/import_only_state.py index 5660df4..7b32887 100644 --- a/flexbe_testing/flexbe_testing/test/import_only_state.py +++ b/flexbe_testing/flexbe_testing/test/import_only_state.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -34,6 +34,7 @@ class ImportOnlyState(EventState): + """Test imports for state.""" def __init__(self): """Construct instance.""" diff --git a/flexbe_testing/flexbe_testing/test/selftest_behavior_sm.py b/flexbe_testing/flexbe_testing/test/selftest_behavior_sm.py index 06798be..3e14bbb 100644 --- a/flexbe_testing/flexbe_testing/test/selftest_behavior_sm.py +++ b/flexbe_testing/flexbe_testing/test/selftest_behavior_sm.py @@ -1,7 +1,7 @@ #!/usr/bin/env python # -*- coding: utf-8 -*- -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -32,13 +32,14 @@ """Content is only intended for the flexbe_testing self-test.""" -from flexbe_core import Behavior, Autonomy, OperatableStateMachine, EventState, Logger +from flexbe_core import Autonomy, Behavior, EventState, Logger, OperatableStateMachine class SelftestBehaviorSM(Behavior): """Simple behavior for the flexbe_testing self-test of behaviors.""" def __init__(self, node): + """Initialize the BehaviorSM test.""" super(SelftestBehaviorSM, self).__init__() self.name = 'Selftest Behavior' @@ -61,6 +62,7 @@ def __init__(self, node): # Behavior comments: def create(self): + """Create test SM.""" _state_machine = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['data'], output_keys=['result']) diff --git a/flexbe_testing/flexbe_testing/test/test_add_state.py b/flexbe_testing/flexbe_testing/test/test_add_state.py index a4264ae..6ecf746 100644 --- a/flexbe_testing/flexbe_testing/test/test_add_state.py +++ b/flexbe_testing/flexbe_testing/test/test_add_state.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -33,6 +33,7 @@ class TestAddState(EventState): + """TestAddState.""" def __init__(self, first_arg): """Construct instance.""" @@ -42,5 +43,6 @@ def __init__(self, first_arg): self._first_arg = first_arg def execute(self, userdata): + """Execute the TestAddState.""" userdata.output_value = userdata.second_arg.data + self._first_arg return 'done' diff --git a/flexbe_testing/flexbe_testing/test/test_sub_state.py b/flexbe_testing/flexbe_testing/test/test_sub_state.py index a2903f5..184f74e 100644 --- a/flexbe_testing/flexbe_testing/test/test_sub_state.py +++ b/flexbe_testing/flexbe_testing/test/test_sub_state.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -30,10 +30,11 @@ """Test setup for substate.""" -import rclpy from flexbe_core import EventState from flexbe_core.proxy import ProxySubscriberCached +import rclpy + from std_msgs.msg import String @@ -50,6 +51,7 @@ def __init__(self, topic): self._timeout = TestSubState._node.get_clock().now() + rclpy.duration.Duration(seconds=1.5) def execute(self, userdata): + """Execute TestSubState.""" if self._msg_counter == 0 and TestSubState._node.get_clock().now() > self._timeout: userdata.output_value = None return 'unavailable' diff --git a/flexbe_testing/flexbe_testing/test_context.py b/flexbe_testing/flexbe_testing/test_context.py index 18c0db7..5c493a2 100644 --- a/flexbe_testing/flexbe_testing/test_context.py +++ b/flexbe_testing/flexbe_testing/test_context.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -37,12 +37,13 @@ import os import re -import rclpy import time -from rclpy.exceptions import ParameterNotDeclaredException from ament_index_python.packages import get_package_share_directory +import rclpy +from rclpy.exceptions import ParameterNotDeclaredException + from .logger import Logger @@ -66,9 +67,11 @@ class TestContext: __test__ = False # Do not pytest this class (it is the test!) def __init__(self): + """Initialize.""" pass def __enter__(self): + """Enter test.""" pass def ok(self): @@ -76,19 +79,24 @@ def ok(self): return rclpy.ok() def verify(self): + """Verify test results.""" return True def spin_once(self): + """Spin event loop once.""" pass def __exit__(self, exception_type, exception_value, traceback): + """Exit the test.""" pass def wait_for_finishing(self): + """Wait for test to finish.""" pass @property def success(self): + """Check test success.""" return True @@ -96,6 +104,7 @@ class PyTestContext(TestContext): """Pylint based state tests uses counter and/or timeout_sec to control execute loop.""" def __init__(self, timeout_sec=None, max_cnt=50): + """Initialize the PyTestContext.""" super().__init__() self._cnt = 0 self._max_cnt = None @@ -106,7 +115,7 @@ def __init__(self, timeout_sec=None, max_cnt=50): if timeout_sec is not None: self._time_out = time.time() + float(timeout_sec) - assert self._max_cnt is not None or self._time_out is not None, "Must have either timeout or max cnt set!" + assert self._max_cnt is not None or self._time_out is not None, 'Must have either timeout or max cnt set!' def ok(self): """Return ok status based on time and count for pytests.""" @@ -123,7 +132,8 @@ def ok(self): class LaunchContext(TestContext): """Test context that runs a specified launch file configuration.""" - def __init__(self, node, launch_config, wait_cond="True"): + def __init__(self, node, launch_config, wait_cond='True'): + """Initialize the launch context.""" self._node = node Logger.initialize(node) @@ -170,7 +180,7 @@ def __init__(self, node, launch_config, wait_cond="True"): # self._valid = True def __enter__(self): - raise NotImplementedError("Not implemented for ROS 2 - TODO!") + raise NotImplementedError('Not implemented for ROS 2 - TODO!') self._launchrunner.launch() self._launchrunner.spin_once() Logger.print_positive('launchfile running') @@ -191,14 +201,17 @@ def __enter__(self): Logger.print_negative('unable to check waiting condition:\n\t%s' % str(e)) def verify(self): + """Verify valid.""" return self._valid def spin_once(self): + """Spin test runner loop once.""" self._launchrunner.spin_once() def wait_for_finishing(self): + """Wait for finishing.""" check_exited_rate = self._node.create_rate(10, self._node.get_clock()) - self._node.get_logger().info("Waiting for all launched nodes to exit") + self._node.get_logger().info('Waiting for all launched nodes to exit') while not all(name in self._exit_codes for name in self._launched_proc_names): check_exited_rate.sleep() self._node.destroy_rate(check_exited_rate) @@ -209,4 +222,5 @@ def __exit__(self, exception_type, exception_value, traceback): @property def success(self): + """Verify success.""" return not any(code > 0 for code in self._exit_codes.values()) diff --git a/flexbe_testing/flexbe_testing/test_interface.py b/flexbe_testing/flexbe_testing/test_interface.py index 92cf0dc..d2d4a7c 100644 --- a/flexbe_testing/flexbe_testing/test_interface.py +++ b/flexbe_testing/flexbe_testing/test_interface.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -32,9 +32,11 @@ """Interface to states and behaviors that are subject to testing.""" import inspect -import rclpy from flexbe_core.core import EventState + +import rclpy + from .logger import Logger @@ -44,6 +46,7 @@ class TestInterface: __test__ = False # Do not pytest this class (it is the test!) def __init__(self, node, path, classname): + """Initialize TestInterface instance.""" package = __import__(path, fromlist=[path]) clsmembers = inspect.getmembers(package, lambda member: ( inspect.isclass(member) and member.__module__ == package.__name__ @@ -54,23 +57,26 @@ def __init__(self, node, path, classname): self._class = next(c for name, c in clsmembers if name == classname) self._instance = None self._node = node - self._node.get_logger().info(f"rclpy.ok={rclpy.ok(context=self._node.context)} context={self._node.context.ok()}") + self._node.get_logger().info(f'rclpy.ok={rclpy.ok(context=self._node.context)} context={self._node.context.ok()}') try: self._class.initialize_ros(node) - Logger.print_positive("Given class is a state") + Logger.print_positive('Given class is a state') except Exception: - Logger.print_positive("Given class is a state machine") + Logger.print_positive('Given class is a state machine') Logger.print_positive('%s imported' % self.get_base_name()) def is_state(self): + """Check is state.""" return issubclass(self._class, EventState) def get_base_name(self): - return "state" if self.is_state() else "behavior" + """Get base name.""" + return 'state' if self.is_state() else 'behavior' # instantiate def instantiate(self, params=None): + """Instantiate the test.""" if self.is_state(): self._instance = self._instantiate_state(params=params) else: @@ -78,12 +84,14 @@ def instantiate(self, params=None): Logger.print_positive('%s instantiated' % self.get_base_name()) def _instantiate_state(self, params=None): + """Instantiate the state.""" if params is None: return self._class() else: return self._class(**params) def _instantiate_behavior(self, params=None): + """Instantiate the behavior.""" be = self._class(node=self._node) if params is not None: for name, value in params.items(): @@ -94,6 +102,7 @@ def _instantiate_behavior(self, params=None): # execute def execute(self, userdata, context, spin_cb=None): + """Execute the test.""" spin_cb = spin_cb or (lambda: None) if self.is_state(): diff --git a/flexbe_testing/flexbe_testing/tester.py b/flexbe_testing/flexbe_testing/tester.py index 7162def..f1e0fea 100644 --- a/flexbe_testing/flexbe_testing/tester.py +++ b/flexbe_testing/flexbe_testing/tester.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -35,10 +35,10 @@ from flexbe_core.core.user_data import UserData +from .data_provider import DataProvider from .logger import Logger +from .test_context import PyTestContext, TestContext from .test_interface import TestInterface -from .test_context import TestContext, PyTestContext -from .data_provider import DataProvider class Tester: @@ -47,7 +47,8 @@ class Tester: __test__ = False # Do not pytest this class (it is the test!) def __init__(self, node, executor=None): - self._tests = dict() + """Initialize Test instance.""" + self._tests = {} self.node = node self.executor = executor self._import_only = False @@ -55,8 +56,9 @@ def __init__(self, node, executor=None): Logger.initialize(node) def import_interface(self, name, config): + """Import interface.""" try: - self.node.get_logger().info(f"Importing test configuration for {name}\n config: {config} ...") + self.node.get_logger().info(f"Importing test configuration for '{name}'\n config: {config} ...") self._verify_config(config) except Exception as e: Logger.print_title(name, 'Invalid', None) @@ -84,10 +86,12 @@ def import_interface(self, name, config): return None def run_pytest(self, name, config, timeout_sec=None, max_cnt=50): - self.node.get_logger().info(f" Running pytest setup for {name} with timeout_sec={timeout_sec} max_cnt={max_cnt} ...") + """Run pytest.""" + self.node.get_logger().info(f" Running pytest setup for '{name}' with timeout_sec={timeout_sec} max_cnt={max_cnt} ...") return self.run_test(name, config, context=PyTestContext(timeout_sec=timeout_sec, max_cnt=max_cnt)) def run_test(self, name, config, context=None): + """Run test.""" test_interface = self.import_interface(name, config) if test_interface is None: @@ -95,14 +99,14 @@ def run_test(self, name, config, context=None): if self._import_only: success = True - self.node.get_logger().info(f" Successfully imported for {name} = ...") + self.node.get_logger().info(f" Successfully imported for '{name}' = ...") Logger.print_result(name, success) self._tests['test_%s_pass' % name] = self._test_pass(success) return 1 if success else 0 # load data source try: - self.node.get_logger().info(f" Get data provider for {name} ...") + self.node.get_logger().info(f" Get data provider for '{name}' ...") data = DataProvider(self.node, bagfile=None) except Exception as e: Logger.print_failure('unable to load data source %s:\n\t%s' % @@ -123,8 +127,8 @@ def run_test(self, name, config, context=None): return 0 # instantiate test subject - self.node.get_logger().info(f" Instantiate {name} with params ...") - params = {key: data.parse(value) for key, value in list(config.get('params', dict()).items())} + self.node.get_logger().info(f" Instantiate '{name}' with params ...") + params = {key: data.parse(value) for key, value in list(config.get('params', {}).items())} try: test_interface.instantiate(params) except Exception as e: @@ -134,15 +138,15 @@ def run_test(self, name, config, context=None): return 0 # prepare user data - self.node.get_logger().info(f" Prepare userdata for {name} ...") + self.node.get_logger().info(f" Prepare userdata for '{name}' ...") userdata = UserData() - for input_key, input_value in list(config.get('input', dict()).items()): + for input_key, input_value in list(config.get('input', {}).items()): userdata[input_key] = data.parse(input_value) - expected = {key: data.parse(value) for key, value in config.get('output', dict()).items()} + expected = {key: data.parse(value) for key, value in config.get('output', {}).items()} # run test subject try: - self.node.get_logger().info(f" Execute {name} ...") + self.node.get_logger().info(f" Execute '{name}' ...") if self.executor is not None: def spin_cb(): self.executor.spin_once(timeout_sec=0.01) @@ -152,14 +156,14 @@ def spin_cb(): context, spin_cb=spin_cb) except Exception as exc: - self.node.get_logger().info(f" failed to execute {name} \n {exc}") + self.node.get_logger().info(f" failed to execute '{name}' \n {exc}") Logger.print_failure('failed to execute %s (%s)\n\t%s' % (config['class'], config['path'], str(exc))) self._tests['test_%s_pass' % name] = self._test_pass(False) return 0 if config.get('require_launch_success', False): - self.node.get_logger().info(f" Wait for finishing context for {name} ...") + self.node.get_logger().info(f" Wait for finishing context for '{name}' ...") context.wait_for_finishing() # evaluate outcome @@ -194,7 +198,7 @@ def spin_cb(): # report result success = outcome_ok and output_ok - # self.node.get_logger().info(f" Success for {name} = {success} = {outcome_ok} and {output_ok}...") + # self.node.get_logger().info(f" Success for '{name}' = {success} = {outcome_ok} and {output_ok}...") Logger.print_result(name, success) self._tests['test_%s_pass' % name] = self._test_pass(success) return 1 if success else 0 @@ -210,20 +214,20 @@ def _verify_config(self, config): def _test_output(self, value, expected): def _test_call(test_self): - test_self.assertEqual(value, expected, "Output value %s does not match expected %s" % (value, expected)) + test_self.assertEqual(value, expected, 'Output value %s does not match expected %s' % (value, expected)) return _test_call def _test_outcome(self, outcome, expected): def _test_call(test_self): - test_self.assertEqual(outcome, expected, "Outcome %s does not match expected %s" % (outcome, expected)) + test_self.assertEqual(outcome, expected, 'Outcome %s does not match expected %s' % (outcome, expected)) return _test_call def _test_pass(self, passed): def _test_call(test_self): - test_self.assertTrue(passed, "Did not pass configured tests.") + test_self.assertTrue(passed, 'Did not pass configured tests.') return _test_call def _test_config_invalid(self, config): def _test_call(test_self): - test_self.fail("Test config is invalid: %s" % config) + test_self.fail('Test config is invalid: %s' % config) return _test_call diff --git a/flexbe_testing/launch/flexbe_testing.launch.py b/flexbe_testing/launch/flexbe_testing.launch.py index 4163c98..6f993c3 100644 --- a/flexbe_testing/launch/flexbe_testing.launch.py +++ b/flexbe_testing/launch/flexbe_testing.launch.py @@ -1,4 +1,4 @@ -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -33,46 +33,48 @@ Generally invoked from test launch, not launched directly here. """ from launch import LaunchDescription -from launch_ros.actions import Node from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + def generate_launch_description(): + """Generate the launch description for flexbe testing.""" return LaunchDescription([ DeclareLaunchArgument( - "testcases", - default_value=""), + 'testcases', + default_value=''), DeclareLaunchArgument( - "package", - default_value=""), + 'package', + default_value=''), DeclareLaunchArgument( - "print_debug_positive", - default_value="true"), + 'print_debug_positive', + default_value='true'), DeclareLaunchArgument( - "print_debug_negative", - default_value="true"), + 'print_debug_negative', + default_value='true'), DeclareLaunchArgument( - "mute_info", - default_value="false"), + 'mute_info', + default_value='false'), DeclareLaunchArgument( - "mute_warn", - default_value="false"), + 'mute_warn', + default_value='false'), DeclareLaunchArgument( - "mute_error", - default_value="false"), + 'mute_error', + default_value='false'), DeclareLaunchArgument( - "compact_format", - default_value="false"), + 'compact_format', + default_value='false'), Node( - package="flexbe_testing", executable="testing_node", output="screen", - name="flexbe_testing", - arguments=[LaunchConfiguration("testcases")], - parameters=[{"~package": LaunchConfiguration("package"), - "~print_debug_positive": LaunchConfiguration("print_debug_positive"), - "~print_debug_negative": LaunchConfiguration("print_debug_negative"), - "~mute_info": LaunchConfiguration("mute_info"), - "~mute_warn": LaunchConfiguration("mute_warn"), - "~mute_error": LaunchConfiguration("mute_error"), - "~compact_format": LaunchConfiguration("compact_format")}]) + package='flexbe_testing', executable='testing_node', output='screen', + name='flexbe_testing', + arguments=[LaunchConfiguration('testcases')], + parameters=[{'~package': LaunchConfiguration('package'), + '~print_debug_positive': LaunchConfiguration('print_debug_positive'), + '~print_debug_negative': LaunchConfiguration('print_debug_negative'), + '~mute_info': LaunchConfiguration('mute_info'), + '~mute_warn': LaunchConfiguration('mute_warn'), + '~mute_error': LaunchConfiguration('mute_error'), + '~compact_format': LaunchConfiguration('compact_format')}]) ]) diff --git a/flexbe_testing/launch/selftest_test.launch.py b/flexbe_testing/launch/selftest_test.launch.py index d6ad2b4..3e5299f 100644 --- a/flexbe_testing/launch/selftest_test.launch.py +++ b/flexbe_testing/launch/selftest_test.launch.py @@ -1,4 +1,4 @@ -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -28,31 +28,32 @@ """Launch flexbe_behavior_engine self tests.""" +from ament_index_python.packages import get_package_share_directory + from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.substitutions import LaunchConfiguration from launch.launch_description_sources import PythonLaunchDescriptionSource -from ament_index_python.packages import get_package_share_directory +from launch.substitutions import LaunchConfiguration def generate_launch_description(): """Generate launch description.""" flexbe_testing_dir = get_package_share_directory('flexbe_testing') - path = flexbe_testing_dir + "/tests/res" + path = flexbe_testing_dir + '/tests/res' - testcases = path + "/import_only.test \n" - testcases += path + "/test_add.test \n" - testcases += path + "/sub_unavailable.test \n" - testcases += path + "/behavior.test \n" + testcases = path + '/import_only.test \n' + testcases += path + '/test_add.test \n' + testcases += path + '/sub_unavailable.test \n' + testcases += path + '/behavior.test \n' return LaunchDescription([ - DeclareLaunchArgument("pkg", default_value="flexbe_testing"), - DeclareLaunchArgument("testcases", default_value=testcases), + DeclareLaunchArgument('pkg', default_value='flexbe_testing'), + DeclareLaunchArgument('testcases', default_value=testcases), IncludeLaunchDescription( - PythonLaunchDescriptionSource(flexbe_testing_dir + "/launch/flexbe_testing.launch.py"), + PythonLaunchDescriptionSource(flexbe_testing_dir + '/launch/flexbe_testing.launch.py'), launch_arguments={ - 'package': LaunchConfiguration("pkg"), - 'testcases': LaunchConfiguration("testcases"), + 'package': LaunchConfiguration('pkg'), + 'testcases': LaunchConfiguration('testcases'), }.items() ) ]) diff --git a/flexbe_testing/setup.py b/flexbe_testing/setup.py index 4f623d0..9d1bdef 100644 --- a/flexbe_testing/setup.py +++ b/flexbe_testing/setup.py @@ -1,5 +1,7 @@ +"""Setup for flexbe_testing package.""" import os from glob import glob + from setuptools import setup package_name = 'flexbe_testing' diff --git a/flexbe_testing/tests/flexbe_colcon_test.py b/flexbe_testing/tests/flexbe_colcon_test.py index 783a010..f986d17 100644 --- a/flexbe_testing/tests/flexbe_colcon_test.py +++ b/flexbe_testing/tests/flexbe_colcon_test.py @@ -1,4 +1,4 @@ -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -29,8 +29,8 @@ """Colcon testing for flexbe_testing.""" -from os.path import join import unittest +from os.path import join from flexbe_testing.py_tester import PyTester @@ -44,20 +44,20 @@ def __init__(self, *args, **kwargs): @classmethod def setUpClass(cls): - - PyTester._package = "flexbe_testing" - PyTester._tests_folder = join("tests", "res") + """Set up the test class.""" + PyTester._package = 'flexbe_testing' + PyTester._tests_folder = join('tests', 'res') super().setUpClass() # Do this last after setting package and tests folder # The tests def test_import_only(self): """Invoke unittest defined .test file.""" - return self.run_test("import_only") + return self.run_test('import_only') def test_add(self): """Invoke unittest defined .test file.""" - return self.run_test("test_add") + return self.run_test('test_add') # def test_add_bagfile(self): # """ invoke unittest defined .test file """ @@ -67,11 +67,11 @@ def test_sub_unavailable(self): """Invoke unittest defined .test file.""" # This test requires longer than normal wait for valid return value # given 1.5 second timeout in test_sub_state.py - return self.run_test("sub_unavailable", timeout_sec=2.5, max_cnt=None) + return self.run_test('sub_unavailable', timeout_sec=2.5, max_cnt=None) def test_behavior(self): """Invoke unittest defined .test file.""" - return self.run_test("behavior") + return self.run_test('behavior') if __name__ == '__main__': diff --git a/flexbe_widget/flexbe_widget/__init__.py b/flexbe_widget/flexbe_widget/__init__.py index d63620f..b424755 100644 --- a/flexbe_widget/flexbe_widget/__init__.py +++ b/flexbe_widget/flexbe_widget/__init__.py @@ -1,4 +1,4 @@ -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -26,7 +26,7 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. - +"""Initialize the flexbe_widget package.""" from .behavior_action_server import BehaviorActionServer __all__ = [ diff --git a/flexbe_widget/flexbe_widget/behavior_action_server.py b/flexbe_widget/flexbe_widget/behavior_action_server.py index c110b1a..ae4c79b 100644 --- a/flexbe_widget/flexbe_widget/behavior_action_server.py +++ b/flexbe_widget/flexbe_widget/behavior_action_server.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -33,18 +33,21 @@ import difflib import os -import yaml import zlib +from flexbe_core import BehaviorLibrary +from flexbe_core.core.topics import Topics + +from flexbe_msgs.action import BehaviorExecution +from flexbe_msgs.msg import BEStatus, BehaviorModification, BehaviorSelection + from rclpy.action import ActionServer + from rosidl_runtime_py import get_interface_path -from flexbe_msgs.msg import BehaviorSelection, BehaviorModification, BEStatus -from flexbe_msgs.action import BehaviorExecution -from flexbe_core import BehaviorLibrary -from flexbe_core.core.topics import Topics +from std_msgs.msg import Empty, String -from std_msgs.msg import String, Empty +import yaml class BehaviorActionServer: @@ -72,7 +75,7 @@ def __init__(self, node): self._behavior_lib = BehaviorLibrary(node) - self._node.get_logger().info("%d behaviors available, ready for start request." % self._behavior_lib.count_behaviors()) + self._node.get_logger().info('%d behaviors available, ready for start request.' % self._behavior_lib.count_behaviors()) def _goal_cb(self, goal_handle): self._current_goal = goal_handle @@ -84,7 +87,7 @@ def _goal_cb(self, goal_handle): self._node.get_logger().info('Received a new request to start behavior: %s' % goal.behavior_name) be_key, behavior = self._behavior_lib.find_behavior(goal.behavior_name) if be_key is None: - self._node.get_logger().error("Deny goal: Did not find behavior with requested name %s" % goal.behavior_name) + self._node.get_logger().error('Deny goal: Did not find behavior with requested name %s' % goal.behavior_name) self._current_goal.canceled() return @@ -125,14 +128,14 @@ def _goal_cb(self, goal_handle): # check for local modifications of the behavior to send them to the onboard behavior be_filepath_new = self._behavior_lib.get_sourcecode_filepath(be_key) - with open(be_filepath_new, "r") as f: + with open(be_filepath_new, 'r') as f: be_content_new = f.read() be_filepath_old = self._behavior_lib.get_sourcecode_filepath(be_key, add_tmp=True) if not os.path.isfile(be_filepath_old): be_selection.behavior_id = zlib.adler32(be_content_new.encode()) & 0x7fffffff else: - with open(be_filepath_old, "r") as f: + with open(be_filepath_old, 'r') as f: be_content_old = f.read() sqm = difflib.SequenceMatcher(a=be_content_old, b=be_content_new) @@ -161,7 +164,7 @@ def _preempt_cb(self, goal_handle): self._node.get_logger().info('Behavior execution preempt requested!') def _execute_cb(self, goal_handle): - self._node.get_logger().info("Executing behavior") + self._node.get_logger().info('Executing behavior') def _status_cb(self, msg): if msg.code == BEStatus.ERROR: @@ -182,19 +185,19 @@ def _status_cb(self, msg): return if msg.behavior_id != self._active_behavior_id: - self._node.get_logger().warn("Ignored status because behavior id differed " - f"({msg.behavior_id} vs {self._active_behavior_id})!") + self._node.get_logger().warn('Ignored status because behavior id differed ' + f'({msg.behavior_id} vs {self._active_behavior_id})!') return elif msg.code == BEStatus.FINISHED: result = msg.args[0] if len(msg.args) >= 1 else '' - self._node.get_logger().info('Finished behavior execution with result "%s"!' % result) + self._node.get_logger().info("Finished behavior execution with result '%s'!" % result) self._current_goal.succeed() elif msg.code == BEStatus.FAILED: - self._node.get_logger().error('Behavior execution failed in state %s!' % str(self._current_state)) + self._node.get_logger().error("Behavior execution failed in state '%s'!" % str(self._current_state)) self._current_goal.abort() def _state_cb(self, msg): self._current_state = msg.data if self._current_goal and self._current_goal.is_active: self._current_goal.publish_feedback(BehaviorExecution.Feedback(current_state=self._current_state)) - self._node.get_logger().loginfo('Current state: %s' % self._current_state) + self._node.get_logger().loginfo("Current state: '%s'" % self._current_state) diff --git a/flexbe_widget/flexbe_widget/behavior_launcher.py b/flexbe_widget/flexbe_widget/behavior_launcher.py index 202c692..85a643f 100644 --- a/flexbe_widget/flexbe_widget/behavior_launcher.py +++ b/flexbe_widget/flexbe_widget/behavior_launcher.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -31,34 +31,38 @@ """Node to launch FlexBE behaviors.""" -from datetime import datetime import argparse import difflib import os import sys import threading -import yaml import zlib +from datetime import datetime + +from flexbe_core import BehaviorLibrary, Logger, MIN_UI_VERSION +from flexbe_core.core import StateMap +from flexbe_core.core.topics import Topics + +from flexbe_msgs.msg import BEStatus, BehaviorModification, BehaviorRequest +from flexbe_msgs.msg import BehaviorSelection, BehaviorSync +from flexbe_msgs.msg import ContainerStructure import rclpy from rclpy.node import Node + from rosidl_runtime_py import get_interface_path from std_msgs.msg import Int32, String -from flexbe_msgs.msg import BehaviorModification, BehaviorRequest, BehaviorSelection -from flexbe_msgs.msg import BehaviorSync, BEStatus, ContainerStructure - -from flexbe_core import BehaviorLibrary, Logger, MIN_UI_VERSION -from flexbe_core.core import StateMap -from flexbe_core.core.topics import Topics +import yaml class BehaviorLauncher(Node): """Node to launch FlexBE behaviors.""" def __init__(self): - # Initiate the Node class's constructor and give it a name + """Initialize BehaviorLauncher instance.""" + # Initiate the Node class's constructor and give it a name. super().__init__('flexbe_widget') self._ready_event = threading.Event() @@ -81,7 +85,7 @@ def __init__(self): self._last_onboard_heartbeat = None self._last_heartbeat_msg = None - self.get_logger().info("%d behaviors available, ready for start request." % self._behavior_lib.count_behaviors()) + self.get_logger().info('%d behaviors available, ready for start request.' % self._behavior_lib.count_behaviors()) def heartbeat_timer_callback(self): """ @@ -92,11 +96,11 @@ def heartbeat_timer_callback(self): self._heartbeat_pub.publish(Int32(data=self.get_clock().now().seconds_nanoseconds()[0])) if self._last_onboard_heartbeat is None: - self.get_logger().warn(f"Behavior Launcher has NOT received update from onboard behavior engine!") + self.get_logger().warn('Behavior Launcher has NOT received update from onboard behavior engine!') else: elapsed = self.get_clock().now() - self._last_onboard_heartbeat if elapsed.nanoseconds > 2e9: - self.get_logger().warn(f"Behavior Launcher is NOT receiving updates from onboard behavior engine!") + self.get_logger().warn('Behavior Launcher is NOT receiving updates from onboard behavior engine!') self._last_onboard_heartbeat = None def _onboard_heartbeat_callback(self, msg): @@ -106,15 +110,15 @@ def _onboard_heartbeat_callback(self, msg): def _status_callback(self, msg): if msg.code in [BEStatus.READY, BEStatus.FINISHED, BEStatus.FAILED, BEStatus.ERROR, BEStatus.RUNNING, BEStatus.STARTED]: - self.get_logger().info(f"BE status code={msg.code} received - READY for new behavior!") + self.get_logger().info(f'BE status code={msg.code} received - READY for new behavior!') self._ready_event.set() else: - self.get_logger().info(f"BE status code={msg.code} received ") + self.get_logger().info(f'BE status code={msg.code} received ') def _request_callback(self, msg): """Process request in separate thread to avoid blocking callbacks.""" if not self._ready_event.is_set(): - Logger.logerr("Behavior engine is not ready - cannot process start request!") + Logger.logerr('Behavior engine is not ready - cannot process start request!') else: # self._ready_event.clear() # require a new ready signal after publishing # thread = threading.Thread(target=self._process_request, args=[msg]) @@ -124,10 +128,10 @@ def _request_callback(self, msg): self._process_request(msg) def _process_request(self, msg): - self.get_logger().info(f"Got message from request behavior for {msg.behavior_name}") + self.get_logger().info(f"Got message from request behavior for '{msg.behavior_name}'") be_key, behavior = self._behavior_lib.find_behavior(msg.behavior_name) if be_key is None: - self.get_logger().error("Did not find behavior with requested name: %s" % msg.behavior_name) + self.get_logger().error("Did not find behavior with requested name: '%s'" % msg.behavior_name) self._status_pub.publish(BEStatus(stamp=self.get_clock().now().to_msg(), code=BEStatus.ERROR)) return @@ -157,8 +161,8 @@ def _process_request(self, msg): be_selection.arg_keys.append(k) be_selection.arg_values.append(v) except Exception as exc: - self.get_logger().warn("Failed to parse and substitute behavior arguments, " - f"will use direct input.\n {type(exc)} - {str(exc)}") + self.get_logger().warn('Failed to parse and substitute behavior arguments, ' + f'will use direct input.\n {type(exc)} - {str(exc)}') be_selection.arg_keys = msg.arg_keys be_selection.arg_values = msg.arg_values @@ -166,35 +170,35 @@ def _process_request(self, msg): be_structure = ContainerStructure() be_structure.containers = msg.structure for container in be_structure.containers: - # self.get_logger().info(f"BELauncher: request_callback: adding container {container.path} ...") + # self.get_logger().info(f'BELauncher: request_callback: adding container {container.path} ...') container_map.add_state(container.path, container) # self.get_logger().info(f"BELauncher: request_callback: {msg.structure}") try: be_filepath_new = self._behavior_lib.get_sourcecode_filepath(be_key) except Exception: # pylint: disable=W0703 - self.get_logger().error("Could not find behavior package '%s'" % (behavior["package"])) - self.get_logger().info("Have you built and updated your setup after creating the behavior?") + self.get_logger().error("Could not find behavior package '%s'" % (behavior['package'])) + self.get_logger().info('Have you built and updated your setup after creating the behavior?') self._status_pub.publish(BEStatus(stamp=self.get_clock().now().to_msg(), code=BEStatus.ERROR)) return - with open(be_filepath_new, "r") as f: + with open(be_filepath_new, 'r') as f: be_content_new = f.read() - self.get_logger().info("Check for behavior change ...") + self.get_logger().info('Check for behavior change ...') be_filepath_old = self._behavior_lib.get_sourcecode_filepath(be_key, add_tmp=True) if not os.path.isfile(be_filepath_old): be_selection.behavior_id = zlib.adler32(be_content_new.encode()) & 0x7fffffff if msg.autonomy_level != 255: be_structure.behavior_id = be_selection.behavior_id - # self.get_logger().info(f"BELauncher: request_callback publish structure : {be_structure}") + # self.get_logger().info(f'BELauncher: request_callback publish structure : {be_structure}') self._mirror_pub.publish(be_structure) self._ready_event.clear() # require a new ready signal after publishing self._pub.publish(be_selection) - self.get_logger().info("No changes to behavior version - restart") + self.get_logger().info('No changes to behavior version - restart') return - with open(be_filepath_old, "r") as f: + with open(be_filepath_old, 'r') as f: be_content_old = f.read() sqm = difflib.SequenceMatcher(a=be_content_old, b=be_content_new) @@ -212,8 +216,8 @@ def _process_request(self, msg): self._ready_event.clear() # Force a new ready message before processing self._pub.publish(be_selection) - self.get_logger().info(f"New behavior key={be_selection.behavior_key} published " - f"with checksum id = {be_selection.behavior_id}- start!") + self.get_logger().info(f'New behavior key={be_selection.behavior_key} published ' + f'with checksum id = {be_selection.behavior_id}- start!') def _version_callback(self, msg): vui = BehaviorLauncher._parse_version(msg.data) @@ -248,7 +252,7 @@ def behavior_launcher_main(): stop_index = len(sys.argv) try: # Stop processing after --ros-args - stop_index = next(i for i in range(len(sys.argv)) if sys.argv[i] == "--ros-args") + stop_index = next(i for i in range(len(sys.argv)) if sys.argv[i] == '--ros-args') except StopIteration: pass @@ -262,14 +266,14 @@ def behavior_launcher_main(): print(exc) sys.exit(-1) - behavior = args.behavior if args.behavior else "" + behavior = args.behavior if args.behavior else '' autonomy = args.autonomy auto_start = args.autostart print(f"Behavior launcher with behavior'{behavior}' autonomy={autonomy} auto_start={auto_start}\n" f" behavior args='{behavior_args}'\n node_args='{node_args}'", flush=True) - print("Set up behavior_launcher ROS connections ...", flush=True) + print('Set up behavior_launcher ROS connections ...', flush=True) rclpy.init(args=node_args, signal_handler_options=rclpy.signals.SignalHandlerOptions.NO) # We will handle shutdown @@ -277,7 +281,7 @@ def behavior_launcher_main(): executor = rclpy.executors.SingleThreadedExecutor() executor.add_node(launcher) - if behavior != "": + if behavior != '': Logger.info(f"Set up behavior_launcher to launch '{behavior}' ({auto_start})...") prior_clock = launcher.get_clock().now() while launcher._last_onboard_heartbeat is None: @@ -293,15 +297,15 @@ def behavior_launcher_main(): if launcher._last_heartbeat_msg and auto_start: # After getting heart beat message, # then presume ready if auto_start is set and no behavior is reported active - if (launcher._last_heartbeat_msg.behavior_id == 0 and - len(launcher._last_heartbeat_msg.current_state_checksums) == 0): - Logger.info('No behavior is currently active on startup ' - 'so presume ready and start new behavior!') - launcher._ready_event.set() + if (launcher._last_heartbeat_msg.behavior_id == 0 + and len(launcher._last_heartbeat_msg.current_state_checksums) == 0): + Logger.info('No behavior is currently active on startup ' + 'so presume ready and start new behavior!') + launcher._ready_event.set() else: if (launcher.get_clock().now() - prior_clock).nanoseconds > 2e9: Logger.warning('Cannot launch behavior while prior behavior is active' - f' (id={launcher._last_heartbeat_msg.behavior_id} ...') + f' (id={launcher._last_heartbeat_msg.behavior_id} ...') prior_clock = launcher.get_clock().now() else: # Wait for confirmed ready signal if not autostarting @@ -334,33 +338,33 @@ def initial_request(): try: # Wait for ctrl-c to stop the application if future: - Logger.info("Run initial request of behavior_launcher spinner ...") + Logger.info('Run initial request of behavior_launcher spinner ...') executor.spin_until_future_complete(future, timeout_sec=10.0) if future.done(): - Logger.info("Initial behavior launcher request is sent!") + Logger.info('Initial behavior launcher request is sent!') else: - Logger.error("Initial request failed to send - timed out before complete!") + Logger.error('Initial request failed to send - timed out before complete!') - print("Start behavior_launcher spinner ...", flush=True) + print('Start behavior_launcher spinner ...', flush=True) executor.spin() except KeyboardInterrupt: - print(f"Keyboard interrupt request at {datetime.now()} - ! Shut the behavior launcher down!", flush=True) + print(f'Keyboard interrupt request at {datetime.now()} - ! Shut the behavior launcher down!', flush=True) except Exception as exc: - print(f"Exception in executor at {datetime.now()} - ! {type(exc)}\n {exc}", flush=True) + print(f'Exception in executor at {datetime.now()} - ! {type(exc)}\n {exc}', flush=True) import traceback print(f"{traceback.format_exc().replace('%', '%%')}", flush=True) try: launcher.destroy_node() except Exception as exc: # pylint: disable=W0703 - print(f"Exception from destroy behavior launcher node at {datetime.now()}: {type(exc)}\n{exc}", flush=True) + print(f'Exception from destroy behavior launcher node at {datetime.now()}: {type(exc)}\n{exc}', flush=True) print(f"{traceback.format_exc().replace('%', '%%')}", flush=True) - print(f"Done with behavior launcher at {datetime.now()}!", flush=True) + print(f'Done with behavior launcher at {datetime.now()}!', flush=True) try: rclpy.try_shutdown() except Exception as exc: # pylint: disable=W0703 - print(f"Exception from rclpy.try_shutdown for behavior launcher: {type(exc)}\n{exc}", flush=True) + print(f'Exception from rclpy.try_shutdown for behavior launcher: {type(exc)}\n{exc}', flush=True) print(f"{traceback.format_exc().replace('%', '%%')}", flush=True) diff --git a/flexbe_widget/launch/flexbe_action_server.launch.py b/flexbe_widget/launch/flexbe_action_server.launch.py index 107e630..5a0f2f6 100644 --- a/flexbe_widget/launch/flexbe_action_server.launch.py +++ b/flexbe_widget/launch/flexbe_action_server.launch.py @@ -1,4 +1,4 @@ -# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -26,37 +26,40 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +"""flexbe_action_server_launch.""" +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch_ros.actions import Node from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.substitutions import LaunchConfiguration from launch.launch_description_sources import PythonLaunchDescriptionSource -from ament_index_python.packages import get_package_share_directory +from launch.substitutions import LaunchConfiguration -flexbe_onboard_dir = get_package_share_directory('flexbe_onboard') +from launch_ros.actions import Node def generate_launch_description(): + """Generate launch description.""" + flexbe_onboard_dir = get_package_share_directory('flexbe_onboard') + return LaunchDescription([ DeclareLaunchArgument( - "no_onboard", - default_value="False"), + 'no_onboard', + default_value='False'), DeclareLaunchArgument( - "log_enabled", - default_value="False"), + 'log_enabled', + default_value='False'), DeclareLaunchArgument( - "log_folder", - default_value="~/.flexbe_logs"), + 'log_folder', + default_value='~/.flexbe_logs'), IncludeLaunchDescription( - PythonLaunchDescriptionSource(flexbe_onboard_dir + "/behavior_onboard.launch.py"), + PythonLaunchDescriptionSource(flexbe_onboard_dir + '/behavior_onboard.launch.py'), launch_arguments={ - 'log_enabled': LaunchConfiguration("log_enabled"), - 'log_folder': LaunchConfiguration("log_folder") + 'log_enabled': LaunchConfiguration('log_enabled'), + 'log_folder': LaunchConfiguration('log_folder') }.items() ), - Node(package="flexbe_mirror", executable="behavior_mirror_sm", name="behavior_mirror"), + Node(package='flexbe_mirror', executable='behavior_mirror_sm', name='behavior_mirror'), Node( - package="flexbe_widget", executable="be_action_server", output="screen", - name="behavior_action_server") + package='flexbe_widget', executable='be_action_server', output='screen', + name='behavior_action_server') ]) diff --git a/flexbe_widget/setup.py b/flexbe_widget/setup.py index 808f1ed..25efcf2 100644 --- a/flexbe_widget/setup.py +++ b/flexbe_widget/setup.py @@ -1,7 +1,37 @@ +#!/usr/bin/env python + +# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the Philipp Schillinger, Team ViGIR, Christopher Newport University nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. """Setup file for package.""" import os from glob import glob + from setuptools import setup PACKAGE_NAME = 'flexbe_widget'