From 51841b51daa063c6419ec888aaaca6adb3804a99 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Wed, 19 Feb 2020 20:57:33 +0100 Subject: [PATCH 01/57] [flexbe_core] Rework state logger implementation --- flexbe_core/src/flexbe_core/__init__.py | 1 + .../flexbe_core/core/concurrency_container.py | 6 +- .../src/flexbe_core/core/event_state.py | 7 + .../src/flexbe_core/core/operatable_state.py | 11 +- flexbe_core/src/flexbe_core/state_logger.py | 246 +++++++++++++----- 5 files changed, 199 insertions(+), 72 deletions(-) diff --git a/flexbe_core/src/flexbe_core/__init__.py b/flexbe_core/src/flexbe_core/__init__.py index 692ca51a..a45787d6 100644 --- a/flexbe_core/src/flexbe_core/__init__.py +++ b/flexbe_core/src/flexbe_core/__init__.py @@ -15,6 +15,7 @@ from .behavior_library import BehaviorLibrary from .logger import Logger +from .state_logger import StateLogger class Autonomy: diff --git a/flexbe_core/src/flexbe_core/core/concurrency_container.py b/flexbe_core/src/flexbe_core/core/concurrency_container.py index d922ba0e..706c41e4 100644 --- a/flexbe_core/src/flexbe_core/core/concurrency_container.py +++ b/flexbe_core/src/flexbe_core/core/concurrency_container.py @@ -11,7 +11,7 @@ from flexbe_core.core.priority_container import PriorityContainer -class ConcurrencyContainer(EventState, OperatableStateMachine): +class ConcurrencyContainer(OperatableStateMachine): """ A state machine that can be operated. It synchronizes its current state with the mirror and supports some control mechanisms. @@ -88,7 +88,7 @@ def _update_once(self): if outcome in self.get_registered_outcomes(): # Call termination callbacks self.call_termination_cbs([s.name for s in self._ordered_states],outcome) - self.on_exit(self.userdata, states = [s for s in self._ordered_states if s.name not in list(self._returned_outcomes.keys()) or self._returned_outcomes[s.name] == self._loopback_name]) + self.exit(self.userdata, states = [s for s in self._ordered_states if s.name not in list(self._returned_outcomes.keys()) or self._returned_outcomes[s.name] == self._loopback_name]) self._returned_outcomes = dict() # right now, going out of a cc may break sync # thus, as a quick fix, explicitly sync again @@ -154,6 +154,6 @@ def _disable_ros_control(self): if isinstance(state, OperatableStateMachine): state._disable_ros_control() - def on_exit(self, userdata, states = None): + def exit(self, userdata, states = None): for state in self._ordered_states if states is None else states: self._execute_state(state, force_exit=True) \ No newline at end of file diff --git a/flexbe_core/src/flexbe_core/core/event_state.py b/flexbe_core/src/flexbe_core/core/event_state.py index 5c0c5c8d..2f74d926 100644 --- a/flexbe_core/src/flexbe_core/core/event_state.py +++ b/flexbe_core/src/flexbe_core/core/event_state.py @@ -9,7 +9,14 @@ from flexbe_msgs.msg import CommandFeedback from std_msgs.msg import Bool, Empty +from flexbe_core.state_logger import StateLogger + +@StateLogger.log_events('flexbe.events', + start='on_start', stop='on_stop', + pause='on_pause', resume='on_resume', + enter='on_enter', exit='on_exit') +@StateLogger.log_userdata('flexbe.userdata') class EventState(OperatableState): """ A state that allows implementing certain events. diff --git a/flexbe_core/src/flexbe_core/core/operatable_state.py b/flexbe_core/src/flexbe_core/core/operatable_state.py index 698df34b..a9614c15 100644 --- a/flexbe_core/src/flexbe_core/core/operatable_state.py +++ b/flexbe_core/src/flexbe_core/core/operatable_state.py @@ -11,6 +11,7 @@ from flexbe_core.state_logger import StateLogger +@StateLogger.log_outcomes('flexbe.outcomes') class OperatableState(PreemptableState): """ A state that supports autonomy levels and silent mode. @@ -89,17 +90,21 @@ def _operatable_execute(self, *args, **kwargs): if outcome != self._last_requested_outcome: self._pub.publish(self._request_topic, OutcomeRequest(outcome=self._outcome_list.index(outcome), target=self._parent._get_path() + "/" + self.name)) rospy.loginfo("<-- Want result: %s > %s", self.name, outcome) - StateLogger.log_state_execution(self._get_path(), self.__class__.__name__, outcome, not self._force_transition, False) + StateLogger.log('flexbe.operator', self, type='request', request=outcome, + autonomy=OperatableStateMachine.autonomy_level, + required=self.autonomy[outcome]) self._last_requested_outcome = outcome outcome = OperatableState._loopback_name # autonomy level is high enough, report the executed transition elif outcome != OperatableState._loopback_name: - self._last_requested_outcome = None rospy.loginfo("State result: %s > %s", self.name, outcome) self._pub.publish(self._outcome_topic, UInt8(self._outcome_list.index(outcome))) self._pub.publish(self._debug_topic, String("%s > %s" % (self._get_path(), outcome))) - StateLogger.log_state_execution(self._get_path(), self.__class__.__name__, outcome, not self._force_transition, True) + if self._force_transition: + StateLogger.log('flexbe.operator', self, type='forced', forced=outcome, + requested=self._last_requested_outcome) + self._last_requested_outcome = None self._force_transition = False diff --git a/flexbe_core/src/flexbe_core/state_logger.py b/flexbe_core/src/flexbe_core/state_logger.py index a72b9ff1..f3e62ae0 100644 --- a/flexbe_core/src/flexbe_core/state_logger.py +++ b/flexbe_core/src/flexbe_core/state_logger.py @@ -2,73 +2,187 @@ import rospy import os import time +import yaml +import pickle import logging +import logging.config +from functools import wraps, partial + +from flexbe_core.core.loopback_state import LoopbackState -''' -Created on 02/18/2015 -@author: Philipp Schillinger -''' class StateLogger(object): - ''' - Realizes logging of active states. - ''' - - LOG_FOLDER = "~/.flexbe_logs" - enabled = True - - @staticmethod - def initialize(be_name = None): - if rospy.has_param("~log_folder"): - log_folder = os.path.expanduser(rospy.get_param("~log_folder")) - else: - log_folder = os.path.expanduser(StateLogger.LOG_FOLDER) - - if log_folder == "" or not rospy.get_param("~log_enabled", False): - StateLogger.enabled = False - return - - if not os.path.exists(log_folder): - os.makedirs(log_folder) - - name = "states" - if be_name is not None: - name = be_name.replace(" ", "_").replace(",", "_").replace(".", "_").replace("/", "_").lower() - - filename = os.path.join(log_folder, name + "_" + time.strftime("%Y-%m-%d-%H_%M_%S") + ".log") - - logger = logging.getLogger('state_logger') - handler = logging.FileHandler(filename) - formatter = logging.Formatter('%(message)s') - handler.setFormatter(formatter) - logger.addHandler(handler) - - # log starting time for being able to calculate execution time of first state - logger.info(str(rospy.get_time()) + "," + be_name + ",INIT,INIT,1,1") - - StateLogger._logger = logger - StateLogger._handler = handler - - - @staticmethod - def log_state_execution(statepath, stateclass, outcome, is_autonomous, is_executed): - """ - Logs the execution of a state. - Should be called once when the state returns an outcome. - """ - if not StateLogger.enabled: return - StateLogger._logger.info( - str(rospy.get_time()) + "," # timestamp in ros time - + statepath + "," # path of the executed state - + stateclass + "," # class of the executed state - + outcome + "," # resulting outcome - + ('1' if is_autonomous else '0') + "," # Outcome triggered by behavior or operator? - + ('1' if is_executed else '0') # Outcome actually executed or only requested? - ) - - - @staticmethod - def shutdown(): - if not StateLogger.enabled: return - StateLogger._handler.close() - StateLogger._logger.removeHandler(StateLogger._handler) + ''' + Realizes logging of active states. + ''' + + LOG_FOLDER = "~/.flexbe_logs" + enabled = False + _serialize_impl = 'yaml' + + @staticmethod + def initialize(be_name=None): + log_folder = os.path.expanduser(rospy.get_param("~log_folder", "~/.flexbe_logs")) + + if log_folder == "" or not rospy.get_param("~log_enabled", False): + StateLogger.enabled = False + return + StateLogger.enabled = True + + if not os.path.exists(log_folder): + os.makedirs(log_folder) + + name = "states" + if be_name is not None: + name = be_name.replace(" ", "_").replace(",", "_").replace(".", "_").replace("/", "_").lower() + + StateLogger._serialize_impl = rospy.get_param('~log_serialize', 'yaml') + + logger_config = dict({ + 'version': 1, + 'disable_existing_loggers': False, + 'formatters': {'yaml': {'()': 'flexbe_core.state_logger.StateLoggerFormatter'}}, + 'handlers': {'file': { + 'class': 'logging.FileHandler', + 'filename': '%(log_folder)s/%(behavior)s_%(timestamp)s.yaml', + 'formatter': 'yaml', + 'level': 'DEBUG' + }}, + 'loggers': {'flexbe': {'level': 'INFO', 'handlers': ['file']}} + }, **rospy.get_param('~log_config', {})) + logger_config['handlers']['file']['filename'] %= { + 'log_folder': log_folder, + 'behavior': name, + 'timestamp': time.strftime("%Y-%m-%d-%H_%M_%S") + } + logger_config['loggers']['flexbe']['level'] = rospy.get_param('~/log_level', 'INFO').upper() + logging.config.dictConfig(logger_config) + + @staticmethod + def shutdown(): + if not StateLogger.enabled: + return + logging.shutdown() + StateLogger.enabled = False + + @staticmethod + def get(name): + """ Obtain a reference to the named logger. """ + return logging.getLogger(name) + + @staticmethod + def log(name, state, **kwargs): + """ Log custom data as given by the keyword arguments. """ + if StateLogger.enabled: + StateLogger.get(name).info(dict(StateLogger._basic(state), **kwargs)) + + # state decorators + + @staticmethod + def log_events(name, **events): + """ Log whenever any of the specified events of the state is activated. """ + def decorator(cls): + cls_init = cls.__init__ + @wraps(cls.__init__) + def log_events_init(self, *args, **kwargs): + cls_init(self, *args, **kwargs) + for event, method in events.items(): + def wrap_event_method(event, method): + if hasattr(self, method): + event_method = getattr(self, method) + @wraps(event_method) + def event_wrapper(*args, **kwargs): + time_start = rospy.get_time() + try: + event_method(*args, **kwargs) + finally: + if StateLogger.enabled: + StateLogger.get(name).info(dict( + StateLogger._basic(self), + event=event, + duration=rospy.get_time() - time_start)) + setattr(self, method, event_wrapper) + wrap_event_method(event, method) + cls.__init__ = log_events_init + return cls + return decorator + + @staticmethod + def log_outcomes(name): + """ Log all outcomes of the state. """ + def decorator(cls): + cls_init = cls.__init__ + @wraps(cls.__init__) + def log_outcomes_init(self, *args, **kwargs): + cls_init(self, *args, **kwargs) + execute_method = getattr(self, 'execute') + @wraps(execute_method) + def execute_wrapper(*args, **kwargs): + outcome = None + try: + outcome = execute_method(*args, **kwargs) + return outcome + finally: + if StateLogger.enabled and outcome != LoopbackState._loopback_name: + StateLogger.get(name).info(dict( + StateLogger._basic(self), + outcome=outcome)) + setattr(self, 'execute', execute_wrapper) + cls.__init__ = log_outcomes_init + return cls + return decorator + + @staticmethod + def log_userdata(name): + """ Log all userdata that is passed to the state. """ + def decorator(cls): + cls_init = cls.__init__ + @wraps(cls.__init__) + def log_userdata_init(self, *args, **kwargs): + cls_init(self, *args, **kwargs) + input_keys = kwargs.get('input_keys', []) + on_enter_method = getattr(self, 'on_enter') + @wraps(on_enter_method) + def on_enter_wrapper(userdata): + logger = StateLogger.get(name) + if StateLogger.enabled and logger.isEnabledFor(logging.DEBUG) and input_keys: + logdata = dict(StateLogger._basic(self), userdata=dict()) + for key in input_keys: + try: + logdata['userdata'][key] = StateLogger._serialize(userdata[key]) + except Exception as e: + rospy.logwarn('State %s failed to log userdata for key %s: %s' % + (self.name, key, str(e))) + logger.debug(logdata) + on_enter_method(userdata) + setattr(self, 'on_enter', on_enter_wrapper) + cls.__init__ = log_userdata_init + return cls + return decorator + + # helpers + + @staticmethod + def _serialize(obj): + return { + 'yaml': partial(yaml.dump, default_flow_style=True), + 'str': str, + 'repr': repr, + 'pickle': pickle.dumps, + }.get(StateLogger._serialize_impl, lambda o: eval(StateLogger._serialize_impl, locals={'object': o}))(obj) + + @staticmethod + def _basic(state): + return { + 'time': rospy.get_time(), + 'name': state.name, + 'state': state.__class__.__name__, + 'path': state._get_path(), + } + + +class StateLoggerFormatter(logging.Formatter): + + def format(self, record): + record.msg.update(logger=record.name, loglevel=record.levelname) + return '- %s' % super(StateLoggerFormatter, self).format(record) From d14996399c3e79f7d7d0aeb818a72213426416d3 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Wed, 19 Feb 2020 20:58:24 +0100 Subject: [PATCH 02/57] [flexbe_onboard] Expose new state logger args in onboard launch file --- flexbe_onboard/launch/behavior_onboard.launch | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/flexbe_onboard/launch/behavior_onboard.launch b/flexbe_onboard/launch/behavior_onboard.launch index be0d5ea5..b81fd803 100644 --- a/flexbe_onboard/launch/behavior_onboard.launch +++ b/flexbe_onboard/launch/behavior_onboard.launch @@ -3,10 +3,14 @@ + + + + From fbb3ed9834347f25fe2a50df3ff439b551c9d372 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Sun, 1 Mar 2020 15:35:07 +0100 Subject: [PATCH 03/57] [flexbe_core] Extend configuration options --- .../core/operatable_state_machine.py | 1 + flexbe_core/src/flexbe_core/logger.py | 2 +- flexbe_core/src/flexbe_core/state_logger.py | 74 ++++++++++++------- 3 files changed, 50 insertions(+), 27 deletions(-) diff --git a/flexbe_core/src/flexbe_core/core/operatable_state_machine.py b/flexbe_core/src/flexbe_core/core/operatable_state_machine.py index 4751c071..b73b1de6 100644 --- a/flexbe_core/src/flexbe_core/core/operatable_state_machine.py +++ b/flexbe_core/src/flexbe_core/core/operatable_state_machine.py @@ -220,6 +220,7 @@ def confirm(self, name, id): self._sub.subscribe('flexbe/request_mirror_structure', Int32, self._mirror_structure_callback) StateLogger.initialize(name) + StateLogger.log('flexbe.initialize', None, behavior=name, autonomy=OperatableStateMachine.autonomy_level) if OperatableStateMachine.autonomy_level != 255: self._enable_ros_control() diff --git a/flexbe_core/src/flexbe_core/logger.py b/flexbe_core/src/flexbe_core/logger.py index 046e3cb3..d4939914 100644 --- a/flexbe_core/src/flexbe_core/logger.py +++ b/flexbe_core/src/flexbe_core/logger.py @@ -25,7 +25,7 @@ class Logger(object): @staticmethod def initialize(): - Logger._pub = ProxyPublisher({Logger.LOGGING_TOPIC: BehaviorLog}) + Logger._pub = ProxyPublisher({Logger.LOGGING_TOPIC: BehaviorLog}) @staticmethod def log(text, severity): diff --git a/flexbe_core/src/flexbe_core/state_logger.py b/flexbe_core/src/flexbe_core/state_logger.py index f3e62ae0..022b99a0 100644 --- a/flexbe_core/src/flexbe_core/state_logger.py +++ b/flexbe_core/src/flexbe_core/state_logger.py @@ -7,8 +7,8 @@ import logging import logging.config from functools import wraps, partial - -from flexbe_core.core.loopback_state import LoopbackState +from flexbe_core.proxy import ProxyPublisher +from std_msgs.msg import String class StateLogger(object): @@ -16,7 +16,6 @@ class StateLogger(object): Realizes logging of active states. ''' - LOG_FOLDER = "~/.flexbe_logs" enabled = False _serialize_impl = 'yaml' @@ -41,21 +40,30 @@ def initialize(be_name=None): logger_config = dict({ 'version': 1, 'disable_existing_loggers': False, - 'formatters': {'yaml': {'()': 'flexbe_core.state_logger.StateLoggerFormatter'}}, - 'handlers': {'file': { - 'class': 'logging.FileHandler', - 'filename': '%(log_folder)s/%(behavior)s_%(timestamp)s.yaml', - 'formatter': 'yaml', - 'level': 'DEBUG' - }}, + 'formatters': {'yaml': {'()': 'flexbe_core.state_logger.YamlFormatter'}}, + 'handlers': { + 'file': { + 'class': 'logging.FileHandler', + 'filename': '%(log_folder)s/%(behavior)s_%(timestamp)s.yaml', + 'formatter': 'yaml' + }, + 'publish': { + 'class': 'flexbe_core.state_logger.PublishBehaviorLogMessage', + 'topic': 'flexbe/state_logger', + 'formatter': 'yaml' + } + }, 'loggers': {'flexbe': {'level': 'INFO', 'handlers': ['file']}} }, **rospy.get_param('~log_config', {})) - logger_config['handlers']['file']['filename'] %= { - 'log_folder': log_folder, - 'behavior': name, - 'timestamp': time.strftime("%Y-%m-%d-%H_%M_%S") - } - logger_config['loggers']['flexbe']['level'] = rospy.get_param('~/log_level', 'INFO').upper() + 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") + } + if 'loggers' in logger_config and 'flexbe' in logger_config['loggers']: + logger_config['loggers']['flexbe']['level'] = rospy.get_param('~log_level', 'INFO').upper() logging.config.dictConfig(logger_config) @staticmethod @@ -74,7 +82,7 @@ def get(name): def log(name, state, **kwargs): """ Log custom data as given by the keyword arguments. """ if StateLogger.enabled: - StateLogger.get(name).info(dict(StateLogger._basic(state), **kwargs)) + StateLogger.get(name).log(kwargs.get('loglevel', logging.INFO), dict(StateLogger._basic(state), **kwargs)) # state decorators @@ -123,7 +131,7 @@ def execute_wrapper(*args, **kwargs): outcome = execute_method(*args, **kwargs) return outcome finally: - if StateLogger.enabled and outcome != LoopbackState._loopback_name: + if StateLogger.enabled and outcome != cls._loopback_name: StateLogger.get(name).info(dict( StateLogger._basic(self), outcome=outcome)) @@ -173,16 +181,30 @@ def _serialize(obj): @staticmethod def _basic(state): - return { - 'time': rospy.get_time(), - 'name': state.name, - 'state': state.__class__.__name__, - 'path': state._get_path(), - } + result = {'time': rospy.get_time()} + if state is not None: + result.update({ + 'name': state.name, + 'state': state.__class__.__name__, + 'path': state._get_path() + }) + return result -class StateLoggerFormatter(logging.Formatter): +class YamlFormatter(logging.Formatter): def format(self, record): record.msg.update(logger=record.name, loglevel=record.levelname) - return '- %s' % super(StateLoggerFormatter, self).format(record) + return '- %s' % super(YamlFormatter, self).format(record) + + +class PublishBehaviorLogMessage(logging.Handler): + + def __init__(self, level=logging.NOTSET, topic='flexbe/state_logger'): + super(PublishBehaviorLogMessage, self).__init__(level) + self._topic = topic + self._pub = ProxyPublisher({self._topic: String}) + + def emit(self, record): + message = self.format(record) + self._pub.publish(self._topic, String(message)) From b4b386f58138609cf661474d2d78babe58e15b44 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Sun, 1 Mar 2020 15:41:46 +0100 Subject: [PATCH 04/57] [flexbe_msgs] [flexbe_core] Add debug level to logger (see #101) --- flexbe_core/src/flexbe_core/logger.py | 26 +++++++++++++++++--------- flexbe_msgs/msg/BehaviorLog.msg | 1 + 2 files changed, 18 insertions(+), 9 deletions(-) diff --git a/flexbe_core/src/flexbe_core/logger.py b/flexbe_core/src/flexbe_core/logger.py index 046e3cb3..b820c480 100644 --- a/flexbe_core/src/flexbe_core/logger.py +++ b/flexbe_core/src/flexbe_core/logger.py @@ -18,6 +18,7 @@ class Logger(object): REPORT_WARN = BehaviorLog.WARN REPORT_HINT = BehaviorLog.HINT REPORT_ERROR = BehaviorLog.ERROR + REPORT_DEBUG = BehaviorLog.DEBUG LOGGING_TOPIC = 'flexbe/log' @@ -42,20 +43,27 @@ def log(text, severity): rospy.loginfo('\033[94mBehavior Hint: %s\033[0m', text) elif severity == Logger.REPORT_ERROR: rospy.logerr(text) - + elif severity == Logger.REPORT_DEBUG: + rospy.logdebug(text) + else: + rospy.logdebug(text + ' (unknown log level %s)' % str(severity)) - @staticmethod + @staticmethod + def logdebug(text): + Logger.log(text, Logger.REPORT_DEBUG) + + @staticmethod def loginfo(text): Logger.log(text, Logger.REPORT_INFO) - - @staticmethod + + @staticmethod def logwarn(text): Logger.log(text, Logger.REPORT_WARN) - - @staticmethod + + @staticmethod def loghint(text): Logger.log(text, Logger.REPORT_HINT) - - @staticmethod + + @staticmethod def logerr(text): - Logger.log(text, Logger.REPORT_ERROR) \ No newline at end of file + Logger.log(text, Logger.REPORT_ERROR) diff --git a/flexbe_msgs/msg/BehaviorLog.msg b/flexbe_msgs/msg/BehaviorLog.msg index 3313e7b6..3603b225 100644 --- a/flexbe_msgs/msg/BehaviorLog.msg +++ b/flexbe_msgs/msg/BehaviorLog.msg @@ -4,5 +4,6 @@ uint8 INFO = 0 uint8 WARN = 1 uint8 HINT = 2 uint8 ERROR = 3 +uint8 DEBUG = 10 uint8 status_code \ No newline at end of file From 9be19be0336de1bbe663ac5aa54169c3da92c88a Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Wed, 4 Mar 2020 18:30:07 +0100 Subject: [PATCH 05/57] [flexbe_core] Several minor improvements of proxies (see #114) --- flexbe_core/.gitignore | 8 -- flexbe_core/src/flexbe_core/logger.py | 13 +- flexbe_core/src/flexbe_core/proxy/.gitignore | 1 - flexbe_core/src/flexbe_core/proxy/__init__.py | 2 - .../flexbe_core/proxy/proxy_action_client.py | 113 ++++++++++-------- .../src/flexbe_core/proxy/proxy_publisher.py | 96 +++++++++------ .../flexbe_core/proxy/proxy_service_caller.py | 111 +++++++++-------- .../proxy/proxy_subscriber_cached.py | 105 +++++++--------- .../proxy/proxy_transform_listener.py | 8 +- 9 files changed, 232 insertions(+), 225 deletions(-) delete mode 100644 flexbe_core/src/flexbe_core/proxy/.gitignore diff --git a/flexbe_core/.gitignore b/flexbe_core/.gitignore index 0bf06bea..60c564d7 100755 --- a/flexbe_core/.gitignore +++ b/flexbe_core/.gitignore @@ -1,14 +1,6 @@ -<<<<<<< HEAD -/build/ -/lib/ -/msg_gen/ -/srv_gen/ -======= build devel -bin lib msg_gen srv_gen qtcreator-build ->>>>>>> refs/remotes/origin/master diff --git a/flexbe_core/src/flexbe_core/logger.py b/flexbe_core/src/flexbe_core/logger.py index b820c480..fc9c6b4d 100644 --- a/flexbe_core/src/flexbe_core/logger.py +++ b/flexbe_core/src/flexbe_core/logger.py @@ -1,15 +1,9 @@ #!/usr/bin/env python import rospy -from flexbe_core.proxy import ProxyPublisher from flexbe_msgs.msg import BehaviorLog -''' -Created on 12/17/2014 - -@author: Philipp Schillinger -''' class Logger(object): ''' Realizes behavior-specific logging. @@ -26,14 +20,17 @@ class Logger(object): @staticmethod def initialize(): - Logger._pub = ProxyPublisher({Logger.LOGGING_TOPIC: BehaviorLog}) + Logger._pub = rospy.Publisher(Logger.LOGGING_TOPIC, BehaviorLog, queue_size=100) @staticmethod def log(text, severity): + if Logger._pub is None: + Logger.initialize() + msg = BehaviorLog() msg.text = str(text) msg.status_code = severity - Logger._pub.publish(Logger.LOGGING_TOPIC, msg) + Logger._pub.publish(msg) if severity == Logger.REPORT_INFO: rospy.loginfo(text) diff --git a/flexbe_core/src/flexbe_core/proxy/.gitignore b/flexbe_core/src/flexbe_core/proxy/.gitignore deleted file mode 100644 index 0d20b648..00000000 --- a/flexbe_core/src/flexbe_core/proxy/.gitignore +++ /dev/null @@ -1 +0,0 @@ -*.pyc diff --git a/flexbe_core/src/flexbe_core/proxy/__init__.py b/flexbe_core/src/flexbe_core/proxy/__init__.py index a68737b4..f9f390a4 100644 --- a/flexbe_core/src/flexbe_core/proxy/__init__.py +++ b/flexbe_core/src/flexbe_core/proxy/__init__.py @@ -1,5 +1,3 @@ -import roslib; roslib.load_manifest('flexbe_core') - from .proxy_subscriber_cached import ProxySubscriberCached from .proxy_publisher import ProxyPublisher from .proxy_service_caller import ProxyServiceCaller diff --git a/flexbe_core/src/flexbe_core/proxy/proxy_action_client.py b/flexbe_core/src/flexbe_core/proxy/proxy_action_client.py index a5eec414..93d408d8 100644 --- a/flexbe_core/src/flexbe_core/proxy/proxy_action_client.py +++ b/flexbe_core/src/flexbe_core/proxy/proxy_action_client.py @@ -1,10 +1,7 @@ #!/usr/bin/env python - -import roslib; roslib.load_manifest('flexbe_core') import rospy import actionlib from threading import Timer -import time from flexbe_core.logger import Logger @@ -21,7 +18,7 @@ class ProxyActionClient(object): def __init__(self, topics={}, wait_duration=10): """ Initializes the proxy with optionally a given set of clients. - + @type topics: dictionary string - message class @param topics: A dictionay containing a collection of topic - message type pairs. @@ -29,63 +26,46 @@ def __init__(self, topics={}, wait_duration=10): @param wait_duration: Defines how long to wait for each client in the given set to become available (if it is not already available). """ - for topic, msg_type in topics.items(): self.setupClient(topic, msg_type, wait_duration) - def setupClient(self, topic, msg_type, wait_duration=10): """ Tries to set up an action client for calling it later. - + @type topic: string @param topic: The topic of the action to call. - + @type msg_type: msg type @param msg_type: The type of messages of this action client. - + @type wait_duration: int @param wait_duration: Defines how long to wait for the given client if it is not available right now. """ if topic not in ProxyActionClient._clients: - client = actionlib.SimpleActionClient(topic, msg_type) - t = Timer(1, self._print_wait_warning, [topic]) - t.start() - available = client.wait_for_server(rospy.Duration.from_sec(wait_duration)) - warning_sent = False - try: - t.cancel() - except Exception as ve: - # already printed the warning - warning_sent = True - - if not available: - Logger.logerr("Action client %s timed out!" % topic) - else: - ProxyActionClient._clients[topic] = client - if warning_sent: - Logger.loginfo("Finally found action client %s..." % (topic)) - - + ProxyActionClient._clients[topic] = actionlib.SimpleActionClient(topic, msg_type) + self._check_topic_available(topic, wait_duration) + def send_goal(self, topic, goal): """ Performs an action call on the given topic. - + @type topic: string @param topic: The topic to call. - + @type goal: action goal @param goal: The request to send to the action server. """ - if topic not in ProxyActionClient._clients: - raise ValueError('ProxyActionClient: topic %s not yet registered!' % topic) - + if not self._check_topic_available(topic): + raise ValueError('Cannot send goal for action client %s: Topic not available.' % topic) + # reset previous results ProxyActionClient._result[topic] = None ProxyActionClient._feedback[topic] = None - - ProxyActionClient._clients[topic].send_goal(goal, - done_cb = lambda ts, r: self._done_callback(topic, ts, r), - feedback_cb = lambda f: self._feedback_callback(topic, f) + # send goal + ProxyActionClient._clients[topic].send_goal( + goal, + done_cb=lambda ts, r: self._done_callback(topic, ts, r), + feedback_cb=lambda f: self._feedback_callback(topic, f) ) def _done_callback(self, topic, terminal_state, result): @@ -94,33 +74,32 @@ def _done_callback(self, topic, terminal_state, result): def _feedback_callback(self, topic, feedback): ProxyActionClient._feedback[topic] = feedback - def is_available(self, topic): """ Checks if the client on the given action topic is available. - + @type topic: string @param topic: The topic of interest. """ - return topic in ProxyActionClient._clients + return self._check_topic_available(topic) def has_result(self, topic): """ Checks if the given action call already has a result. - + @type topic: string @param topic: The topic of interest. """ - return ProxyActionClient._result[topic] is not None + return ProxyActionClient._result.get(topic) is not None def get_result(self, topic): """ Returns the result message of the given action call. - + @type topic: string @param topic: The topic of interest. """ - return ProxyActionClient._result[topic] + return ProxyActionClient._result.get(topic) def remove_result(self, topic): """ @@ -134,20 +113,20 @@ def remove_result(self, topic): def has_feedback(self, topic): """ Checks if the given action call has any feedback. - + @type topic: string @param topic: The topic of interest. """ - return ProxyActionClient._feedback[topic] is not None + return ProxyActionClient._feedback.get(topic) is not None def get_feedback(self, topic): """ Returns the latest feedback message of the given action call. - + @type topic: string @param topic: The topic of interest. """ - return ProxyActionClient._feedback[topic] + return ProxyActionClient._feedback.get(topic) def remove_feedback(self, topic): """ @@ -162,7 +141,7 @@ def get_state(self, topic): """ Determines the current actionlib state of the given action topic. A list of possible states is defined in actionlib_msgs/GoalStatus. - + @type topic: string @param topic: The topic of interest. """ @@ -171,7 +150,7 @@ def get_state(self, topic): def is_active(self, topic): """ Determines if an action request is already being processed on the given topic. - + @type topic: string @param topic: The topic of interest. """ @@ -180,11 +159,43 @@ def is_active(self, topic): def cancel(self, topic): """ Cancels the current action call on the given action topic. - + @type topic: string @param topic: The topic of interest. """ ProxyActionClient._clients[topic].cancel_goal() + def _check_topic_available(self, topic, wait_duration=1): + """ + Checks whether a topic is available. + + @type topic: string + @param topic: The topic of the action. + + @type wait_duration: int + @param wait_duration: Defines how long to wait for the given client if it is not available right now. + """ + client = ProxyActionClient._clients.get(topic) + if client is None: + Logger.logerr("Action client %s not yet registered, need to add it first!" % topic) + return False + t = Timer(.5, self._print_wait_warning, [topic]) + t.start() + available = client.wait_for_server(rospy.Duration.from_sec(wait_duration)) + warning_sent = False + try: + t.cancel() + except Exception: + # already printed the warning + warning_sent = True + + if not available: + Logger.logerr("Action client %s timed out!" % topic) + return False + else: + if warning_sent: + Logger.loginfo("Finally found action client %s..." % (topic)) + return True + def _print_wait_warning(self, topic): Logger.logwarn("Waiting for action client %s..." % (topic)) diff --git a/flexbe_core/src/flexbe_core/proxy/proxy_publisher.py b/flexbe_core/src/flexbe_core/proxy/proxy_publisher.py index a0ddd793..9503d795 100644 --- a/flexbe_core/src/flexbe_core/proxy/proxy_publisher.py +++ b/flexbe_core/src/flexbe_core/proxy/proxy_publisher.py @@ -1,100 +1,122 @@ #!/usr/bin/env python - -import roslib; roslib.load_manifest('flexbe_core') import rospy -from flexbe_msgs.msg import BEStatus +from threading import Timer -import time -import random +from flexbe_core.logger import Logger class ProxyPublisher(object): """ A proxy for publishing topics. """ - _simulate_delay = False - _topics = {} - - def __init__(self, topics = {}, _latch=False): + + def __init__(self, topics={}, _latch=False, _queue_size=100): """ Initializes the proxy with optionally a given set of topics. Automatically creates a publisher for sending status messages. - + @type topics: dictionary string - message class @param topics: A dictionay containing a collection of topic - message type pairs. - + @type _latch: bool @param: _latch: Defines if messages on the given topics should be latched. + + @type _queue_size: int + @param: _queue_size: Defines the queue size of the new publishers. """ for topic, msg_type in topics.items(): - self.createPublisher(topic, msg_type, _latch) - - - def createPublisher(self, topic, msg_type, _latch = False): + self.createPublisher(topic, msg_type, _latch, _queue_size) + + def createPublisher(self, topic, msg_type, _latch=False, _queue_size=100): """ Adds a new publisher to the proxy. - + @type topic: string @param topic: The topic to publish on. - + @type msg_type: a message class @param msg_type: The type of messages of this topic. - + @type _latch: bool @param: _latch: Defines if messages on the given topics should be latched. + + @type _queue_size: int + @param: _queue_size: Defines the queue size of the publisher. """ if topic not in ProxyPublisher._topics: - pub = rospy.Publisher(topic, msg_type, latch = _latch, queue_size=100) - ProxyPublisher._topics[topic] = pub - + ProxyPublisher._topics[topic] = rospy.Publisher(topic, msg_type, latch=_latch, queue_size=_queue_size) def is_available(self, topic): """ Checks if the publisher on the given topic is available. - + @type topic: string @param topic: The topic of interest. """ return topic in ProxyPublisher._topics - def publish(self, topic, msg): """ Publishes a message on the specified topic. - + @type topic: string @param topic: The topic to publish on. - + @type msg: message class (defined when created publisher) @param msg: The message to publish. """ - if ProxyPublisher._simulate_delay: - time.sleep(max(0, random.gauss(1.5, 1))) # for simulating comms_bridge delay - if topic not in ProxyPublisher._topics: - rospy.logwarn('ProxyPublisher: topic '+ topic +' not yet registered!') + Logger.logwarn('ProxyPublisher: topic %s not yet registered!' % topic) return try: ProxyPublisher._topics[topic].publish(msg) except Exception as e: - rospy.logwarn('Something went wrong when publishing to %s!\n%s', topic, str(e)) - - + Logger.logwarn('Something went wrong when publishing to %s!\n%s' % (topic, str(e))) + def wait_for_any(self, topic, timeout=5.0): """ Blocks until there are any subscribers to the given topic. - + @type topic: string @param topic: The topic to publish on. - + @type timeout: float @param timeout: How many seconds should be the maximum blocked time. """ - pub = ProxyPublisher._topics[topic] + pub = ProxyPublisher._topics.get(topic) + if pub is None: + Logger.logerr("Publisher %s not yet registered, need to add it first!" % topic) + return False + t = Timer(.5, self._print_wait_warning, [topic]) + t.start() + available = self._wait_for_subscribers(pub, timeout) + warning_sent = False + try: + t.cancel() + except Exception: + # already printed the warning + warning_sent = True + + if not available: + Logger.logerr("Waiting for subscribers on %s timed out!" % topic) + return False + else: + if warning_sent: + Logger.loginfo("Finally found subscriber on %s..." % (topic)) + return True + + def _print_wait_warning(self, topic): + Logger.logwarn("Waiting for subscribers on %s..." % (topic)) + + def _wait_for_subscribers(self, pub, timeout=5.0): starting_time = rospy.get_rostime() - while pub.get_num_connections() < 1: + rate = rospy.Rate(100) + while not rospy.is_shutdown(): elapsed = rospy.get_rostime() - starting_time if elapsed.to_sec() >= timeout: return False - return True + if pub.get_num_connections() >= 1: + return True + rate.sleep() + return False diff --git a/flexbe_core/src/flexbe_core/proxy/proxy_service_caller.py b/flexbe_core/src/flexbe_core/proxy/proxy_service_caller.py index 272dfde7..29c2e8cb 100644 --- a/flexbe_core/src/flexbe_core/proxy/proxy_service_caller.py +++ b/flexbe_core/src/flexbe_core/proxy/proxy_service_caller.py @@ -1,9 +1,6 @@ #!/usr/bin/env python - -import roslib; roslib.load_manifest('flexbe_core') import rospy from threading import Timer -import time from flexbe_core.logger import Logger @@ -14,89 +11,103 @@ class ProxyServiceCaller(object): """ _services = {} - def __init__(self, topics = {}): + def __init__(self, topics={}, persistent=False, wait_duration=10): """ Initializes the proxy with optionally a given set of clients. - + @type topics: dictionary string - message class @param topics: A dictionay containing a collection of topic - message type pairs. - """ + @type persistent: bool + @param persistent: Defines if the service callers are persistent. + + @type wait_duration: int + @param wait_duration: Defines how long to wait for the given services if not available right now. + """ for topic, msg_type in topics.items(): - self.setupService(topic, msg_type) - + self.setupService(topic, msg_type, persistent, wait_duration) def setupService(self, topic, msg_type, persistent=False, wait_duration=10): """ Tries to set up a service caller for calling it later. - + @type topic: string @param topic: The topic of the service to call. - + @type msg_type: service class @param msg_type: The type of messages of this service. - + @type persistent: bool @param persistent: Defines if this service caller is persistent. - + @type wait_duration: int @param wait_duration: Defines how long to wait for the given service if it is not available right now. """ if topic not in ProxyServiceCaller._services: - warning_sent = False - available = False - try: - t = Timer(1, self._print_wait_warning, [topic]) - t.start() - rospy.wait_for_service(topic, wait_duration) - available = True - except rospy.exceptions.ROSException as e: - available = False - - try: - t.cancel() - except Exception as ve: - # already printed the warning - warning_sent = True - - if not available: - Logger.logerr("Service client %s timed out!" % topic) - else: - ProxyServiceCaller._services[topic] = rospy.ServiceProxy(topic, msg_type, persistent) - if warning_sent: - Logger.loginfo("Finally found action client %s..." % (topic)) - + ProxyServiceCaller._services[topic] = rospy.ServiceProxy(topic, msg_type, persistent) + self._check_service_available(topic, wait_duration) def is_available(self, topic): """ Checks if the service on the given topic is available. - + @type topic: string @param topic: The topic of interest. """ - return topic in ProxyServiceCaller._services - - + return self._check_service_available(topic) + def call(self, topic, request): """ Performs a service call on the given topic. - + @type topic: string @param topic: The topic to call. - + @type request: service @param request: The request to send to this service. """ - if topic not in ProxyServiceCaller._services: - rospy.logwarn('ProxyServiceCaller: topic not yet registered!') - return - + if not self._check_service_available(topic): + raise ValueError('Cannot call service client %s: Topic not available.' % topic) + # call service (forward any exceptions) + return ProxyServiceCaller._services[topic].call(request) + + def _check_service_available(self, topic, wait_duration=1): + """ + Checks whether a service is available. + + @type topic: string + @param topic: The topic of the service. + + @type wait_duration: int + @param wait_duration: Defines how long to wait for the given service if it is not available right now. + """ + client = ProxyServiceCaller._services.get(topic) + if client is None: + Logger.logerr("Service client %s not yet registered, need to add it first!" % topic) + return False + warning_sent = False + available = False + try: + t = Timer(1, self._print_wait_warning, [topic]) + t.start() + rospy.wait_for_service(topic, wait_duration) + available = True + except rospy.exceptions.ROSException: + available = False + try: - return ProxyServiceCaller._services[topic].call(request) - except Exception as e: - print('error: ' + str(e)) - raise + t.cancel() + except Exception: + # already printed the warning + warning_sent = True + if not available: + Logger.logerr("Service client %s timed out!" % topic) + return False + else: + if warning_sent: + Logger.loginfo("Finally found service %s..." % (topic)) + return True def _print_wait_warning(self, topic): - Logger.logwarn("Waiting for service client %s..." % (topic)) + Logger.logwarn("Waiting for service %s..." % (topic)) diff --git a/flexbe_core/src/flexbe_core/proxy/proxy_subscriber_cached.py b/flexbe_core/src/flexbe_core/proxy/proxy_subscriber_cached.py index b4e06abb..e4ec9d50 100644 --- a/flexbe_core/src/flexbe_core/proxy/proxy_subscriber_cached.py +++ b/flexbe_core/src/flexbe_core/proxy/proxy_subscriber_cached.py @@ -1,92 +1,79 @@ #!/usr/bin/env python - -import roslib; roslib.load_manifest('flexbe_core') import rospy -import time -import random +from flexbe_core.logger import Logger class ProxySubscriberCached(object): """ A proxy for subscribing topics that caches and buffers received messages. """ - _simulate_delay = False - _topics = {} _persistant_topics = [] - + def __init__(self, topics={}): """ Initializes the proxy with optionally a given set of topics. - + @type topics: dictionary string - message_class @param topics: A dictionary containing a collection of topic - message type pairs. """ for topic, msg_type in topics.items(): self.subscribe(topic, msg_type) - - + def subscribe(self, topic, msg_type, callback=None, buffered=False): """ Adds a new subscriber to the proxy. - + @type topic: string @param topic: The topic to subscribe. - + @type msg_type: a message class @param msg_type: The type of messages of this topic. - + @type callback: function @param callback: A function to be called when receiving messages. - + @type buffered: boolean @param buffered: True if all messages should be bufferd, False if only the last message should be cached. """ - if not topic in ProxySubscriberCached._topics: + if topic not in ProxySubscriberCached._topics: sub = rospy.Subscriber(topic, msg_type, self._callback, callback_args=topic) - ProxySubscriberCached._topics[topic] = {'subscriber': sub, - 'last_msg': None, - 'buffered': buffered, - 'msg_queue':[]} - + ProxySubscriberCached._topics[topic] = {'subscriber': sub, + 'last_msg': None, + 'buffered': buffered, + 'msg_queue': []} if callback is not None: ProxySubscriberCached._topics[topic]['subscriber'].impl.add_callback(callback, None) - - + def _callback(self, msg, topic): """ Standard callback that is executed when a message is received. - + @type topic: message @param topic: The latest message received on this topic. - + @type topic: string @param topic: The topic to which this callback belongs. """ - if ProxySubscriberCached._simulate_delay: - time.sleep(max(0, random.gauss(2, 0.6))) # for simulating comms_bridge delay - if topic not in ProxySubscriberCached._topics: return ProxySubscriberCached._topics[topic]['last_msg'] = msg if ProxySubscriberCached._topics[topic]['buffered']: ProxySubscriberCached._topics[topic]['msg_queue'].append(msg) - - + def set_callback(self, topic, callback): - """ + """ Adds the given callback to the topic subscriber. - + @type topic: string @param topic: The topic to add the callback to. - + @type callback: function @param callback: The callback to be added. """ ProxySubscriberCached._topics[topic]['subscriber'].impl.add_callback(callback, None) - - + def enable_buffer(self, topic): """ Enables the buffer on the given topic. @@ -95,8 +82,7 @@ def enable_buffer(self, topic): @param topic: The topic of interest. """ ProxySubscriberCached._topics[topic]['buffered'] = True - - + def disable_buffer(self, topic): """ Disables the buffer on the given topic. @@ -106,18 +92,16 @@ def disable_buffer(self, topic): """ ProxySubscriberCached._topics[topic]['buffered'] = False ProxySubscriberCached._topics[topic]['msg_queue'] = [] - def is_available(self, topic): """ Checks if the subscriber on the given topic is available. - + @type topic: string @param topic: The topic of interest. """ return topic in ProxySubscriberCached._topics - - + def get_last_msg(self, topic): """ Returns the latest cached message of the given topic. @@ -126,8 +110,7 @@ def get_last_msg(self, topic): @param topic: The topic of interest. """ return ProxySubscriberCached._topics[topic]['last_msg'] - - + def get_from_buffer(self, topic): """ Pops the oldest buffered message of the given topic. @@ -136,13 +119,14 @@ def get_from_buffer(self, topic): @param topic: The topic of interest. """ if not ProxySubscriberCached._topics[topic]['buffered']: - rospy.logwarn('Attempted to access buffer of non-buffered topic!') + Logger.logwarn('Attempted to access buffer of non-buffered topic!') + return None + if len(ProxySubscriberCached._topics[topic]['msg_queue']) == 0: return None msg = ProxySubscriberCached._topics[topic]['msg_queue'][0] ProxySubscriberCached._topics[topic]['msg_queue'] = ProxySubscriberCached._topics[topic]['msg_queue'][1:] return msg - - + def has_msg(self, topic): """ Determines if the given topic has a message in its cache. @@ -151,8 +135,7 @@ def has_msg(self, topic): @param topic: The topic of interest. """ return ProxySubscriberCached._topics[topic]['last_msg'] is not None - - + def has_buffered(self, topic): """ Determines if the given topic has any messages in its buffer. @@ -161,9 +144,8 @@ def has_buffered(self, topic): @param topic: The topic of interest. """ return len(ProxySubscriberCached._topics[topic]['msg_queue']) > 0 - - - def remove_last_msg(self, topic, clear_buffer=False): # better pop last msg + + def remove_last_msg(self, topic, clear_buffer=False): """ Removes the cached message of the given topic and optionally clears its buffer. @@ -173,12 +155,12 @@ def remove_last_msg(self, topic, clear_buffer=False): # better pop last msg @type topic: boolean @param topic: Set to true if the buffer of the given topic should be cleared as well. """ - if ProxySubscriberCached._persistant_topics.count(topic) > 0: return + if topic in ProxySubscriberCached._persistant_topics: + return ProxySubscriberCached._topics[topic]['last_msg'] = None if clear_buffer: ProxySubscriberCached._topics[topic]['msg_queue'] = [] - - + def make_persistant(self, topic): """ Makes the given topic persistant which means messages can no longer be removed @@ -187,9 +169,9 @@ def make_persistant(self, topic): @type topic: string @param topic: The topic of interest. """ - ProxySubscriberCached._persistant_topics.append(topic) - - + if topic not in ProxySubscriberCached._persistant_topics: + ProxySubscriberCached._persistant_topics.append(topic) + def has_topic(self, topic): """ Determines if the given topic is already subscribed. @@ -197,11 +179,9 @@ def has_topic(self, topic): @type topic: string @param topic: The topic of interest. """ - if topic in ProxySubscriberCached._topics: - return True - return False - - + Logger.logwarn('Deprecated (ProxySubscriberCached): use "is_available(topic)" instead of "has_topic(topic)".') + return self.is_available(topic) + def unsubscribe_topic(self, topic): """ Removes the given topic from the list of subscribed topics. @@ -212,8 +192,7 @@ def unsubscribe_topic(self, topic): if topic in ProxySubscriberCached._topics: ProxySubscriberCached._topics[topic]['subscriber'].unregister() ProxySubscriberCached._topics.pop(topic) - - + def shutdown(self): """ Shuts this proxy down by unregistering all subscribers. """ try: diff --git a/flexbe_core/src/flexbe_core/proxy/proxy_transform_listener.py b/flexbe_core/src/flexbe_core/proxy/proxy_transform_listener.py index f7a6aba5..88ab2fb0 100644 --- a/flexbe_core/src/flexbe_core/proxy/proxy_transform_listener.py +++ b/flexbe_core/src/flexbe_core/proxy/proxy_transform_listener.py @@ -1,15 +1,13 @@ #!/usr/bin/env python - -import roslib; roslib.load_manifest('flexbe_core') import tf class ProxyTransformListener(object): _listener = None - + def __init__(self): if ProxyTransformListener._listener is None: - ProxyTransformListener._listener = tf.TransformListener() + ProxyTransformListener._listener = tf.TransformListener() def listener(self): - return ProxyTransformListener._listener \ No newline at end of file + return ProxyTransformListener._listener From a91d607cdf7bd5e626e7dc76facd7ac6ce431c5e Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Wed, 4 Mar 2020 19:40:34 +0100 Subject: [PATCH 06/57] [flexbe_core] Add tests for proxies --- flexbe_core/CMakeLists.txt | 5 ++ flexbe_core/package.xml | 2 + flexbe_core/test/flexbe_proxies.test | 5 ++ flexbe_core/test/test_proxies.py | 120 +++++++++++++++++++++++++++ 4 files changed, 132 insertions(+) create mode 100644 flexbe_core/test/flexbe_proxies.test create mode 100755 flexbe_core/test/test_proxies.py diff --git a/flexbe_core/CMakeLists.txt b/flexbe_core/CMakeLists.txt index 1a3b3ff8..6d33d524 100644 --- a/flexbe_core/CMakeLists.txt +++ b/flexbe_core/CMakeLists.txt @@ -25,3 +25,8 @@ catkin_package( #install(PROGRAMS bin/hello # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +# run tests +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest(test/flexbe_proxies.test) +endif() diff --git a/flexbe_core/package.xml b/flexbe_core/package.xml index b6e3b033..56583832 100644 --- a/flexbe_core/package.xml +++ b/flexbe_core/package.xml @@ -12,6 +12,8 @@ catkin + rostest + smach_ros rospy tf diff --git a/flexbe_core/test/flexbe_proxies.test b/flexbe_core/test/flexbe_proxies.test new file mode 100644 index 00000000..0150dae4 --- /dev/null +++ b/flexbe_core/test/flexbe_proxies.test @@ -0,0 +1,5 @@ + + + + + diff --git a/flexbe_core/test/test_proxies.py b/flexbe_core/test/test_proxies.py new file mode 100755 index 00000000..5f3d5e08 --- /dev/null +++ b/flexbe_core/test/test_proxies.py @@ -0,0 +1,120 @@ +#!/usr/bin/env python +import unittest +import rospy +import actionlib +from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached, ProxyActionClient, ProxyServiceCaller + +from std_msgs.msg import String +from std_srvs.srv import Trigger, TriggerRequest +from flexbe_msgs.msg import BehaviorExecutionAction, BehaviorExecutionGoal, BehaviorExecutionResult + + +class TestProxies(unittest.TestCase): + + def test_publish_subscribe(self): + t1 = '/pubsub_1' + t2 = '/pubsub_2' + pub = ProxyPublisher({t1: String}) + pub = ProxyPublisher({t2: String}, _latch=True) + sub = ProxySubscriberCached({t1: String}) + self.assertTrue(pub.is_available(t1)) + + self.assertTrue(pub.wait_for_any(t1)) + self.assertFalse(pub.wait_for_any(t2)) + pub.publish(t1, String('1')) + pub.publish(t2, String('2')) + + rospy.sleep(.5) # make sure latched message is sent before subscriber is added + sub = ProxySubscriberCached({t2: String}) + rospy.sleep(.5) # make sure latched message can be received before checking + + self.assertTrue(sub.has_msg(t1)) + self.assertEqual(sub.get_last_msg(t1).data, '1') + sub.remove_last_msg(t1) + self.assertFalse(sub.has_msg(t1)) + self.assertIsNone(sub.get_last_msg(t1)) + + self.assertTrue(sub.has_msg(t2)) + self.assertEqual(sub.get_last_msg(t2).data, '2') + + def test_subscribe_buffer(self): + t1 = '/buffered_1' + pub = ProxyPublisher({t1: String}) + sub = ProxySubscriberCached({t1: String}) + sub.enable_buffer(t1) + self.assertTrue(pub.wait_for_any(t1)) + + pub.publish(t1, String('1')) + pub.publish(t1, String('2')) + rospy.sleep(.5) # make sure messages can be received + + self.assertTrue(sub.has_msg(t1)) + self.assertTrue(sub.has_buffered(t1)) + self.assertEqual(sub.get_from_buffer(t1).data, '1') + + pub.publish(t1, String('3')) + rospy.sleep(.5) # make sure messages can be received + + self.assertEqual(sub.get_from_buffer(t1).data, '2') + self.assertEqual(sub.get_from_buffer(t1).data, '3') + self.assertIsNone(sub.get_from_buffer(t1)) + self.assertFalse(sub.has_buffered(t1)) + + def test_service_caller(self): + t1 = '/service_1' + rospy.Service(t1, Trigger, lambda r: (True, 'ok')) + + srv = ProxyServiceCaller({t1: Trigger}) + + result = srv.call(t1, TriggerRequest()) + self.assertIsNotNone(result) + self.assertTrue(result.success) + self.assertEqual(result.message, 'ok') + + self.assertFalse(srv.is_available('/not_there')) + srv = ProxyServiceCaller({'/invalid': Trigger}, wait_duration=.1) + self.assertFalse(srv.is_available('/invalid')) + + def test_action_client(self): + t1 = '/action_1' + server = None + + def execute_cb(goal): + rospy.sleep(.5) + if server.is_preempt_requested(): + server.set_preempted() + else: + server.set_succeeded(BehaviorExecutionResult(outcome='ok')) + + server = actionlib.SimpleActionServer(t1, BehaviorExecutionAction, execute_cb, auto_start=False) + server.start() + + client = ProxyActionClient({t1: BehaviorExecutionAction}) + self.assertFalse(client.has_result(t1)) + client.send_goal(t1, BehaviorExecutionGoal()) + + rate = rospy.Rate(20) + for i in range(20): + self.assertTrue(client.is_active(t1) or client.has_result(t1)) + rate.sleep() + self.assertTrue(client.has_result(t1)) + + result = client.get_result(t1) + self.assertEqual(result.outcome, 'ok') + + client.send_goal(t1, BehaviorExecutionGoal()) + rospy.sleep(.1) + client.cancel(t1) + rospy.sleep(.9) + + self.assertFalse(client.is_active(t1)) + + self.assertFalse(client.is_available('/not_there')) + client = ProxyActionClient({'/invalid': BehaviorExecutionAction}, wait_duration=.1) + self.assertFalse(client.is_available('/invalid')) + + +if __name__ == '__main__': + rospy.init_node('test_flexbe_proxies') + import rostest + rostest.rosrun('flexbe_core', 'test_flexbe_proxies', TestProxies) From 053cf7a65f4aec35853abc00a2816c8e5af5ac51 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Wed, 25 Mar 2020 15:54:40 +0100 Subject: [PATCH 07/57] Update changelog --- flexbe_behavior_engine/CHANGELOG.rst | 3 +++ flexbe_core/CHANGELOG.rst | 23 +++++++++++++++++++++++ flexbe_input/CHANGELOG.rst | 7 +++++++ flexbe_mirror/CHANGELOG.rst | 6 ++++++ flexbe_msgs/CHANGELOG.rst | 6 ++++++ flexbe_msgs/msg/UserdataInfo.msg | 4 ++++ flexbe_onboard/CHANGELOG.rst | 10 ++++++++++ flexbe_states/CHANGELOG.rst | 6 ++++++ flexbe_testing/CHANGELOG.rst | 13 +++++++++++++ flexbe_widget/CHANGELOG.rst | 8 ++++++++ 10 files changed, 86 insertions(+) create mode 100644 flexbe_msgs/msg/UserdataInfo.msg diff --git a/flexbe_behavior_engine/CHANGELOG.rst b/flexbe_behavior_engine/CHANGELOG.rst index 29683dd1..8800ef46 100644 --- a/flexbe_behavior_engine/CHANGELOG.rst +++ b/flexbe_behavior_engine/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package flexbe_behavior_engine ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.2.3 (2020-01-10) ------------------ * Merge remote-tracking branch 'origin/develop' into feature/test_behaviors diff --git a/flexbe_core/CHANGELOG.rst b/flexbe_core/CHANGELOG.rst index e3a1e434..9a3db28c 100644 --- a/flexbe_core/CHANGELOG.rst +++ b/flexbe_core/CHANGELOG.rst @@ -2,6 +2,29 @@ Changelog for package flexbe_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [flexbe_core] Add tests for proxies +* [flexbe_core] Several minor improvements of proxies + (see `#114 `_) +* [flexbe_msgs] [flexbe_core] Add debug level to logger + (see `#101 `_) +* Merge pull request `#110 `_ from team-vigir/fix/catkin_install + Let behavior library find sourcecode in devel or install spaces +* Let behavior library find sourcecode in devel or install spaces + (fix `#104 `_) +* [flexbe_core] Fix reset of requested outcome when re-visiting the same state and immediately requesting an outcome +* [flexbe_core] Fix duplicate sleep in case of state machine inside concurrency +* [flexbe_core] Robustify priority container path handling +* Do not trigger on_resume and on_exit when stopped during pause + (see `#103 `_) +* Remove mistakenly added text +* Merge branch 'fmessmer-feature/python3_compatibility' into develop +* Remove explicit list construction where not required +* Remove redundant type check +* python3 compatibility via 2to3 +* Contributors: Philipp Schillinger, fmessmer + 1.2.3 (2020-01-10) ------------------ * Merge pull request `#97 `_ from team-vigir/feature/test_behaviors diff --git a/flexbe_input/CHANGELOG.rst b/flexbe_input/CHANGELOG.rst index 16d18c6a..117f184f 100644 --- a/flexbe_input/CHANGELOG.rst +++ b/flexbe_input/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package flexbe_input ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge branch 'fmessmer-feature/python3_compatibility' into develop +* use six.moves for Queue +* python3 compatibility via 2to3 +* Contributors: Philipp Schillinger, fmessmer + 1.2.3 (2020-01-10) ------------------ * Merge remote-tracking branch 'origin/develop' into feature/test_behaviors diff --git a/flexbe_mirror/CHANGELOG.rst b/flexbe_mirror/CHANGELOG.rst index 726845d6..12caaac2 100644 --- a/flexbe_mirror/CHANGELOG.rst +++ b/flexbe_mirror/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package flexbe_mirror ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge branch 'fmessmer-feature/python3_compatibility' into develop +* python3 compatibility via 2to3 +* Contributors: Philipp Schillinger, fmessmer + 1.2.3 (2020-01-10) ------------------ * Revise internal dependencies diff --git a/flexbe_msgs/CHANGELOG.rst b/flexbe_msgs/CHANGELOG.rst index 5a261f5d..75c4d04c 100644 --- a/flexbe_msgs/CHANGELOG.rst +++ b/flexbe_msgs/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package flexbe_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [flexbe_msgs] [flexbe_core] Add debug level to logger + (see `#101 `_) +* Contributors: Philipp Schillinger + 1.2.3 (2020-01-10) ------------------ * Merge remote-tracking branch 'origin/develop' into feature/test_behaviors diff --git a/flexbe_msgs/msg/UserdataInfo.msg b/flexbe_msgs/msg/UserdataInfo.msg new file mode 100644 index 00000000..9b33d844 --- /dev/null +++ b/flexbe_msgs/msg/UserdataInfo.msg @@ -0,0 +1,4 @@ +string state +string key +string type +string data diff --git a/flexbe_onboard/CHANGELOG.rst b/flexbe_onboard/CHANGELOG.rst index 3690c470..426f9063 100644 --- a/flexbe_onboard/CHANGELOG.rst +++ b/flexbe_onboard/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package flexbe_onboard ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge pull request `#110 `_ from team-vigir/fix/catkin_install + Let behavior library find sourcecode in devel or install spaces +* Let behavior library find sourcecode in devel or install spaces + (fix `#104 `_) +* Merge branch 'fmessmer-feature/python3_compatibility' into develop +* python3 compatibility via 2to3 +* Contributors: Philipp Schillinger, fmessmer + 1.2.3 (2020-01-10) ------------------ * Merge pull request `#97 `_ from team-vigir/feature/test_behaviors diff --git a/flexbe_states/CHANGELOG.rst b/flexbe_states/CHANGELOG.rst index 78254553..10cc3f44 100644 --- a/flexbe_states/CHANGELOG.rst +++ b/flexbe_states/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package flexbe_states ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge branch 'fmessmer-feature/python3_compatibility' into develop +* python3 compatibility via 2to3 +* Contributors: Philipp Schillinger, fmessmer + 1.2.3 (2020-01-10) ------------------ * Merge pull request `#97 `_ from team-vigir/feature/test_behaviors diff --git a/flexbe_testing/CHANGELOG.rst b/flexbe_testing/CHANGELOG.rst index ca6659da..3b07395d 100644 --- a/flexbe_testing/CHANGELOG.rst +++ b/flexbe_testing/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package flexbe_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge pull request `#109 `_ from Achllle/feature/testing/timeout_parameter + Expose time-limit parameter from rostest +* Merge pull request `#108 `_ from Achllle/fix/test_bagfile_topic + Retry reading bag file messages without backslash in unit tests +* Expose time-limit parameter from rostest +* Ignore topic backslash when no messages are found that way +* Merge branch 'fmessmer-feature/python3_compatibility' into develop +* Remove explicit list construction where not required +* python3 compatibility via 2to3 +* Contributors: Achille, Philipp Schillinger, fmessmer + 1.2.3 (2020-01-10) ------------------ * Merge pull request `#97 `_ from team-vigir/feature/test_behaviors diff --git a/flexbe_widget/CHANGELOG.rst b/flexbe_widget/CHANGELOG.rst index 996fb404..5a76eee9 100644 --- a/flexbe_widget/CHANGELOG.rst +++ b/flexbe_widget/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package flexbe_widget ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge pull request `#110 `_ from team-vigir/fix/catkin_install + Let behavior library find sourcecode in devel or install spaces +* Let behavior library find sourcecode in devel or install spaces + (fix `#104 `_) +* Contributors: Philipp Schillinger + 1.2.3 (2020-01-10) ------------------ * Revise internal dependencies From 8be5199ff505917c56aab4846eb45a837273aadb Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Wed, 25 Mar 2020 15:54:52 +0100 Subject: [PATCH 08/57] 1.2.4 --- flexbe_behavior_engine/CHANGELOG.rst | 4 ++-- flexbe_behavior_engine/package.xml | 2 +- flexbe_core/CHANGELOG.rst | 4 ++-- flexbe_core/package.xml | 2 +- flexbe_input/CHANGELOG.rst | 4 ++-- flexbe_input/package.xml | 2 +- flexbe_mirror/CHANGELOG.rst | 4 ++-- flexbe_mirror/package.xml | 2 +- flexbe_msgs/CHANGELOG.rst | 4 ++-- flexbe_msgs/package.xml | 2 +- flexbe_onboard/CHANGELOG.rst | 4 ++-- flexbe_onboard/package.xml | 2 +- flexbe_states/CHANGELOG.rst | 4 ++-- flexbe_states/package.xml | 2 +- flexbe_testing/CHANGELOG.rst | 4 ++-- flexbe_testing/package.xml | 2 +- flexbe_widget/CHANGELOG.rst | 4 ++-- flexbe_widget/package.xml | 2 +- 18 files changed, 27 insertions(+), 27 deletions(-) diff --git a/flexbe_behavior_engine/CHANGELOG.rst b/flexbe_behavior_engine/CHANGELOG.rst index 8800ef46..7e38ae12 100644 --- a/flexbe_behavior_engine/CHANGELOG.rst +++ b/flexbe_behavior_engine/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_behavior_engine ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.4 (2020-03-25) +------------------ 1.2.3 (2020-01-10) ------------------ diff --git a/flexbe_behavior_engine/package.xml b/flexbe_behavior_engine/package.xml index a0f84b21..8bc1f761 100644 --- a/flexbe_behavior_engine/package.xml +++ b/flexbe_behavior_engine/package.xml @@ -1,7 +1,7 @@ flexbe_behavior_engine - 1.2.3 + 1.2.4 A meta-package to aggregate all the FlexBE packages diff --git a/flexbe_core/CHANGELOG.rst b/flexbe_core/CHANGELOG.rst index 9a3db28c..5daf91f6 100644 --- a/flexbe_core/CHANGELOG.rst +++ b/flexbe_core/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.4 (2020-03-25) +------------------ * [flexbe_core] Add tests for proxies * [flexbe_core] Several minor improvements of proxies (see `#114 `_) diff --git a/flexbe_core/package.xml b/flexbe_core/package.xml index 56583832..3ba85ff0 100644 --- a/flexbe_core/package.xml +++ b/flexbe_core/package.xml @@ -1,6 +1,6 @@ flexbe_core - 1.2.3 + 1.2.4 flexbe_core provides the core smach extension for the FlexBE behavior engine. diff --git a/flexbe_input/CHANGELOG.rst b/flexbe_input/CHANGELOG.rst index 117f184f..847fff9e 100644 --- a/flexbe_input/CHANGELOG.rst +++ b/flexbe_input/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_input ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.4 (2020-03-25) +------------------ * Merge branch 'fmessmer-feature/python3_compatibility' into develop * use six.moves for Queue * python3 compatibility via 2to3 diff --git a/flexbe_input/package.xml b/flexbe_input/package.xml index a3668552..ec6a5f14 100644 --- a/flexbe_input/package.xml +++ b/flexbe_input/package.xml @@ -1,6 +1,6 @@ flexbe_input - 1.2.3 + 1.2.4 flexbe_input enables to send data to onboard behavior when required. diff --git a/flexbe_mirror/CHANGELOG.rst b/flexbe_mirror/CHANGELOG.rst index 12caaac2..33fd7ca1 100644 --- a/flexbe_mirror/CHANGELOG.rst +++ b/flexbe_mirror/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_mirror ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.4 (2020-03-25) +------------------ * Merge branch 'fmessmer-feature/python3_compatibility' into develop * python3 compatibility via 2to3 * Contributors: Philipp Schillinger, fmessmer diff --git a/flexbe_mirror/package.xml b/flexbe_mirror/package.xml index 9e3774d6..6acd3406 100644 --- a/flexbe_mirror/package.xml +++ b/flexbe_mirror/package.xml @@ -1,6 +1,6 @@ flexbe_mirror - 1.2.3 + 1.2.4 flexbe_mirror implements functionality to remotely mirror an executed behavior. diff --git a/flexbe_msgs/CHANGELOG.rst b/flexbe_msgs/CHANGELOG.rst index 75c4d04c..9a83622c 100644 --- a/flexbe_msgs/CHANGELOG.rst +++ b/flexbe_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.4 (2020-03-25) +------------------ * [flexbe_msgs] [flexbe_core] Add debug level to logger (see `#101 `_) * Contributors: Philipp Schillinger diff --git a/flexbe_msgs/package.xml b/flexbe_msgs/package.xml index d7c31b51..f8e67abc 100644 --- a/flexbe_msgs/package.xml +++ b/flexbe_msgs/package.xml @@ -1,6 +1,6 @@ flexbe_msgs - 1.2.3 + 1.2.4 flexbe_msgs provides the messages used by FlexBE. diff --git a/flexbe_onboard/CHANGELOG.rst b/flexbe_onboard/CHANGELOG.rst index 426f9063..f7888a68 100644 --- a/flexbe_onboard/CHANGELOG.rst +++ b/flexbe_onboard/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_onboard ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.4 (2020-03-25) +------------------ * Merge pull request `#110 `_ from team-vigir/fix/catkin_install Let behavior library find sourcecode in devel or install spaces * Let behavior library find sourcecode in devel or install spaces diff --git a/flexbe_onboard/package.xml b/flexbe_onboard/package.xml index a978c13e..159945c7 100644 --- a/flexbe_onboard/package.xml +++ b/flexbe_onboard/package.xml @@ -1,6 +1,6 @@ flexbe_onboard - 1.2.3 + 1.2.4 flexbe_onboard implements the robot-side of the behavior engine from where all behaviors are started. diff --git a/flexbe_states/CHANGELOG.rst b/flexbe_states/CHANGELOG.rst index 10cc3f44..9cf6b39c 100644 --- a/flexbe_states/CHANGELOG.rst +++ b/flexbe_states/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_states ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.4 (2020-03-25) +------------------ * Merge branch 'fmessmer-feature/python3_compatibility' into develop * python3 compatibility via 2to3 * Contributors: Philipp Schillinger, fmessmer diff --git a/flexbe_states/package.xml b/flexbe_states/package.xml index 49859ac4..55abe343 100644 --- a/flexbe_states/package.xml +++ b/flexbe_states/package.xml @@ -1,6 +1,6 @@ flexbe_states - 1.2.3 + 1.2.4 flexbe_states provides a collection of predefined states. Feel free to add new states. diff --git a/flexbe_testing/CHANGELOG.rst b/flexbe_testing/CHANGELOG.rst index 3b07395d..1c2c1d3d 100644 --- a/flexbe_testing/CHANGELOG.rst +++ b/flexbe_testing/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.4 (2020-03-25) +------------------ * Merge pull request `#109 `_ from Achllle/feature/testing/timeout_parameter Expose time-limit parameter from rostest * Merge pull request `#108 `_ from Achllle/fix/test_bagfile_topic diff --git a/flexbe_testing/package.xml b/flexbe_testing/package.xml index d775195a..43213437 100644 --- a/flexbe_testing/package.xml +++ b/flexbe_testing/package.xml @@ -1,6 +1,6 @@ flexbe_testing - 1.2.3 + 1.2.4 flexbe_testing provides a framework for unit testing states. diff --git a/flexbe_widget/CHANGELOG.rst b/flexbe_widget/CHANGELOG.rst index 5a76eee9..a59aca15 100644 --- a/flexbe_widget/CHANGELOG.rst +++ b/flexbe_widget/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_widget ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.4 (2020-03-25) +------------------ * Merge pull request `#110 `_ from team-vigir/fix/catkin_install Let behavior library find sourcecode in devel or install spaces * Let behavior library find sourcecode in devel or install spaces diff --git a/flexbe_widget/package.xml b/flexbe_widget/package.xml index e41beb90..b2cf4099 100644 --- a/flexbe_widget/package.xml +++ b/flexbe_widget/package.xml @@ -1,6 +1,6 @@ flexbe_widget - 1.2.3 + 1.2.4 flexbe_widget implements some smaller scripts for the behavior engine. From deeb7d2e54646e8c4efd392868bf580b95788366 Mon Sep 17 00:00:00 2001 From: Stefan Fabian Date: Wed, 25 Mar 2020 16:15:27 +0100 Subject: [PATCH 09/57] Using event based action server instead of control loop. Waiting for terminal state of flexbe before setting goal to a terminal state and accepting a new one. --- .../flexbe_widget/behavior_action_server.py | 96 ++++++++----------- 1 file changed, 38 insertions(+), 58 deletions(-) diff --git a/flexbe_widget/src/flexbe_widget/behavior_action_server.py b/flexbe_widget/src/flexbe_widget/behavior_action_server.py index 2172dc4b..5ed18099 100644 --- a/flexbe_widget/src/flexbe_widget/behavior_action_server.py +++ b/flexbe_widget/src/flexbe_widget/behavior_action_server.py @@ -20,14 +20,16 @@ class BehaviorActionServer(object): def __init__(self): self._behavior_started = False self._current_state = None - self._engine_status = None + self._active_behavior_id = None self._pub = rospy.Publisher('flexbe/start_behavior', BehaviorSelection, queue_size=100) self._preempt_pub = rospy.Publisher('flexbe/command/preempt', Empty, queue_size=100) self._status_sub = rospy.Subscriber('flexbe/status', BEStatus, self._status_cb) self._state_sub = rospy.Subscriber('flexbe/behavior_update', String, self._state_cb) - self._as = actionlib.SimpleActionServer('flexbe/execute_behavior', BehaviorExecutionAction, self._execute_cb, False) + self._as = actionlib.SimpleActionServer('flexbe/execute_behavior', BehaviorExecutionAction, None, False) + self._as.register_preempt_callback(self._preempt_cb) + self._as.register_goal_callback(self._goal_cb) self._rp = RosPack() self._behavior_lib = BehaviorLibrary() @@ -38,7 +40,10 @@ def __init__(self): rospy.loginfo("%d behaviors available, ready for start request." % self._behavior_lib.count_behaviors()) - def _execute_cb(self, goal): + def _goal_cb(self): + if not self._as.is_new_goal_available(): + return + goal = self._as.accept_new_goal() rospy.loginfo('Received a new request to start behavior: %s' % goal.behavior_name) be_id, behavior = self._behavior_lib.find_behavior(goal.behavior_name) if be_id is None: @@ -102,74 +107,49 @@ def _execute_cb(self, goal): be_selection.behavior_checksum = zlib.adler32(be_content_new) # reset state before starting new behavior - self._engine_status = None self._current_state = None self._behavior_started = False # start new behavior self._pub.publish(be_selection) - try: - rate = rospy.Rate(10) - while not rospy.is_shutdown(): - if self._current_state is not None: - self._as.publish_feedback(BehaviorExecutionFeedback(self._current_state)) - self._current_state = None - - # check if goal has been preempted first - if self._as.is_preempt_requested(): - rospy.loginfo('Behavior execution preempt requested!') - self._preempt_pub.publish() - rate.sleep() - self._as.set_preempted('') - break - - if self._engine_status is None: - rospy.logdebug_throttle(1, 'No behavior engine status received yet. Waiting for it...') - rate.sleep() - continue - - if self._engine_status.code == BEStatus.ERROR: - rospy.logerr('Failed to run behavior! Check onboard terminal for further infos.') - rate.sleep() - self._as.set_aborted('') - break - - if not self._behavior_started: - rospy.logdebug_throttle(1, 'Behavior execution has not yet started. Waiting for it...') - rate.sleep() - continue - - if self._engine_status.code == BEStatus.FINISHED: - result = self._engine_status.args[0] \ - if len(self._engine_status.args) >= 1 else '' - rospy.loginfo('Finished behavior execution with result "%s"!' % result) - self._as.set_succeeded(BehaviorExecutionResult(outcome=result)) - break - - if self._engine_status.code == BEStatus.FAILED: - rospy.logerr('Behavior execution failed in state %s!' % str(self._current_state)) - rate.sleep() - self._as.set_aborted('') - break - - rate.sleep() - - rospy.loginfo('Ready for next behavior start request.') - - except rospy.ROSInterruptException: - pass # allow clean exit on ROS shutdown + def _preempt_cb(self): + self._preempt_pub.publish() + rospy.loginfo('Behavior execution preempt requested!') - def _status_cb(self, msg): - self._engine_status = msg - # check for behavior start here, to avoid race condition between execute_cb and status_cb threads - if not self._behavior_started and self._engine_status.code == BEStatus.STARTED: + def _status_cb(self, msg): + if not self._behavior_started and msg.code == BEStatus.STARTED: self._behavior_started = True + self._active_behavior_id = msg.behavior_id rospy.loginfo('Behavior execution has started!') + # Ignore status until behavior start was received + if not self._behavior_started: + return + + if msg.behavior_id != self._active_behavior_id: + rospy.logwarn('Ignored status because behavior id differed ({} vs {})!'.format(msg.behavior_id, self._active_behavior_id)) + return + + if msg.code == BEStatus.ERROR: + rospy.logerr('Failed to run behavior! Check onboard terminal for further infos.') + self._as.set_aborted('') + elif msg.code == BEStatus.FINISHED: + result = msg.args[0] if len(msg.args) >= 1 else '' + rospy.loginfo('Finished behavior execution with result "%s"!' % result) + self._as.set_succeeded(BehaviorExecutionResult(outcome=result)) + elif msg.code == BEStatus.FAILED: + rospy.logerr('Behavior execution failed in state %s!' % str(self._current_state)) + self._as.set_aborted('') + + # Start new goal if available and current is not active anymore + if not self._as.is_active() and self._as.is_new_goal_available(): + self._goal_cb() def _state_cb(self, msg): self._current_state = msg.data + if self._as.is_active(): + self._as.publish_feedback(BehaviorExecutionFeedback(self._current_state)) rospy.loginfo('Current state: %s' % self._current_state) From 4e78ed2dfaca29527b07ccd4ed905314221ad488 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Fri, 27 Mar 2020 18:01:51 +0100 Subject: [PATCH 10/57] [flexbe_onboard] Cleanup onboard and add thread locks (see #117) --- flexbe_onboard/bin/start_behavior | 8 +- .../src/flexbe_onboard/flexbe_onboard.py | 241 +++++++++--------- 2 files changed, 122 insertions(+), 127 deletions(-) diff --git a/flexbe_onboard/bin/start_behavior b/flexbe_onboard/bin/start_behavior index fca11fd1..e487d31c 100755 --- a/flexbe_onboard/bin/start_behavior +++ b/flexbe_onboard/bin/start_behavior @@ -1,16 +1,14 @@ #!/usr/bin/env python - -import roslib; roslib.load_manifest('flexbe_onboard') import rospy from flexbe_core.proxy import ProxySubscriberCached +from flexbe_onboard.flexbe_onboard import FlexbeOnboard -from flexbe_onboard.flexbe_onboard import VigirBeOnboard if __name__ == '__main__': rospy.init_node('flexbe_onboard') - - VigirBeOnboard() + + FlexbeOnboard() # Wait for ctrl-c to stop the application rospy.spin() diff --git a/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py b/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py index a074aa4e..90fe6a1e 100644 --- a/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py +++ b/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py @@ -1,8 +1,5 @@ #!/usr/bin/env python - -import roslib; roslib.load_manifest('flexbe_onboard') import rospy -import rospkg import os import sys import inspect @@ -10,91 +7,75 @@ import threading import time import smach -import random -import yaml import zlib -import xml.etree.ElementTree as ET from ast import literal_eval as cast from flexbe_core import Logger, BehaviorLibrary - -from flexbe_msgs.msg import BehaviorSelection, BEStatus, ContainerStructure, CommandFeedback from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached -from std_msgs.msg import Int32, Empty - +from flexbe_msgs.msg import BehaviorSelection, BEStatus, CommandFeedback +from std_msgs.msg import Empty -''' -Created on 20.05.2013 -@author: Philipp Schillinger -''' - -class VigirBeOnboard(object): - ''' - Implements an idle state where the robot waits for a behavior to be started. - ''' +class FlexbeOnboard(object): + """ + Controls the execution of robot behaviors. + """ def __init__(self): - ''' - Constructor - ''' self.be = None Logger.initialize() - smach.set_loggers ( - rospy.logdebug, # hide SMACH transition log spamming - rospy.logwarn, - rospy.logdebug, - rospy.logerr - ) - - #ProxyPublisher._simulate_delay = True - #ProxySubscriberCached._simulate_delay = True + # hide SMACH transition log spamming + smach.set_loggers(rospy.logdebug, rospy.logwarn, rospy.logdebug, rospy.logerr) # prepare temp folder - rp = rospkg.RosPack() self._tmp_folder = tempfile.mkdtemp() sys.path.append(self._tmp_folder) rospy.on_shutdown(self._cleanup_tempdir) - + # prepare manifest folder access self._behavior_lib = BehaviorLibrary() - - self._pub = ProxyPublisher() - self._sub = ProxySubscriberCached() + # prepare communication self.status_topic = 'flexbe/status' self.feedback_topic = 'flexbe/command_feedback' - - self._pub.createPublisher(self.status_topic, BEStatus, _latch = True) - self._pub.createPublisher(self.feedback_topic, CommandFeedback) + self._pub = ProxyPublisher({ + self.feedback_topic: CommandFeedback, + 'flexbe/heartbeat': Empty + }) + self._pub.createPublisher(self.status_topic, BEStatus, _latch=True) + self._execute_heartbeat() # listen for new behavior to start self._running = False + self._run_lock = threading.Lock() self._switching = False + self._switch_lock = threading.Lock() + self._sub = ProxySubscriberCached() self._sub.subscribe('flexbe/start_behavior', BehaviorSelection, self._behavior_callback) - # heartbeat - self._pub.createPublisher('flexbe/heartbeat', Empty) - self._execute_heartbeat() - - rospy.sleep(0.5) # wait for publishers etc to really be set up + rospy.sleep(0.5) # wait for publishers etc to really be set up self._pub.publish(self.status_topic, BEStatus(code=BEStatus.READY)) rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m') - def _behavior_callback(self, msg): thread = threading.Thread(target=self._behavior_execution, args=[msg]) thread.daemon = True thread.start() + # =================== # + # Main execution loop # + # ------------------- # + def _behavior_execution(self, msg): + # sending a behavior while one is already running is considered as switching if self._running: Logger.loginfo('--> Initiating behavior switch...') self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['received'])) else: Logger.loginfo('--> Starting new behavior...') + # construct the behavior that should be executed be = self._prepare_behavior(msg) if be is None: Logger.logerr('Dropped behavior start request because preparation failed.') @@ -104,71 +85,84 @@ def _behavior_execution(self, msg): rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m') return - if self._running: - if self._switching: - Logger.logwarn('Already switching, dropped new start request.') - return - self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['start'])) - if not self._is_switchable(be): - Logger.logerr('Dropped behavior start request because switching is not possible.') - self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['not_switchable'])) - return + # perform the behavior switch if required + with self._switch_lock: self._switching = True - active_state = self.be.get_current_state() - rospy.loginfo("Current state %s is kept active.", active_state.name) - try: - be.prepare_for_switch(active_state) - self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['prepared'])) - except Exception as e: - Logger.logerr('Failed to prepare behavior switch:\n%s' % str(e)) - self._switching = False - self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['failed'])) - return - rospy.loginfo('Preempting current behavior version...') - self.be.preempt_for_switch() - rate = rospy.Rate(10) - while self._running: - rate.sleep() + if self._running: + self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['start'])) + # ensure that switching is possible + if not self._is_switchable(be): + Logger.logerr('Dropped behavior start request because switching is not possible.') + self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['not_switchable'])) + return + # wait if running behavior is currently starting or stopping + rate = rospy.Rate(100) + while not rospy.is_shutdown(): + active_state = self.be.get_current_state() + if active_state is not None or not self._running: + break + rate.sleep() + # extract the active state if any + if active_state is not None: + rospy.loginfo("Current state %s is kept active.", active_state.name) + try: + be.prepare_for_switch(active_state) + self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['prepared'])) + except Exception as e: + Logger.logerr('Failed to prepare behavior switch:\n%s' % str(e)) + self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['failed'])) + return + # stop the rest + rospy.loginfo('Preempting current behavior version...') + self.be.preempt_for_switch() + + # execute the behavior + with self._run_lock: self._switching = False + self.be = be + self._running = True - self._running = True - self.be = be - - result = "" - try: - rospy.loginfo('Behavior ready, execution starts now.') - rospy.loginfo('[%s : %s]', be.name, msg.behavior_checksum) - self.be.confirm() - args = [self.be.requested_state_path] if self.be.requested_state_path is not None else [] - self._pub.publish(self.status_topic, BEStatus(behavior_id=self.be.id, code=BEStatus.STARTED, args=args)) - result = self.be.execute() - if self._switching: - self._pub.publish(self.status_topic, BEStatus(behavior_id=self.be.id, code=BEStatus.SWITCHING)) - else: - self._pub.publish(self.status_topic, BEStatus(behavior_id=self.be.id, code=BEStatus.FINISHED, args=[str(result)])) - except Exception as e: - self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.FAILED)) - Logger.logerr('Behavior execution failed!\n%s' % str(e)) - import traceback - Logger.loginfo(traceback.format_exc()) - result = "failed" + result = None + try: + rospy.loginfo('Behavior ready, execution starts now.') + rospy.loginfo('[%s : %s]', be.name, msg.behavior_checksum) + self.be.confirm() + args = [self.be.requested_state_path] if self.be.requested_state_path is not None else [] + self._pub.publish(self.status_topic, + BEStatus(behavior_id=self.be.id, code=BEStatus.STARTED, args=args)) + result = self.be.execute() + if self._switching: + self._pub.publish(self.status_topic, + BEStatus(behavior_id=self.be.id, code=BEStatus.SWITCHING)) + else: + self._pub.publish(self.status_topic, + BEStatus(behavior_id=self.be.id, code=BEStatus.FINISHED, args=[str(result)])) + except Exception as e: + self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.FAILED)) + Logger.logerr('Behavior execution failed!\n%s' % str(e)) + import traceback + Logger.loginfo(traceback.format_exc()) + result = result or "exception" # only set result if not executed - try: - self._cleanup_behavior(msg.behavior_checksum) - except Exception as e: - rospy.logerr('Failed to clean up behavior:\n%s' % str(e)) + # done, remove left-overs like the temporary behavior file + try: + self._cleanup_behavior(msg.behavior_checksum) + except Exception as e: + rospy.logerr('Failed to clean up behavior:\n%s' % str(e)) - self.be = None - if not self._switching: - rospy.loginfo('Behavior execution finished with result %s.', str(result)) - rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m') - self._running = False + if not self._switching: + rospy.loginfo('Behavior execution finished with result %s.', str(result)) + rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m') + self._running = False + self.be = None + # ==================================== # + # Preparation of new behavior requests # + # ------------------------------------ # def _prepare_behavior(self, msg): # get sourcecode from ros package try: - rp = rospkg.RosPack() behavior = self._behavior_lib.get_behavior(msg.behavior_id) if behavior is None: raise ValueError(msg.behavior_id) @@ -179,8 +173,10 @@ def _prepare_behavior(self, msg): else: be_filepath = self._behavior_lib.get_sourcecode_filepath(msg.behavior_id) be_file = open(be_filepath, "r") - be_content = be_file.read() - be_file.close() + try: + be_content = be_file.read() + finally: + be_file.close() except Exception as e: Logger.logerr('Failed to retrieve behavior from library:\n%s' % str(e)) self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) @@ -210,9 +206,8 @@ def _prepare_behavior(self, msg): # create temp file for behavior class try: file_path = os.path.join(self._tmp_folder, 'tmp_%d.py' % msg.behavior_checksum) - sc_file = open(file_path, "w") - sc_file.write(file_content) - sc_file.close() + with open(file_path, "w") as sc_file: + sc_file.write(file_content) except Exception as e: Logger.logerr('Failed to create temporary file for behavior class:\n%s' % str(e)) self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) @@ -221,7 +216,8 @@ def _prepare_behavior(self, msg): # import temp class file and initialize behavior try: package = __import__("tmp_%d" % msg.behavior_checksum, fromlist=["tmp_%d" % msg.behavior_checksum]) - clsmembers = inspect.getmembers(package, lambda member: inspect.isclass(member) and member.__module__ == package.__name__) + clsmembers = inspect.getmembers(package, lambda member: (inspect.isclass(member) and + member.__module__ == package.__name__)) beclass = clsmembers[0][1] be = beclass() rospy.loginfo('Behavior ' + be.name + ' created.') @@ -247,7 +243,6 @@ def _prepare_behavior(self, msg): rospy.loginfo(key + ' = ' + msg.arg_values[i] + suffix) else: rospy.logwarn('Parameter ' + msg.arg_keys[i] + ' (set to ' + msg.arg_values[i] + ') not defined') - except Exception as e: Logger.logerr('Failed to initialize parameters:\n%s' % str(e)) self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) @@ -259,27 +254,26 @@ def _prepare_behavior(self, msg): be.prepare_for_execution(self._convert_input_data(msg.input_keys, msg.input_values)) rospy.loginfo('State machine built.') except Exception as e: - Logger.logerr('Behavior construction failed!\n%s' % str(e)) - self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) - return + Logger.logerr('Behavior construction failed!\n%s' % str(e)) + self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) + return return be + # ================ # + # Helper functions # + # ---------------- # def _is_switchable(self, be): if self.be.name != be.name: - Logger.logerr('Unable to switch behavior, names do not match:\ncurrent: %s <--> new: %s' % (self.be.name, be.name)) + Logger.logerr('Unable to switch behavior, names do not match:\ncurrent: %s <--> new: %s' % + (self.be.name, be.name)) return False # locked inside # locked state exists in new behavior - #if self.be.id == be.id: - #Logger.logwarn('Behavior version ID is the same.') - # Logger.logwarn('Skipping behavior switch, version ID is the same.') - # return False # ok, can switch return True - def _cleanup_behavior(self, behavior_checksum): del(sys.modules["tmp_%d" % behavior_checksum]) file_path = os.path.join(self._tmp_folder, 'tmp_%d.pyc' % behavior_checksum) @@ -292,7 +286,6 @@ def _cleanup_behavior(self, behavior_checksum): except OSError: pass - def _cleanup_tempdir(self): try: os.remove(self._tmp_folder) @@ -301,17 +294,19 @@ def _cleanup_tempdir(self): def _convert_input_data(self, keys, values): result = dict() - for k, v in zip(keys, values): + # action call has empty string as default, not a valid input key + if k == '': + continue try: result[k] = self._convert_dict(cast(v)) except ValueError: # 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('Unable to parse input value for key "%s", assuming string:\n%s\n%s' % + (k, str(v), str(se))) result[k] = str(v) - return result def _execute_heartbeat(self): @@ -322,9 +317,8 @@ def _execute_heartbeat(self): def _heartbeat_worker(self): while True: self._pub.publish('flexbe/heartbeat', Empty()) - time.sleep(1) # sec + time.sleep(1) - class _attr_dict(dict): __getattr__ = dict.__getitem__ def _convert_dict(self, o): if isinstance(o, list): return [self._convert_dict(e) for e in o] @@ -332,3 +326,6 @@ def _convert_dict(self, o): return self._attr_dict((k, self._convert_dict(v)) for k, v in list(o.items())) else: return o + + class _attr_dict(dict): + __getattr__ = dict.__getitem__ From 5197eac59123fec4e57da64b9acccb8ce1ec3332 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Fri, 27 Mar 2020 18:22:56 +0100 Subject: [PATCH 11/57] [flexbe_msgs] Remove deprecated constants from input action --- flexbe_msgs/action/BehaviorInput.action | 14 +------------- 1 file changed, 1 insertion(+), 13 deletions(-) diff --git a/flexbe_msgs/action/BehaviorInput.action b/flexbe_msgs/action/BehaviorInput.action index afd0ef4a..7a394901 100644 --- a/flexbe_msgs/action/BehaviorInput.action +++ b/flexbe_msgs/action/BehaviorInput.action @@ -1,18 +1,8 @@ -# Available request types - -uint8 POINT_LOCATION = 0 -uint8 SELECTED_OBJECT_ID = 1 -uint8 WAYPOINT_GOAL_POSE = 2 -uint8 GHOST_JOINT_STATES = 3 -uint8 FOOTSTEP_PLAN_HEADER = 4 - - -# Choose one of the available request types above +# If desired, custom request types can be defined and choosen here uint8 request_type # Request message displayed to the operator # Provide context information, i.e. for which purpose the data is required. -# The operator will be told which type of data to provide indepently from this message. string msg --- @@ -29,5 +19,3 @@ uint8 result_code string data --- - - From 3318f197ff91d67365bfa323f16b563f8d79fa10 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Fri, 27 Mar 2020 18:23:21 +0100 Subject: [PATCH 12/57] [flexbe_states] Clean up input state --- .../src/flexbe_states/input_state.py | 170 +++++++----------- 1 file changed, 68 insertions(+), 102 deletions(-) diff --git a/flexbe_states/src/flexbe_states/input_state.py b/flexbe_states/src/flexbe_states/input_state.py index bfd2c177..75d8806d 100644 --- a/flexbe_states/src/flexbe_states/input_state.py +++ b/flexbe_states/src/flexbe_states/input_state.py @@ -15,105 +15,71 @@ ''' class InputState(EventState): - ''' - Implements a state where the state machine needs an input from the operator. - Requests of different types, such as requesting a waypoint, a template, or a pose, can be specified. - - -- request uint8 One of the class constants to specify the type of request. - -- message string Message displayed to the operator to let him know what to do. - - #> data object The data provided by the operator. The exact type depends on the request. - - <= received Returned as soon as valid data is available. - <= aborted The operator declined to provide the requested data. - <= no_connection No request could be sent to the operator. - <= data_error Data has been received, but could not be deserialized successfully. - - ''' - - POINT_LOCATION = BehaviorInputGoal.POINT_LOCATION - SELECTED_OBJECT_ID = BehaviorInputGoal.SELECTED_OBJECT_ID - WAYPOINT_GOAL_POSE = BehaviorInputGoal.WAYPOINT_GOAL_POSE - GHOST_JOINT_STATES = BehaviorInputGoal.GHOST_JOINT_STATES - FOOTSTEP_PLAN_HEADER = BehaviorInputGoal.FOOTSTEP_PLAN_HEADER - - - def __init__(self, request, message): - ''' - Constructor - ''' - super(InputState, self).__init__(outcomes=['received', 'aborted', 'no_connection', 'data_error'], - output_keys=['data']) - - self._action_topic = 'flexbe/behavior_input' - - self._client = ProxyActionClient({self._action_topic: BehaviorInputAction}) - - self._request = request - self._message = message - self._connected = True - self._received = False - - - def execute(self, userdata): - ''' - Execute this state - ''' - if not self._connected: - return 'no_connection' - if self._received: - return 'received' - - if self._client.has_result(self._action_topic): - result = self._client.get_result(self._action_topic) - if result.result_code != BehaviorInputResult.RESULT_OK: - userdata.data = None - return 'aborted' - else: - try: - response_data = pickle.loads(result.data) - #print 'Input request response:', response_data - userdata.data = response_data - # If this was a template ID request, log that template ID: - if self._request == BehaviorInputGoal.SELECTED_OBJECT_ID: - Logger.loginfo('Received template ID: %d' % int(response_data)) - - except Exception as e: - Logger.logwarn('Was unable to load provided data:\n%s' % str(e)) - userdata.data = None - return 'data_error' - - self._received = True - return 'received' - - - def on_enter(self, userdata): - - self._received = False - - if not self._connected: return - - action_goal = BehaviorInputGoal() - action_goal.request_type = self._request - action_goal.msg = self._message - - try: - self._client.send_goal(self._action_topic, action_goal) - except Exception as e: - Logger.logwarn('Was unable to send data request:\n%s' % str(e)) - self._connected = False - - - - - - - - - - - - - - - + ''' + Implements a state where the state machine needs an input from the operator. + Requests of different types, such as requesting a waypoint, a template, or a pose, can be specified. + + -- request uint8 One of the custom-defined values to specify the type of request. + -- message string Message displayed to the operator to let him know what to do. + + #> data object The data provided by the operator. The exact type depends on the request. + + <= received Returned as soon as valid data is available. + <= aborted The operator declined to provide the requested data. + <= no_connection No request could be sent to the operator. + <= data_error Data has been received, but could not be deserialized successfully. + ''' + + def __init__(self, request, message): + ''' + Constructor + ''' + super(InputState, self).__init__(outcomes=['received', 'aborted', 'no_connection', 'data_error'], + output_keys=['data']) + + self._action_topic = 'flexbe/behavior_input' + self._client = ProxyActionClient({self._action_topic: BehaviorInputAction}) + + self._request = request + self._message = message + self._connected = True + self._received = False + + def execute(self, userdata): + ''' + Execute this state + ''' + if not self._connected: + return 'no_connection' + if self._received: + return 'received' + + if self._client.has_result(self._action_topic): + result = self._client.get_result(self._action_topic) + if result.result_code != BehaviorInputResult.RESULT_OK: + userdata.data = None + return 'aborted' + else: + try: + response_data = pickle.loads(result.data) + userdata.data = response_data + except Exception as e: + Logger.logwarn('Was unable to load provided data:\n%s' % str(e)) + userdata.data = None + return 'data_error' + + self._received = True + return 'received' + + def on_enter(self, userdata): + self._received = False + + action_goal = BehaviorInputGoal() + action_goal.request_type = self._request + action_goal.msg = self._message + + try: + self._client.send_goal(self._action_topic, action_goal) + except Exception as e: + Logger.logwarn('Was unable to send data request:\n%s' % str(e)) + self._connected = False From 53b960d3a7f94d10fd2d6ac8bd88ae371f51bd64 Mon Sep 17 00:00:00 2001 From: Stefan Fabian Date: Sat, 28 Mar 2020 10:29:09 +0100 Subject: [PATCH 13/57] Handle errors before behavior start. --- .../src/flexbe_widget/behavior_action_server.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/flexbe_widget/src/flexbe_widget/behavior_action_server.py b/flexbe_widget/src/flexbe_widget/behavior_action_server.py index 5ed18099..df6f7b85 100644 --- a/flexbe_widget/src/flexbe_widget/behavior_action_server.py +++ b/flexbe_widget/src/flexbe_widget/behavior_action_server.py @@ -120,6 +120,12 @@ def _preempt_cb(self): def _status_cb(self, msg): + if msg.code == BEStatus.ERROR: + rospy.logerr('Failed to run behavior! Check onboard terminal for further infos.') + self._as.set_aborted('') + # Call goal cb in case there is a queued goal available + self._goal_cb() + return if not self._behavior_started and msg.code == BEStatus.STARTED: self._behavior_started = True self._active_behavior_id = msg.behavior_id @@ -131,10 +137,6 @@ def _status_cb(self, msg): if msg.behavior_id != self._active_behavior_id: rospy.logwarn('Ignored status because behavior id differed ({} vs {})!'.format(msg.behavior_id, self._active_behavior_id)) return - - if msg.code == BEStatus.ERROR: - rospy.logerr('Failed to run behavior! Check onboard terminal for further infos.') - self._as.set_aborted('') elif msg.code == BEStatus.FINISHED: result = msg.args[0] if len(msg.args) >= 1 else '' rospy.loginfo('Finished behavior execution with result "%s"!' % result) From 413a11d11a2c179dfed3da4102ed7e9c70565ed6 Mon Sep 17 00:00:00 2001 From: Stefan Fabian Date: Sat, 28 Mar 2020 10:29:32 +0100 Subject: [PATCH 14/57] Only accept goal if ActionServer is not active. --- flexbe_widget/src/flexbe_widget/behavior_action_server.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/flexbe_widget/src/flexbe_widget/behavior_action_server.py b/flexbe_widget/src/flexbe_widget/behavior_action_server.py index df6f7b85..01f01ce8 100644 --- a/flexbe_widget/src/flexbe_widget/behavior_action_server.py +++ b/flexbe_widget/src/flexbe_widget/behavior_action_server.py @@ -41,7 +41,7 @@ def __init__(self): def _goal_cb(self): - if not self._as.is_new_goal_available(): + if self._as.is_active() or not self._as.is_new_goal_available(): return goal = self._as.accept_new_goal() rospy.loginfo('Received a new request to start behavior: %s' % goal.behavior_name) @@ -141,12 +141,12 @@ def _status_cb(self, msg): result = msg.args[0] if len(msg.args) >= 1 else '' rospy.loginfo('Finished behavior execution with result "%s"!' % result) self._as.set_succeeded(BehaviorExecutionResult(outcome=result)) + # Call goal cb in case there is a queued goal available + self._goal_cb() elif msg.code == BEStatus.FAILED: rospy.logerr('Behavior execution failed in state %s!' % str(self._current_state)) self._as.set_aborted('') - - # Start new goal if available and current is not active anymore - if not self._as.is_active() and self._as.is_new_goal_available(): + # Call goal cb in case there is a queued goal available self._goal_cb() From 64fd986898ff12a3cb1005a593cc370bf4e997a7 Mon Sep 17 00:00:00 2001 From: Stefan Fabian Date: Sat, 28 Mar 2020 10:33:45 +0100 Subject: [PATCH 15/57] Improved preempt logic. --- flexbe_widget/src/flexbe_widget/behavior_action_server.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/flexbe_widget/src/flexbe_widget/behavior_action_server.py b/flexbe_widget/src/flexbe_widget/behavior_action_server.py index 01f01ce8..3ad27bee 100644 --- a/flexbe_widget/src/flexbe_widget/behavior_action_server.py +++ b/flexbe_widget/src/flexbe_widget/behavior_action_server.py @@ -19,6 +19,7 @@ class BehaviorActionServer(object): def __init__(self): self._behavior_started = False + self._preempt_requested = False self._current_state = None self._active_behavior_id = None @@ -109,12 +110,16 @@ def _goal_cb(self): # reset state before starting new behavior self._current_state = None self._behavior_started = False + self._preempt_requested = False # start new behavior self._pub.publish(be_selection) def _preempt_cb(self): + self._preempt_requested = True + if not self._behavior_started: + return self._preempt_pub.publish() rospy.loginfo('Behavior execution preempt requested!') @@ -130,6 +135,9 @@ def _status_cb(self, msg): self._behavior_started = True self._active_behavior_id = msg.behavior_id rospy.loginfo('Behavior execution has started!') + # Preempt if the goal was asked to preempt before the behavior started + if self._preempt_requested: + self._preempt_cb() # Ignore status until behavior start was received if not self._behavior_started: return From adbf7d440207c6286a6fedeae43083f8316884fa Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Sun, 29 Mar 2020 18:14:01 +0200 Subject: [PATCH 16/57] [flexbe_widget] Update evaluate_logs script to new format --- flexbe_widget/bin/evaluate_logs | 242 ++++++++++++++++++-------------- 1 file changed, 134 insertions(+), 108 deletions(-) diff --git a/flexbe_widget/bin/evaluate_logs b/flexbe_widget/bin/evaluate_logs index 0abaa46c..23b66aec 100755 --- a/flexbe_widget/bin/evaluate_logs +++ b/flexbe_widget/bin/evaluate_logs @@ -1,113 +1,139 @@ #!/usr/bin/env python - -import roslib; roslib.load_manifest('flexbe_widget') -import rospy -import os import sys +import yaml +import argparse +from collections import defaultdict + if __name__ == '__main__': - # config - sep = ", " - output_latex = False - - if len(sys.argv) < 4: - print "Usage:" - print "> rosrun flexbe_widget evaluate_logs [logfile] [key] [value1 value2 ...]" - print "" - print ' - logfile: One of the logged files in "~/.flexbe_logs".' - print ' Use an empty string ("") for latest file.' - print " - key: One of the following keys:" - print " total, state_path, state_name, state_class, transition" - print " - value: List of generated output values, choose from:" - print " count_interventions, count_wait, time, time_wait, time_run, wrong_decisions, right_decisions, approved_decisions, visits" - - else: - - filename = sys.argv[1] - keyname = sys.argv[2] - valuelist = sys.argv[3:] - - behavior_name = "" - log_lines = [] # raw - entries = [] # pre-processed - result = {} # fully processed - - # read file - logfolder = os.chdir(os.path.expanduser("~/.flexbe_logs")) - file_path = max(os.listdir('.'), key = os.path.getctime) if filename == "" else filename - with open(file_path) as f: - log_lines = [line.rstrip('\n') for line in f] - - # pre-process content - last_time = None - last_outcome = None - last_blocked = False - for line in log_lines: - l = line.split(',') - - if last_time is None: - last_time = float(l[0]) - last_outcome = l[3] - behavior_name = l[1] - continue - - time = float(l[0]) - last_time - entries.append({ - 'total': "TOTAL", - 'state_path': l[1], - 'state_name': l[1].rsplit('/',1)[1].replace('_', ' '), - 'state_class': l[2], - 'transition': l[1].rsplit('/',1)[1].replace('_', ' ') + " > " + l[3], - 'count_interventions': 1 - int(l[4]), - 'count_wait': 1 - int(l[5]), - 'time': time, - 'time_wait': time if int(l[5]) == 0 else 0, - 'time_run': time if int(l[5]) == 1 else 0, - 'wrong_decisions': 1 if last_blocked and int(l[5]) == 1 and int(l[4]) == 1 and last_outcome != l[3] else 0, - 'right_decisions': 1 if int(l[5]) == 1 and int(l[4]) == 1 else 0, - 'approved_decisions': 1 if last_blocked and int(l[5]) == 1 and int(l[4]) == 1 and last_outcome == l[3] else 0, - 'visits': 1 if not last_blocked else 0 - }) - last_time = float(l[0]) - last_outcome = l[3] - last_blocked = int(l[5]) == 0 - - # collect result - for entry in entries: - k = entry[keyname] - if not result.has_key(k): - result[k] = dict() - for v in valuelist: - result[k][v] = 0 - - for v in valuelist: - result[k][v] += entry[v] - - # output - printed_header = False - for k,r in result.iteritems(): - if not printed_header: - print behavior_name - header = "%s%s%s" % (keyname, sep, sep.join([str(h) for h,v in r.iteritems()])) - print header - printed_header = True - - print "%s%s%s" % (k, sep, sep.join([str(v) for h,v in r.iteritems()])) - - if output_latex: - print "" - print "" - printed_header = False - print "\\begin{tabular}{|l|%s}" % "".join(['r|' for x in range(len(valuelist))]) - for k,r in result.iteritems(): - if not printed_header: - header = "%s & %s \\\\" % (keyname, " & ".join([str(h) for h,v in r.iteritems()])) - print "\\hline" - print header - print "\\hline" - printed_header = True - - print "%s & %s \\\\" % (k.replace("_", " "), " & ".join([str("%.3f" % v) for h,v in r.iteritems()])) - print "\\hline" - print "\\end{tabular}" + parser = argparse.ArgumentParser(description="Evaluate a logged behavior execution.") + parser.add_argument('logfile', type=str, help="The log file to be evaluated.") + parser.add_argument('key', type=str, + help="Defines what should be used as key for grouping logs. Choices are:\n" + "[total] Determine a single value for the complete behavior execution.\n" + "[state_path] One value for each individual state.\n" + "[state_name] Group states with the same name.\n" + "[state_class] One value for each class, i.e., group same state types.\n" + "[transition] Each outcome is handled separately for each state.\n") + parser.add_argument('values', type=str, nargs='+', + help="List of calculated evaluation values, result is sorted by first value specified." + "Choices are:\n" + "[time] Execution time, i.e., time of being the active state.\n" + "[time_wait] Part of execution time of waiting for a requested outcome.\n" + "[time_run] Part of execution time of actually executing.\n" + "[visits] Number of state visits.\n" + "[interact] Number of interactions, e.g., requesting and forcing outcomes.\n" + "[count_wait] Counts how often the state needed to wait for the operator.\n" + "[correct] Number of requested outcomes that got approved by the operator.\n" + "[wrong] Number of forced outcomes that are different from requested ones.\n") + args = parser.parse_args() + + # load logfile + + try: + with open(args.logfile) as f: + logs = yaml.load(f) + except Exception as e: + print('Failed to load logfile "%s":\n%s' % (str(args.logfile), str(e))) + sys.exit(1) + + if logs is None: + print('Given logfile is empty, please evaluate a different one.') + sys.exit(1) + + behavior = "unknown" + execution_time = None + state_history = list() + active_states = dict() + + # process logs to construct state execution instances + + for log in logs: + if log['logger'] == 'flexbe.initialize': + behavior = log['behavior'] + execution_time = log['time'] + + elif log['logger'] == 'flexbe.events' and log['event'] == 'enter': + active_states[log['path']] = defaultdict(int) + active_states[log['path']]['state_name'] = log['name'] + active_states[log['path']]['state_path'] = log['path'] + active_states[log['path']]['state_class'] = log['state'] + active_states[log['path']]['begin'] = log['time'] + active_states[log['path']]['visits'] = 1 + + elif log['logger'] == 'flexbe.events' and log['event'] == 'exit': + active_states[log['path']]['time'] = log['time'] - active_states[log['path']]['begin'] + del active_states[log['path']]['begin'] + if 'begin_wait' in active_states[log['path']]: + active_states[log['path']]['time_wait'] = log['time'] - active_states[log['path']]['begin_wait'] + del active_states[log['path']]['begin_wait'] + active_states[log['path']]['time_run'] = (active_states[log['path']]['time'] - + active_states[log['path']]['time_wait']) + else: + active_states[log['path']]['time_wait'] = 0 + active_states[log['path']]['time_run'] = active_states[log['path']]['time'] + state_history.append(active_states[log['path']]) + del active_states[log['path']] + + elif log['logger'] == 'flexbe.outcomes': + active_states[log['path']]['outcome'] = log['outcome'] + + elif log['logger'] == 'flexbe.operator' and log['type'] == 'forced': + active_states[log['path']]['interact'] += 1 + if log['forced'] == log['requested']: + active_states[log['path']]['correct'] += 1 + else: + active_states[log['path']]['wrong'] += 1 + + elif log['logger'] == 'flexbe.operator' and log['type'] == 'request': + active_states[log['path']]['count_wait'] += 1 + active_states[log['path']]['begin_wait'] = log['time'] + + if len(active_states) > 0: + print('Log file appears incomplete, ignoring execution of states:\n%s' % ', '.join(active_states.keys())) + + # combine state execution instances according to selected key and values + + try: + key_definitions = { + 'total': lambda s: "TOTAL", + 'state_path': lambda s: s['state_path'], + 'state_name': lambda s: s['state_name'], + 'state_class': lambda s: s['state_class'], + 'transition': lambda s: "%s > %s" % (s['state_path'], s['outcome']) + } + key_function = key_definitions[args.key] + except KeyError as e: + print('Invalid key selected: %s\nAvailable: %s' % (str(args.key), ', '.join(key_definitions.keys()))) + sys.exit(1) + + values = args.values + try: + accumulated = dict() + for state in state_history: + key = key_function(state) + if key not in accumulated: + accumulated[key] = defaultdict(int) + for value in values: + accumulated[key][value] += state[value] + except KeyError as e: + keys = set(state.keys()) - {'state_name', 'state_path', 'state_class'} + print('Invalid value selected: %s\nAvailable: %s' % (str(args.key), ', '.join(keys))) + sys.exit(1) + + # display evaluation result + + sorted_keys = sorted(accumulated.keys(), key=lambda k: accumulated[k][values[0]], reverse=True) + + print('Evaluation of behavior "%s"' % behavior) + print(("{:^25} " + "{:^10} " * len(values)).format('key', *values)) + for key in sorted_keys: + line = "{:<25.25} ".format(key if len(key) <= 25 else '..'+key[-23:]) + for value in values: + value = accumulated[key][value] + if isinstance(value, float): + line += "{:>10.3f} ".format(value) + else: + line += "{:>10} ".format(value) + print(line) From cc51dada90e9651c72a4196b72a90dda0a224394 Mon Sep 17 00:00:00 2001 From: Tobias Doernbach Date: Mon, 30 Mar 2020 10:32:41 +0200 Subject: [PATCH 17/57] wait for READY status from Behavior Engine before launching behavior Whenever behavior_onboard and be_launcher are launched together, there used to be a race condition of publishing the behavior in behavior_launcher, but the subscriber in behavior_onboard not being ready yet. Hence, behavior_launcher now waits for the READY status to appear on the flexbe/status topic before it actually attempts to launch the behavior. --- flexbe_widget/src/flexbe_widget/behavior_launcher.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/flexbe_widget/src/flexbe_widget/behavior_launcher.py b/flexbe_widget/src/flexbe_widget/behavior_launcher.py index 2d37bb7a..5fbc11bf 100644 --- a/flexbe_widget/src/flexbe_widget/behavior_launcher.py +++ b/flexbe_widget/src/flexbe_widget/behavior_launcher.py @@ -13,6 +13,7 @@ import os import yaml import xml.etree.ElementTree as ET +import threading class BehaviorLauncher(object): @@ -26,13 +27,18 @@ def __init__(self): self._pub = rospy.Publisher("flexbe/start_behavior", BehaviorSelection, queue_size=100) self._status_pub = rospy.Publisher("flexbe/status", BEStatus, queue_size=100) + self._status_sub = rospy.Subscriber("flexbe/status", BEStatus, self._status_callback) self._mirror_pub = rospy.Publisher("flexbe/mirror/structure", ContainerStructure, queue_size=100) self._rp = RosPack() self._behavior_lib = BehaviorLibrary() + self._ready_event = threading.Event() rospy.loginfo("%d behaviors available, ready for start request." % self._behavior_lib.count_behaviors()) + def _status_callback(self, msg): + if msg.code == BEStatus.READY: + self._ready_event.set() def _callback(self, msg): be_id, behavior = self._behavior_lib.find_behavior(msg.behavior_name) @@ -70,6 +76,9 @@ def _callback(self, msg): be_selection.arg_keys = msg.arg_keys be_selection.arg_values = msg.arg_values + # wait until Behavior Engine status is BEStatus.READY + self._ready_event.wait() + be_structure = ContainerStructure() be_structure.containers = msg.structure From d18551dd3a704b7e2e6fef7b37de0b650568e9f6 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Mon, 30 Mar 2020 15:56:51 +0200 Subject: [PATCH 18/57] [flexbe_core] Allow to specify a subset of logged userdata keys --- flexbe_core/src/flexbe_core/state_logger.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/flexbe_core/src/flexbe_core/state_logger.py b/flexbe_core/src/flexbe_core/state_logger.py index 022b99a0..71f438e4 100644 --- a/flexbe_core/src/flexbe_core/state_logger.py +++ b/flexbe_core/src/flexbe_core/state_logger.py @@ -141,7 +141,7 @@ def execute_wrapper(*args, **kwargs): return decorator @staticmethod - def log_userdata(name): + def log_userdata(name, keys=None): """ Log all userdata that is passed to the state. """ def decorator(cls): cls_init = cls.__init__ @@ -156,6 +156,8 @@ def on_enter_wrapper(userdata): if StateLogger.enabled and logger.isEnabledFor(logging.DEBUG) and input_keys: logdata = dict(StateLogger._basic(self), userdata=dict()) for key in input_keys: + if keys is not None and key not in keys: + continue try: logdata['userdata'][key] = StateLogger._serialize(userdata[key]) except Exception as e: From c857e674ab4344aaf14e720d9cd6dc545d581e53 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Mon, 30 Mar 2020 18:43:54 +0200 Subject: [PATCH 19/57] [flexbe_onboard] Add test cases for onboard engine --- flexbe_onboard/CMakeLists.txt | 5 + flexbe_onboard/package.xml | 2 + flexbe_onboard/test/flexbe_onboard.test | 5 + .../test/flexbe_onboard_test_data/__init__.py | 0 .../test_behavior_complex.xml | 28 +++++ .../test_behavior_complex_sm.py | 96 ++++++++++++++++ .../test_behavior_log.xml | 18 +++ .../test_behavior_log_sm.py | 75 +++++++++++++ flexbe_onboard/test/test_onboard.py | 106 ++++++++++++++++++ 9 files changed, 335 insertions(+) create mode 100644 flexbe_onboard/test/flexbe_onboard.test create mode 100644 flexbe_onboard/test/flexbe_onboard_test_data/__init__.py create mode 100644 flexbe_onboard/test/flexbe_onboard_test_data/test_behavior_complex.xml create mode 100644 flexbe_onboard/test/flexbe_onboard_test_data/test_behavior_complex_sm.py create mode 100644 flexbe_onboard/test/flexbe_onboard_test_data/test_behavior_log.xml create mode 100644 flexbe_onboard/test/flexbe_onboard_test_data/test_behavior_log_sm.py create mode 100755 flexbe_onboard/test/test_onboard.py diff --git a/flexbe_onboard/CMakeLists.txt b/flexbe_onboard/CMakeLists.txt index a05481bd..ae410d38 100644 --- a/flexbe_onboard/CMakeLists.txt +++ b/flexbe_onboard/CMakeLists.txt @@ -26,3 +26,8 @@ install(PROGRAMS bin/start_behavior DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +# run tests +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest(test/flexbe_onboard.test) +endif() diff --git a/flexbe_onboard/package.xml b/flexbe_onboard/package.xml index 159945c7..2f9804d6 100644 --- a/flexbe_onboard/package.xml +++ b/flexbe_onboard/package.xml @@ -11,6 +11,8 @@ Philipp Schillinger catkin + + rostest smach_ros rospy diff --git a/flexbe_onboard/test/flexbe_onboard.test b/flexbe_onboard/test/flexbe_onboard.test new file mode 100644 index 00000000..3e648c6d --- /dev/null +++ b/flexbe_onboard/test/flexbe_onboard.test @@ -0,0 +1,5 @@ + + + + + diff --git a/flexbe_onboard/test/flexbe_onboard_test_data/__init__.py b/flexbe_onboard/test/flexbe_onboard_test_data/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/flexbe_onboard/test/flexbe_onboard_test_data/test_behavior_complex.xml b/flexbe_onboard/test/flexbe_onboard_test_data/test_behavior_complex.xml new file mode 100644 index 00000000..9295f1cf --- /dev/null +++ b/flexbe_onboard/test/flexbe_onboard_test_data/test_behavior_complex.xml @@ -0,0 +1,28 @@ + + + + + + + Philipp Schillinger + Mon Mar 30 2020 + + A more complex behavior for testing the onboard engine. +Note: This behavior contains intentional errors that are fixed by sent modifications. + + + + + + + + + + + + + \ No newline at end of file diff --git a/flexbe_onboard/test/flexbe_onboard_test_data/test_behavior_complex_sm.py b/flexbe_onboard/test/flexbe_onboard_test_data/test_behavior_complex_sm.py new file mode 100644 index 00000000..dea38f9b --- /dev/null +++ b/flexbe_onboard/test/flexbe_onboard_test_data/test_behavior_complex_sm.py @@ -0,0 +1,96 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +########################################################### +# WARNING: Generated code! # +# ************************** # +# Manual changes may get lost if file is generated again. # +# Only code inside the [MANUAL] tags will be kept. # +########################################################### + +from flexbe_INVALID import Behavior, Autonomy, OperatableStateMachine, ConcurrencyContainer, PriorityContainer, Logger +from flexbe_states.wait_state import WaitState +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 +# Additional imports can be added inside the following tags +# [MANUAL_IMPORT] +raise ValueError("TODO: Remove!") +# [/MANUAL_IMPORT] + + +''' +Created on Mon Mar 30 2020 +@author: Philipp Schillinger +''' +class TestBehaviorComplexSM(Behavior): + ''' + A more complex behavior for testing the onboard engine. +Note: This behavior contains intentional errors that are fixed by sent modifications. + ''' + + + def __init__(self): + super(TestBehaviorComplexSM, self).__init__() + self.name = 'Test Behavior Complex' + + # parameters of this behavior + self.add_parameter('param', 'value_1') + + # references to used behaviors + + # Additional initialization code can be added inside the following tags + # [MANUAL_INIT] + + # [/MANUAL_INIT] + + # Behavior comments: + + + + def create(self): + # x:67 y:463, x:336 y:160 + _state_machine = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['data']) + _state_machine.userdata.data = 0 + + # Additional creation code can be added inside the following tags + # [MANUAL_CREATE] + + # [/MANUAL_CREATE] + + + with _state_machine: + # x:30 y:40 + OperatableStateMachine.add('Wait', + WaitState(wait_time=0.5), + transitions={'done': 'Calculate'}, + autonomy={'done': Autonomy.Off}) + + # x:36 y:240 + OperatableStateMachine.add('Log Param', + flexbe_states__LogState(text=self.param, severity=2), + transitions={'done': 'Verify Input'}, + autonomy={'done': Autonomy.Off}) + + # x:32 y:340 + OperatableStateMachine.add('Verify Input', + DecisionState(outcomes=['accepted', 'rejected'], conditions=lambda x: 'accepted' if x > 3 else 'rejected'), + transitions={'accepted': 'finished', 'rejected': 'failed'}, + autonomy={'accepted': Autonomy.Off, 'rejected': Autonomy.Off}, + remapping={'input_value': 'data'}) + + # x:28 y:136 + OperatableStateMachine.add('Calculate', + CalculationState(calculation=self._calculate), + transitions={'done': 'Log Param'}, + autonomy={'done': Autonomy.Off}, + remapping={'input_value': 'data', 'output_value': 'data'}) + + + return _state_machine + + + # Private functions can be added inside the following tags + # [MANUAL_FUNC] + def _calculate(self, data): + return data**2 + # [/MANUAL_FUNC] diff --git a/flexbe_onboard/test/flexbe_onboard_test_data/test_behavior_log.xml b/flexbe_onboard/test/flexbe_onboard_test_data/test_behavior_log.xml new file mode 100644 index 00000000..197b1fed --- /dev/null +++ b/flexbe_onboard/test/flexbe_onboard_test_data/test_behavior_log.xml @@ -0,0 +1,18 @@ + + + + + + + Philipp Schillinger + Mon Mar 30 2020 + + Simple behavior for testing the onboard engine. + + + + + + + + \ No newline at end of file diff --git a/flexbe_onboard/test/flexbe_onboard_test_data/test_behavior_log_sm.py b/flexbe_onboard/test/flexbe_onboard_test_data/test_behavior_log_sm.py new file mode 100644 index 00000000..5d0262fe --- /dev/null +++ b/flexbe_onboard/test/flexbe_onboard_test_data/test_behavior_log_sm.py @@ -0,0 +1,75 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +########################################################### +# WARNING: Generated code! # +# ************************** # +# Manual changes may get lost if file is generated again. # +# Only code inside the [MANUAL] tags will be kept. # +########################################################### + +from flexbe_core import Behavior, Autonomy, OperatableStateMachine, ConcurrencyContainer, PriorityContainer, Logger +from flexbe_states.log_state import LogState as flexbe_states__LogState +from flexbe_states.wait_state import WaitState +# Additional imports can be added inside the following tags +# [MANUAL_IMPORT] + +# [/MANUAL_IMPORT] + + +''' +Created on Mon Mar 30 2020 +@author: Philipp Schillinger +''' +class TestBehaviorLogSM(Behavior): + ''' + Simple behavior for testing the onboard engine. + ''' + + + def __init__(self): + super(TestBehaviorLogSM, self).__init__() + self.name = 'Test Behavior Log' + + # parameters of this behavior + + # references to used behaviors + + # Additional initialization code can be added inside the following tags + # [MANUAL_INIT] + + # [/MANUAL_INIT] + + # Behavior comments: + + + + def create(self): + # x:30 y:365, x:130 y:365 + _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) + + # Additional creation code can be added inside the following tags + # [MANUAL_CREATE] + + # [/MANUAL_CREATE] + + + with _state_machine: + # x:30 y:40 + OperatableStateMachine.add('Log', + flexbe_states__LogState(text="Test data", severity=2), + transitions={'done': 'Wait'}, + autonomy={'done': Autonomy.Off}) + # x:30 y:90 + OperatableStateMachine.add('Wait', + WaitState(wait_time=1.0), + transitions={'done': 'finished'}, + autonomy={'done': Autonomy.Off}) + + + return _state_machine + + + # Private functions can be added inside the following tags + # [MANUAL_FUNC] + + # [/MANUAL_FUNC] diff --git a/flexbe_onboard/test/test_onboard.py b/flexbe_onboard/test/test_onboard.py new file mode 100755 index 00000000..30e8d348 --- /dev/null +++ b/flexbe_onboard/test/test_onboard.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python +import sys +import os +import unittest +import zlib +import rospy + +from flexbe_onboard.flexbe_onboard import FlexbeOnboard +from flexbe_core.proxy import ProxySubscriberCached + +from flexbe_msgs.msg import BehaviorSelection, BEStatus, BehaviorLog, BehaviorModification + + +class TestOnboard(unittest.TestCase): + + def __init__(self, name): + super(TestOnboard, self).__init__(name) + self.sub = ProxySubscriberCached({ + 'flexbe/status': BEStatus, + 'flexbe/log': BehaviorLog + }) + self.rate = rospy.Rate(100) + # make sure that behaviors can be imported + data_folder = os.path.dirname(os.path.realpath(__file__)) + sys.path.insert(0, data_folder) + # run onboard and add custom test behaviors to onboard lib + self.onboard = FlexbeOnboard() + self.lib = self.onboard._behavior_lib + self.lib._add_behavior_manifests(data_folder) + + def assertStatus(self, expected, timeout): + """ Assert that the expected onboard status is received before the timeout. """ + for i in range(int(timeout*100)): + self.rate.sleep() + if self.sub.has_msg('flexbe/status'): + break + else: + raise AssertionError('Did not receive a status as required.') + msg = self.sub.get_last_msg('flexbe/status') + self.sub.remove_last_msg('flexbe/status') + self.assertEqual(msg.code, expected) + return msg + + def test_onboard_behaviors(self): + behavior_pub = rospy.Publisher('flexbe/start_behavior', BehaviorSelection, queue_size=1) + rospy.sleep(0.5) # wait for publisher + + # wait for the initial status message + self.assertStatus(BEStatus.READY, 1) + + # send simple behavior request without checksum + be_id, _ = self.lib.find_behavior("Test Behavior Log") + request = BehaviorSelection() + request.behavior_id = be_id + request.autonomy_level = 255 + behavior_pub.publish(request) + self.assertStatus(BEStatus.ERROR, 2) + + # send valid simple behavior request + with open(self.lib.get_sourcecode_filepath(be_id)) as f: + request.behavior_checksum = zlib.adler32(f.read()) + behavior_pub.publish(request) + self.assertStatus(BEStatus.STARTED, 1) + self.assertStatus(BEStatus.FINISHED, 3) + self.assertIn('data', self.sub.get_last_msg('flexbe/log').text) + + # send valid complex behavior request + be_id, _ = self.lib.find_behavior("Test Behavior Complex") + request = BehaviorSelection() + request.behavior_id = be_id + request.autonomy_level = 255 + request.arg_keys = ['param'] + request.arg_values = ['value_2'] + request.input_keys = ['data'] + request.input_values = ['2'] + with open(self.lib.get_sourcecode_filepath(be_id)) as f: + content = f.read() + modifications = [('flexbe_INVALID', 'flexbe_core'), ('raise ValueError("TODO: Remove!")', '')] + for replace, by in modifications: + index = content.index(replace) + request.modifications.append(BehaviorModification(index, index + len(replace), by)) + for replace, by in modifications: + content = content.replace(replace, by) + request.behavior_checksum = zlib.adler32(content) + behavior_pub.publish(request) + self.assertStatus(BEStatus.STARTED, 1) + result = self.assertStatus(BEStatus.FINISHED, 3) + self.assertEqual(result.args[0], 'finished') + self.assertIn('value_2', self.sub.get_last_msg('flexbe/log').text) + + # send the same behavior with different parameters + request.arg_keys = ['param', 'invalid'] + request.arg_values = ['value_1', 'should be ignored'] + request.input_keys = [] + request.input_values = [] + behavior_pub.publish(request) + self.assertStatus(BEStatus.STARTED, 1) + result = self.assertStatus(BEStatus.FINISHED, 3) + self.assertEqual(result.args[0], 'failed') + self.assertIn('value_1', self.sub.get_last_msg('flexbe/log').text) + + +if __name__ == '__main__': + rospy.init_node('test_flexbe_onboard') + import rostest + rostest.rosrun('flexbe_onboard', 'test_flexbe_onboard', TestOnboard) From 4285e6028872769df86f08eb6291a466bda1614d Mon Sep 17 00:00:00 2001 From: Tobias Doernbach Date: Tue, 31 Mar 2020 18:21:48 +0200 Subject: [PATCH 20/57] avoid callback trigger before ready event has been created --- flexbe_widget/src/flexbe_widget/behavior_launcher.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/flexbe_widget/src/flexbe_widget/behavior_launcher.py b/flexbe_widget/src/flexbe_widget/behavior_launcher.py index 5fbc11bf..6f6c4242 100644 --- a/flexbe_widget/src/flexbe_widget/behavior_launcher.py +++ b/flexbe_widget/src/flexbe_widget/behavior_launcher.py @@ -22,6 +22,8 @@ class BehaviorLauncher(object): def __init__(self): Logger.initialize() + self._ready_event = threading.Event() + self._sub = rospy.Subscriber("flexbe/request_behavior", BehaviorRequest, self._callback) self._version_sub = rospy.Subscriber("flexbe/ui_version", String, self._version_callback) @@ -32,7 +34,6 @@ def __init__(self): self._rp = RosPack() self._behavior_lib = BehaviorLibrary() - self._ready_event = threading.Event() rospy.loginfo("%d behaviors available, ready for start request." % self._behavior_lib.count_behaviors()) From 68820d052d1596813e4c3fe42e15c3f24faf61bb Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Tue, 31 Mar 2020 19:53:45 +0200 Subject: [PATCH 21/57] Remove smach dependency --- flexbe_core/package.xml | 3 +- flexbe_core/src/flexbe_core/behavior.py | 9 +- .../flexbe_core/core/concurrency_container.py | 68 +++++------ .../core/jumpable_state_machine.py | 7 +- .../core/lockable_state_machine.py | 24 ++-- .../src/flexbe_core/core/loopback_state.py | 2 +- .../core/loopback_state_machine.py | 2 +- .../src/flexbe_core/core/monitoring_state.py | 7 +- .../core/operatable_state_machine.py | 114 ++---------------- .../flexbe_core/core/priority_container.py | 5 +- .../flexbe_core/core/silent_state_machine.py | 8 +- flexbe_core/src/flexbe_core/core/state.py | 62 ++++++++++ .../src/flexbe_core/core/state_machine.py | 113 +++++++++++++++++ flexbe_core/src/flexbe_core/core/user_data.py | 36 ++++++ flexbe_input/package.xml | 1 - flexbe_mirror/package.xml | 1 - .../src/flexbe_mirror/flexbe_mirror.py | 12 +- flexbe_msgs/package.xml | 1 - flexbe_onboard/package.xml | 1 - .../src/flexbe_onboard/flexbe_onboard.py | 3 - flexbe_states/package.xml | 1 - flexbe_testing/package.xml | 1 - flexbe_testing/src/flexbe_testing/tester.py | 8 +- flexbe_widget/package.xml | 1 - 24 files changed, 279 insertions(+), 211 deletions(-) create mode 100644 flexbe_core/src/flexbe_core/core/state.py create mode 100644 flexbe_core/src/flexbe_core/core/state_machine.py create mode 100644 flexbe_core/src/flexbe_core/core/user_data.py diff --git a/flexbe_core/package.xml b/flexbe_core/package.xml index 3ba85ff0..97a55ea1 100644 --- a/flexbe_core/package.xml +++ b/flexbe_core/package.xml @@ -2,7 +2,7 @@ flexbe_core 1.2.4 - flexbe_core provides the core smach extension for the FlexBE behavior engine. + flexbe_core provides the core components for the FlexBE behavior engine. Philipp Schillinger BSD @@ -14,7 +14,6 @@ rostest - smach_ros rospy tf diagnostic_msgs diff --git a/flexbe_core/src/flexbe_core/behavior.py b/flexbe_core/src/flexbe_core/behavior.py index 8e76c223..c0224977 100644 --- a/flexbe_core/src/flexbe_core/behavior.py +++ b/flexbe_core/src/flexbe_core/behavior.py @@ -1,10 +1,5 @@ #!/usr/bin/env python import rospy -import smach_ros -import smach -import importlib -import sys -import string from flexbe_core import OperatableStateMachine, LockableStateMachine from flexbe_core.core import PreemptableState @@ -178,7 +173,7 @@ def execute(self): @return: A string containing the execution result such as finished or failed. """ PreemptableState.switching = False - result = self._state_machine.execute() + result = self._state_machine.spin() self._state_machine.destroy() @@ -194,7 +189,7 @@ def prepare_for_switch(self, state): """ states = self._get_states_of_path(state._get_path(), self._state_machine) if states is None: - raise smach.InvalidConstructionError("Did not find locked state in new behavior!") + raise RuntimeError("Did not find locked state in new behavior!") state_container = state._parent for sm in states[1:]: sm.set_initial_state([sm._initial_state_label], state_container.userdata) diff --git a/flexbe_core/src/flexbe_core/core/concurrency_container.py b/flexbe_core/src/flexbe_core/core/concurrency_container.py index 706c41e4..97aaaef8 100644 --- a/flexbe_core/src/flexbe_core/core/concurrency_container.py +++ b/flexbe_core/src/flexbe_core/core/concurrency_container.py @@ -1,15 +1,14 @@ #!/usr/bin/env python import traceback -import smach import rospy -from smach.state_machine import StateMachine - from flexbe_core.core.event_state import EventState from flexbe_core.core.operatable_state_machine import OperatableStateMachine from flexbe_core.core.preemptable_state import PreemptableState from flexbe_core.core.priority_container import PriorityContainer +from .user_data import UserData + class ConcurrencyContainer(OperatableStateMachine): """ @@ -22,6 +21,7 @@ def __init__(self, conditions=dict(), *args, **kwargs): self._conditions = conditions self._returned_outcomes = dict() self._do_rate_sleep = False + self._sleep_dur = None self.__execute = self.execute self.execute = self._concurrency_execute @@ -36,16 +36,19 @@ def _build_msg(self, *args, **kwargs): # use state machine _build_msg, not state execute return OperatableStateMachine._build_msg(self, *args, **kwargs) + def sleep(self): + if self._sleep_dur is not None: + rospy.sleep(self._sleep_dur) + self._sleep_dur = None - def _update_once(self): - # Check if a preempt was requested before or while the last state was running - if self.preempt_requested() or PreemptableState.preempt: - return self._preempted_name + @property + def sleep_duration(self): + return self._sleep_dur or 0. - #self._state_transitioning_lock.release() + def _update_once(self): sleep_dur = None - for state in self._ordered_states: - if state.name in list(self._returned_outcomes.keys()) and self._returned_outcomes[state.name] != self._loopback_name: + for state in self._states: + if state.name in list(self._returned_outcomes.keys()) and self._returned_outcomes[state.name] != self._loopback_name and self._returned_outcomes[state.name] is not None: continue if (PriorityContainer.active_container is not None and not all(a == s for a, s in zip(PriorityContainer.active_container.split('/'), @@ -55,23 +58,22 @@ def _update_once(self): elif state._get_deep_state() is not None: state._get_deep_state()._notify_skipped() continue - state_sleep_dur = state._rate.remaining().to_sec() + state_sleep_dur = state.sleep_duration if state_sleep_dur <= 0: sleep_dur = 0 self._returned_outcomes[state.name] = self._execute_state(state) # check again in case the sleep has already been handled by the child - if state._rate.remaining().to_sec() < 0: + if state.sleep_duration < 0: # this sleep returns immediately since sleep duration is negative, # but is required here to reset the sleep time after executing - state._rate.sleep() + state.sleep() else: if sleep_dur is None: sleep_dur = state_sleep_dur else: sleep_dur = min(sleep_dur, state_sleep_dur) if sleep_dur > 0: - rospy.sleep(sleep_dur) - #self._state_transitioning_lock.acquire() + self._sleep_dur = sleep_dur # Determine outcome outcome = self._loopback_name @@ -85,10 +87,9 @@ def _update_once(self): if outcome == self._loopback_name: return None - if outcome in self.get_registered_outcomes(): + if outcome in self.outcomes: # Call termination callbacks - self.call_termination_cbs([s.name for s in self._ordered_states],outcome) - self.exit(self.userdata, states = [s for s in self._ordered_states if s.name not in list(self._returned_outcomes.keys()) or self._returned_outcomes[s.name] == self._loopback_name]) + self.exit(self.userdata, states = [s for s in self._states if s.name not in list(self._returned_outcomes.keys()) or self._returned_outcomes[s.name] == self._loopback_name]) self._returned_outcomes = dict() # right now, going out of a cc may break sync # thus, as a quick fix, explicitly sync again @@ -97,49 +98,40 @@ def _update_once(self): return outcome else: - raise smach.InvalidTransitionError("Outcome '%s' of state '%s' with transition target '%s' is neither a registered state nor a registered container outcome." % + raise ValueError("Outcome '%s' of state '%s' with transition target '%s' is neither a registered state nor a registered container outcome." % (outcome, self.name, outcome)) def _execute_state(self, state, force_exit=False): result = None try: - ud = smach.Remapper( - self.userdata, - state.get_registered_input_keys(), - state.get_registered_output_keys(), - self._remappings[state.name]) + ud = UserData(reference=self.userdata, input_keys=state.input_keys, + output_keys=state.output_keys, remap=self._remappings[state.name]) if force_exit: state._entering = True state.on_exit(ud) else: result = state.execute(ud) - #print 'execute %s --> %s' % (state.name, self._returned_outcomes[state.name]) - except smach.InvalidUserCodeError as ex: - smach.logerr("State '%s' failed to execute." % state.name) - raise ex - except: - raise smach.InvalidUserCodeError("Could not execute state '%s' of type '%s': " % - (state.name, state) - + traceback.format_exc()) + except Exception: + raise RuntimeError("Could not execute state '%s' of type '%s': " % + (state.name, state) + traceback.format_exc()) return result - def _notify_start(self): - for state in self._ordered_states: + for state in self._states: if isinstance(state, EventState): state.on_start() if isinstance(state, OperatableStateMachine): state._notify_start() def _enable_ros_control(self): - state = self._ordered_states[0] + state = self._states[0] if isinstance(state, EventState): state._enable_ros_control() if isinstance(state, OperatableStateMachine): state._enable_ros_control() def _notify_stop(self): - for state in self._ordered_states: + for state in self._states: if isinstance(state, EventState): state.on_stop() if isinstance(state, OperatableStateMachine): @@ -148,12 +140,12 @@ def _notify_stop(self): state._disable_ros_control() def _disable_ros_control(self): - state = self._ordered_states[0] + state = self._states[0] if isinstance(state, EventState): state._disable_ros_control() if isinstance(state, OperatableStateMachine): state._disable_ros_control() def exit(self, userdata, states = None): - for state in self._ordered_states if states is None else states: + for state in self._states if states is None else states: self._execute_state(state, force_exit=True) \ No newline at end of file diff --git a/flexbe_core/src/flexbe_core/core/jumpable_state_machine.py b/flexbe_core/src/flexbe_core/core/jumpable_state_machine.py index f44d49c2..ce084b83 100644 --- a/flexbe_core/src/flexbe_core/core/jumpable_state_machine.py +++ b/flexbe_core/src/flexbe_core/core/jumpable_state_machine.py @@ -1,8 +1,9 @@ #!/usr/bin/env python import rospy -import smach import time +from .state_machine import StateMachine + from flexbe_msgs.msg import OutcomeRequest from ..proxy import ProxySubscriberCached @@ -33,7 +34,7 @@ def jump_to(self, target_state_label): """ # set current state and current container container = self - while isinstance(container._current_state, smach.StateMachine): + while isinstance(container._current_state, StateMachine): container = container._current_state current_state = container._current_state @@ -89,7 +90,7 @@ def _search_state(self, origin_state, target_state_label, container, seen_states target_path = None # enter the state machine (next lower hierarchy level) - if isinstance(origin_state, smach.StateMachine): + if isinstance(origin_state, StateMachine): # do not need to enter, inital state is implicitly active if origin_state._initial_state_label == target_state_label: return path diff --git a/flexbe_core/src/flexbe_core/core/lockable_state_machine.py b/flexbe_core/src/flexbe_core/core/lockable_state_machine.py index df4915e3..453a2a9b 100644 --- a/flexbe_core/src/flexbe_core/core/lockable_state_machine.py +++ b/flexbe_core/src/flexbe_core/core/lockable_state_machine.py @@ -1,9 +1,9 @@ #!/usr/bin/env python -import smach import rospy +from .state_machine import StateMachine -class LockableStateMachine(smach.StateMachine): +class LockableStateMachine(StateMachine): """ A state machine that can be locked. When locked, no transition can be done regardless of the resulting outcome. @@ -16,10 +16,7 @@ def __init__(self, *args, **kwargs): super(LockableStateMachine, self).__init__(*args, **kwargs) self._locked = False - self._parent = None - self.transitions = None - self.name = None self._is_controlled = False @@ -30,24 +27,23 @@ def _get_deep_state(self): @return: The current state (not state machine) """ container = self - while isinstance(container._current_state, smach.StateMachine): + while isinstance(container._current_state, StateMachine): container = container._current_state return container._current_state - - def _update_once(self): + + def execute(self, userdata): if LockableStateMachine.path_for_switch is not None and LockableStateMachine.path_for_switch.startswith(self._get_path()): path_segments = LockableStateMachine.path_for_switch.replace(self._get_path(), "", 1).split("/") wanted_state = path_segments[1] - self._set_current_state(wanted_state) + self._current_state(wanted_state) if len(path_segments) <= 2: LockableStateMachine.path_for_switch = None - #self._current_state._entering = False - return super(LockableStateMachine, self)._update_once() + return super(LockableStateMachine, self).execute(userdata) def _is_internal_transition(self, transition_target): - return transition_target in self._states + return transition_target in self._labels def transition_allowed(self, state, outcome): @@ -91,7 +87,7 @@ def is_locked_inside(self): for state in self._states: result = False - if isinstance(state, smach.StateMachine): + if isinstance(state, StateMachine): result = state.is_locked_inside() else: result = state.is_locked() @@ -108,7 +104,7 @@ def get_locked_state(self): for state in self._states: if state.is_locked(): return state - elif isinstance(state, smach.StateMachine): + elif isinstance(state, StateMachine): return state.get_locked_state() return None diff --git a/flexbe_core/src/flexbe_core/core/loopback_state.py b/flexbe_core/src/flexbe_core/core/loopback_state.py index 3f390208..453ab8ed 100644 --- a/flexbe_core/src/flexbe_core/core/loopback_state.py +++ b/flexbe_core/src/flexbe_core/core/loopback_state.py @@ -11,7 +11,6 @@ class LoopbackState(LockableState): _loopback_name = 'loopback' def __init__(self, *args, **kwargs): - self._rate = rospy.Rate(10) # add loopback outcome if len(args) > 0 and type(args[0]) is list: # need this ugly check for list type, because first argument in CBState is the callback @@ -22,6 +21,7 @@ def __init__(self, *args, **kwargs): kwargs['outcomes'] = outcomes super(LoopbackState, self).__init__(*args, **kwargs) + self._rate = rospy.Rate(10) self.__execute = self.execute self.execute = self._loopback_execute diff --git a/flexbe_core/src/flexbe_core/core/loopback_state_machine.py b/flexbe_core/src/flexbe_core/core/loopback_state_machine.py index 022b990a..4b27baa1 100644 --- a/flexbe_core/src/flexbe_core/core/loopback_state_machine.py +++ b/flexbe_core/src/flexbe_core/core/loopback_state_machine.py @@ -20,4 +20,4 @@ def __init__(self, *args, **kwargs): outcomes.append(self._loopback_name) kwargs['outcomes'] = outcomes - super(LoopbackStateMachine, self).__init__(*args, **kwargs) \ No newline at end of file + super(LoopbackStateMachine, self).__init__(*args, **kwargs) diff --git a/flexbe_core/src/flexbe_core/core/monitoring_state.py b/flexbe_core/src/flexbe_core/core/monitoring_state.py index e15c16ef..49b7c8ca 100644 --- a/flexbe_core/src/flexbe_core/core/monitoring_state.py +++ b/flexbe_core/src/flexbe_core/core/monitoring_state.py @@ -1,6 +1,5 @@ #!/usr/bin/env python import rospy -import smach from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached from flexbe_msgs.msg import CommandFeedback, OutcomeRequest @@ -8,8 +7,10 @@ from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus +from .state import State -class MonitoringState(smach.State): + +class MonitoringState(State): """ A state to monitor a custom set of conditions. For each conditions, an outcome is added or mapped which will be returned if the condition is not met. @@ -22,8 +23,6 @@ def __init__(self, *args, **kwargs): self._outcome_list = list(outcomes) self._outcome_list.remove('loopback') - self.name = None - self._parent = None self._is_controlled = False self._force_monitoring = False diff --git a/flexbe_core/src/flexbe_core/core/operatable_state_machine.py b/flexbe_core/src/flexbe_core/core/operatable_state_machine.py index b73b1de6..b862bd95 100644 --- a/flexbe_core/src/flexbe_core/core/operatable_state_machine.py +++ b/flexbe_core/src/flexbe_core/core/operatable_state_machine.py @@ -1,14 +1,14 @@ #!/usr/bin/env python import rospy import zlib -import smach from rospy.exceptions import ROSInterruptException from flexbe_core.core.preemptable_state_machine import PreemptableStateMachine from flexbe_core.core.lockable_state_machine import LockableStateMachine -from smach.state_machine import StateMachine +from .state_machine import StateMachine +from .user_data import UserData from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached from flexbe_msgs.msg import Container, ContainerStructure, OutcomeRequest, BehaviorSync, CommandFeedback, BehaviorLog @@ -45,99 +45,6 @@ def __init__(self, *args, **kwargs): self._sub = ProxySubscriberCached() - def execute(self, parent_ud = smach.UserData()): - # First time in this state machine - if self._current_state is None: - # Spew some info - smach.loginfo("State machine starting in initial state '%s' with userdata: \n\t%s" % - (self._current_label, list(self.userdata.keys()))) - # Set initial state - self._set_current_state(self._initial_state_label) - - # Initialize preempt state - self._preempted_label = None - self._preempted_state = None - - # Call start callbacks - self.call_start_cbs() - - # Copy input keys - self._copy_input_keys(parent_ud, self.userdata) - - container_outcome = self._loopback_name - if self.id is not None: - # only top-level statemachine should loop for outcome - while container_outcome == self._loopback_name and not smach.is_shutdown(): - # Update the state machine - container_outcome = self._async_execute(parent_ud) - else: - container_outcome = self._async_execute(parent_ud) - - if smach.is_shutdown(): - container_outcome = 'preempted' - - return container_outcome - - - def _async_execute(self, parent_ud = smach.UserData()): - """Run the state machine on entry to this state. - This will set the "closed" flag and spin up the execute thread. Once - this flag has been set, it will prevent more states from being added to - the state machine. - """ - - # This will prevent preempts from getting propagated to non-existent children - with self._state_transitioning_lock: - # Check state consistency - try: - self.check_consistency() - except (smach.InvalidStateError, smach.InvalidTransitionError): - smach.logerr("Container consistency check failed.") - return None - - # Set running flag - self._is_running = True - - # Initialize container outcome - container_outcome = None - - # Step through state machine - if self._is_running and not smach.is_shutdown(): - # Update the state machine - container_outcome = self._update_once() - if self._do_rate_sleep and self._current_state is not None and not isinstance(self._current_state, OperatableStateMachine): - try: - # sleep with the rate of the state and update container rate accordingly - self._rate = self._current_state._rate - self._rate.sleep() - except ROSInterruptException: - rospy.logwarn('Interrupted rate sleep.') - - if container_outcome is not None and container_outcome != self._loopback_name: - # Copy output keys - self._copy_output_keys(self.userdata, parent_ud) - else: - container_outcome = self._loopback_name - - # provide explicit sync as back-up functionality - # should be used only if there is no other choice - # since it requires additional 8 byte + header update bandwith and time to restart mirror - if self._inner_sync_request and self._get_deep_state() is not None: - self._inner_sync_request = False - if self.id is None: - self._parent._inner_sync_request = True - else: - msg = BehaviorSync() - msg.behavior_id = self.id - msg.current_state_checksum = zlib.adler32(self._get_deep_state()._get_path()) - self._pub.publish('flexbe/mirror/sync', msg) - - # We're no longer running - self._is_running = False - - return container_outcome - - @staticmethod def add(label, state, transitions = None, autonomy = None, remapping = None): """ @@ -157,7 +64,7 @@ def add(label, state, transitions = None, autonomy = None, remapping = None): @param remapping: A dictionary mapping local userdata keys to userdata keys in the container. """ - self = StateMachine._currently_opened_container() + self = StateMachine._currently_opened_container # add loopback transition to loopback states if isinstance(state, LoopbackState): @@ -168,10 +75,8 @@ def add(label, state, transitions = None, autonomy = None, remapping = None): autonomy[OperatableStateMachine._loopback_name] = -1 self._ordered_states.append(state) - state.name = label state.transitions = transitions state.autonomy = autonomy - state._parent = self StateMachine.add(label, state, transitions, remapping) self._autonomy[label] = autonomy @@ -205,7 +110,7 @@ def confirm(self, name, id): @type name: string @param name: The name of this state machine to identify it. """ - self.name = name + self.set_name(name) self.id = id self._pub.createPublisher('flexbe/mirror/sync', BehaviorSync, _latch = True) # Update mirror with currently active state (high bandwidth mode) @@ -364,12 +269,9 @@ def _disable_ros_control(self): def on_exit(self, userdata): if self._current_state is not None: - ud = smach.Remapper( - self.userdata, - self._current_state.get_registered_input_keys(), - self._current_state.get_registered_output_keys(), - self._remappings[self._current_state.name] - ) + ud = UserData(reference=self.userdata, input_keys=self._current_state.input_keys, + output_keys=self._current_state.output_keys, + remap=self._remappings[self._current_state.name]) self._current_state._entering = True self._current_state.on_exit(ud) - self._current_state = None \ No newline at end of file + self._current_state = None diff --git a/flexbe_core/src/flexbe_core/core/priority_container.py b/flexbe_core/src/flexbe_core/core/priority_container.py index 92d0fa78..b18bf0c4 100644 --- a/flexbe_core/src/flexbe_core/core/priority_container.py +++ b/flexbe_core/src/flexbe_core/core/priority_container.py @@ -1,8 +1,5 @@ #!/usr/bin/env python import traceback -import smach - -from smach.state_machine import StateMachine from flexbe_core.core.operatable_state_machine import OperatableStateMachine @@ -27,7 +24,7 @@ def execute(self, *args, **kwargs): outcome = OperatableStateMachine.execute(self, *args, **kwargs) - if outcome != self._loopback_name: + if outcome != self._loopback_name and outcome is not None: PriorityContainer.active_container = self._parent_active_container return outcome diff --git a/flexbe_core/src/flexbe_core/core/silent_state_machine.py b/flexbe_core/src/flexbe_core/core/silent_state_machine.py index aca286cb..d0d55ba7 100644 --- a/flexbe_core/src/flexbe_core/core/silent_state_machine.py +++ b/flexbe_core/src/flexbe_core/core/silent_state_machine.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -from smach.state_machine import StateMachine +from .state_machine import StateMachine from .preemptable_state_machine import PreemptableStateMachine from flexbe_core.core.loopback_state import LoopbackState @@ -14,10 +14,8 @@ class SilentStateMachine(PreemptableStateMachine): def __init__(self, *args, **kwargs): super(SilentStateMachine, self).__init__(*args, **kwargs) - self.name = None self.transitions = None self.autonomy = None - self._parent = None @staticmethod def add(label, state, transitions = None, autonomy = None, remapping = None): @@ -38,7 +36,7 @@ def add(label, state, transitions = None, autonomy = None, remapping = None): @param remapping: A dictrionary mapping local userdata keys to userdata keys in the container. """ - self = StateMachine._currently_opened_container() + self = StateMachine._currently_opened_container # add loopback transition to loopback states if isinstance(state, LoopbackState): @@ -46,10 +44,8 @@ def add(label, state, transitions = None, autonomy = None, remapping = None): StateMachine.add(label, state, transitions, remapping) - state.name = label state.transitions = transitions state.autonomy = None - state._parent = self state._mute = True def destroy(self): diff --git a/flexbe_core/src/flexbe_core/core/state.py b/flexbe_core/src/flexbe_core/core/state.py new file mode 100644 index 00000000..d6e2e7c8 --- /dev/null +++ b/flexbe_core/src/flexbe_core/core/state.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python + + +class State(object): + + def __init__(self, *args, **kwargs): + self._outcomes = set(kwargs.get('outcomes', [])) + io_keys = kwargs.get('io_keys', []) + self._input_keys = set(kwargs.get('input_keys', []) + io_keys) + self._output_keys = set(kwargs.get('output_keys', []) + io_keys) + self._rate = None + # properties of instances of a state machine + self._name = None + self._parent = None + + def execute(self, userdata): + pass + + def sleep(self): + if self._rate is not None: + self._rate.sleep() + + # instance properties + + @property + def name(self): + return self._name + + def set_name(self, value): + if self._name is not None: + raise ValueError("Cannot change the name of a state!") + else: + self._name = value + + @property + def parent(self): + return self._parent + + def set_parent(self, value): + if self._parent is not None: + raise ValueError("Cannot change the parent of a state!") + else: + self._parent = value + + @property + def outcomes(self): + return self._outcomes + + @property + def input_keys(self): + return self._input_keys + + @property + def output_keys(self): + return self._output_keys + + @property + def sleep_duration(self): + if self._rate is not None: + return self._rate.remaining().to_sec() + else: + return 0. diff --git a/flexbe_core/src/flexbe_core/core/state_machine.py b/flexbe_core/src/flexbe_core/core/state_machine.py new file mode 100644 index 00000000..455969dd --- /dev/null +++ b/flexbe_core/src/flexbe_core/core/state_machine.py @@ -0,0 +1,113 @@ +#!/usr/bin/env python +from .state import State +from .user_data import UserData + + +class StateMachine(State): + + _currently_opened_container = None + + def __init__(self, *args, **kwargs): + super(StateMachine, self).__init__(*args, **kwargs) + self._states = list() + self._labels = dict() + self._transitions = dict() + self._remappings = dict() + self._current_state = None + self._userdata = UserData() + self._previously_opened_container = None + + def __enter__(self): + self._previously_opened_container = StateMachine._currently_opened_container + StateMachine._currently_opened_container = self + + def __exit__(self, *args): + StateMachine._currently_opened_container = self._previously_opened_container + self._previously_opened_container = None + + # construction + + @staticmethod + def add(label, state, transitions, remappings): + self = StateMachine._currently_opened_container + if label in self._labels: + raise ValueError("The label %s has already been added to this state machine!" % label) + if label in self._outcomes: + raise ValueError("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 + self._transitions[label] = transitions + self._remappings[label] = remappings + # update state instance + state.set_name(label) + state.set_parent(self) + + # execution + + def spin(self, userdata=None): + self._userdata = userdata or self._userdata + outcome = None + while True: + outcome = self.execute(self._userdata) + if outcome is not None: + break + self.sleep() + return outcome + + def execute(self, userdata): + if self._current_state is None: + self._current_state = self.initial_state + self._userdata = userdata + outcome = self._update_once() + return outcome + + def sleep(self): + if self._current_state is not None: + self._current_state.sleep() + + def _update_once(self): + outcome = self._current_state.execute( + UserData(reference=self.userdata, input_keys=self._current_state.input_keys, + output_keys=self._current_state.output_keys, remap=self._remappings[self._current_state.name]) + ) + if outcome is not None: + target = self._transitions[self._current_state.name][outcome] + self._current_state = self._labels.get(target) + if self._current_state is None and target in self._outcomes: + return target + + # properties + + @property + def userdata(self): + return self._userdata + + @property + def current_state(self): + if self._current_state is not None: + return self._current_state + else: + raise ValueError("No state active!") + + @property + def current_state_label(self): + return self.current_state.name + + @property + def initial_state(self): + if len(self._states) > 0: + return self._states[0] + else: + raise ValueError("No states added yet!") + + @property + def initial_state_label(self): + return self.initial_state.name + + @property + def sleep_duration(self): + if self._current_state is not None: + return self._current_state.sleep_duration + else: + return 0. diff --git a/flexbe_core/src/flexbe_core/core/user_data.py b/flexbe_core/src/flexbe_core/core/user_data.py new file mode 100644 index 00000000..4a8ffb0e --- /dev/null +++ b/flexbe_core/src/flexbe_core/core/user_data.py @@ -0,0 +1,36 @@ +#!/user/bin/env python + + +class UserData(object): + + def __init__(self, reference=None, input_keys=None, output_keys=None, remap=None): + self._data = reference._data if reference is not None else dict() + self._input_keys = input_keys + self._output_keys = output_keys + self._remap = remap or dict() + + def __contains__(self, key): + if self._input_keys is not None and key not in self._input_keys: + return False + else: + return self._remap.get(key, key) in self._data + + def __getitem__(self, key): + if key not in self: + raise KeyError("Key %s not contained, make sure it is defined as input key." % key) + return self._data[self._remap.get(key, key)] + + def __setitem__(self, key, value): + if self._output_keys is not None and key not in self._output_keys: + raise KeyError("Key %s cannot be set, make sure it is defined as output key." % key) + self._data[self._remap.get(key, key)] = value + + def __getattr__(self, key): + if key.startswith('_'): + return object.__getattr__(self, key) + return self[key] + + def __setattr__(self, key, value): + if key.startswith('_'): + return object.__setattr__(self, key, value) + self[key] = value diff --git a/flexbe_input/package.xml b/flexbe_input/package.xml index ec6a5f14..7941023c 100644 --- a/flexbe_input/package.xml +++ b/flexbe_input/package.xml @@ -15,7 +15,6 @@ actionlib actionlib - smach_ros rospy flexbe_msgs python-six diff --git a/flexbe_mirror/package.xml b/flexbe_mirror/package.xml index 6acd3406..f41efc27 100644 --- a/flexbe_mirror/package.xml +++ b/flexbe_mirror/package.xml @@ -12,7 +12,6 @@ catkin - smach_ros rospy flexbe_core flexbe_msgs diff --git a/flexbe_mirror/src/flexbe_mirror/flexbe_mirror.py b/flexbe_mirror/src/flexbe_mirror/flexbe_mirror.py index eb3ddf32..03a49c87 100644 --- a/flexbe_mirror/src/flexbe_mirror/flexbe_mirror.py +++ b/flexbe_mirror/src/flexbe_mirror/flexbe_mirror.py @@ -2,7 +2,6 @@ import roslib; roslib.load_manifest('flexbe_mirror') import rospy -import smach import threading import zlib @@ -30,13 +29,6 @@ def __init__(self): Constructor ''' self._sm = None - - smach.set_loggers ( - rospy.logdebug, # hide SMACH transition log spamming - rospy.logwarn, - rospy.logdebug, - rospy.logerr - ) # set up proxys for sm <--> GUI communication # publish topics @@ -197,7 +189,7 @@ def _restart_mirror(self, msg): try: self._mirror_state_machine(self._current_struct) rospy.loginfo('Mirror built.') - except (AttributeError, smach.InvalidStateError): + except (AttributeError, RuntimeError): rospy.loginfo('Stopping synchronization because behavior has stopped.') self._execute_mirror() @@ -214,7 +206,7 @@ def _execute_mirror(self): result = 'preempted' try: - result = self._sm.execute() + result = self._sm.spin() except Exception as e: rospy.loginfo('Caught exception on preempt:\n%s' % str(e)) result = 'preempted' diff --git a/flexbe_msgs/package.xml b/flexbe_msgs/package.xml index f8e67abc..6c7b3339 100644 --- a/flexbe_msgs/package.xml +++ b/flexbe_msgs/package.xml @@ -19,7 +19,6 @@ actionlib actionlib_msgs message_runtime - smach_ros rospy diff --git a/flexbe_onboard/package.xml b/flexbe_onboard/package.xml index 2f9804d6..e156f7e0 100644 --- a/flexbe_onboard/package.xml +++ b/flexbe_onboard/package.xml @@ -14,7 +14,6 @@ rostest - smach_ros rospy flexbe_core flexbe_msgs diff --git a/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py b/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py index 90fe6a1e..55095aef 100644 --- a/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py +++ b/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py @@ -6,7 +6,6 @@ import tempfile import threading import time -import smach import zlib from ast import literal_eval as cast @@ -25,8 +24,6 @@ class FlexbeOnboard(object): def __init__(self): self.be = None Logger.initialize() - # hide SMACH transition log spamming - smach.set_loggers(rospy.logdebug, rospy.logwarn, rospy.logdebug, rospy.logerr) # prepare temp folder self._tmp_folder = tempfile.mkdtemp() diff --git a/flexbe_states/package.xml b/flexbe_states/package.xml index 55abe343..638eeca4 100644 --- a/flexbe_states/package.xml +++ b/flexbe_states/package.xml @@ -15,7 +15,6 @@ rostest - smach_ros rospy rosbag flexbe_core diff --git a/flexbe_testing/package.xml b/flexbe_testing/package.xml index 43213437..3dc3ed36 100644 --- a/flexbe_testing/package.xml +++ b/flexbe_testing/package.xml @@ -15,7 +15,6 @@ rostest rospy - smach_ros flexbe_core flexbe_msgs diff --git a/flexbe_testing/src/flexbe_testing/tester.py b/flexbe_testing/src/flexbe_testing/tester.py index adbb8804..0ae6d967 100644 --- a/flexbe_testing/src/flexbe_testing/tester.py +++ b/flexbe_testing/src/flexbe_testing/tester.py @@ -4,15 +4,13 @@ import unittest import re +from flexbe_core.core.user_data import UserData + from .logger import Logger from .test_interface import TestInterface from .test_context import TestContext, LaunchContext from .data_provider import DataProvider -import smach -# hide SMACH transition log spamming -smach.set_loggers(rospy.logdebug, rospy.logwarn, rospy.logdebug, rospy.logerr) - class Tester(object): @@ -81,7 +79,7 @@ def run_test(self, name, config): return 0 # prepare user data - userdata = smach.UserData() + userdata = UserData() for input_key, input_value in list(config.get('input', dict()).items()): userdata[input_key] = data.parse(input_value) expected = {key: data.parse(value) for key, value in config.get('output', dict()).items()} diff --git a/flexbe_widget/package.xml b/flexbe_widget/package.xml index b2cf4099..4ee8cbf6 100644 --- a/flexbe_widget/package.xml +++ b/flexbe_widget/package.xml @@ -12,7 +12,6 @@ catkin - smach_ros rospy flexbe_core flexbe_msgs From 163cc66f6e71ad935dfed570371fe93e92544def Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Fri, 3 Apr 2020 11:58:50 +0200 Subject: [PATCH 22/57] Major clean-up of most core components --- flexbe_core/src/flexbe_core/__init__.py | 2 +- flexbe_core/src/flexbe_core/behavior.py | 159 ++++------ .../src/flexbe_core/behavior_library.py | 272 ++++++++---------- flexbe_core/src/flexbe_core/core/__init__.py | 7 +- .../flexbe_core/core/concurrency_container.py | 98 +++---- .../src/flexbe_core/core/event_state.py | 66 ++--- .../core/jumpable_state_machine.py | 193 ------------- .../src/flexbe_core/core/lockable_state.py | 61 ++-- .../core/lockable_state_machine.py | 75 +++-- .../src/flexbe_core/core/loopback_state.py | 46 --- .../core/loopback_state_machine.py | 23 -- .../core/manually_transitionable_state.py | 32 +-- .../src/flexbe_core/core/monitoring_state.py | 97 ------- .../src/flexbe_core/core/operatable_state.py | 123 ++------ .../core/operatable_state_machine.py | 263 +++++++---------- .../src/flexbe_core/core/preemptable_state.py | 44 +-- .../core/preemptable_state_machine.py | 30 +- .../flexbe_core/core/priority_container.py | 12 +- flexbe_core/src/flexbe_core/core/ros_state.py | 49 ++++ .../src/flexbe_core/core/ros_state_machine.py | 38 +++ .../flexbe_core/core/silent_state_machine.py | 52 ---- flexbe_core/src/flexbe_core/core/state.py | 45 ++- .../src/flexbe_core/core/state_machine.py | 25 +- flexbe_core/src/flexbe_core/logger.py | 34 ++- flexbe_core/src/flexbe_core/proxy.py | 5 - flexbe_core/src/flexbe_core/state_logger.py | 4 +- .../src/flexbe_mirror/flexbe_mirror.py | 37 +-- .../src/flexbe_mirror/mirror_state.py | 48 +--- .../src/flexbe_onboard/flexbe_onboard.py | 2 +- .../src/flexbe_states/decision_state.py | 4 +- .../src/flexbe_testing/test_interface.py | 15 +- 31 files changed, 649 insertions(+), 1312 deletions(-) delete mode 100644 flexbe_core/src/flexbe_core/core/jumpable_state_machine.py delete mode 100644 flexbe_core/src/flexbe_core/core/loopback_state.py delete mode 100644 flexbe_core/src/flexbe_core/core/loopback_state_machine.py delete mode 100644 flexbe_core/src/flexbe_core/core/monitoring_state.py create mode 100644 flexbe_core/src/flexbe_core/core/ros_state.py create mode 100644 flexbe_core/src/flexbe_core/core/ros_state_machine.py delete mode 100644 flexbe_core/src/flexbe_core/core/silent_state_machine.py delete mode 100644 flexbe_core/src/flexbe_core/proxy.py diff --git a/flexbe_core/src/flexbe_core/__init__.py b/flexbe_core/src/flexbe_core/__init__.py index a45787d6..56c380dc 100644 --- a/flexbe_core/src/flexbe_core/__init__.py +++ b/flexbe_core/src/flexbe_core/__init__.py @@ -8,7 +8,7 @@ # from .core import EventState -from .core import OperatableStateMachine, JumpableStateMachine, SilentStateMachine, LockableStateMachine, ConcurrencyContainer, PriorityContainer +from .core import OperatableStateMachine, ConcurrencyContainer, PriorityContainer from .behavior import Behavior diff --git a/flexbe_core/src/flexbe_core/behavior.py b/flexbe_core/src/flexbe_core/behavior.py index c0224977..4536f8a4 100644 --- a/flexbe_core/src/flexbe_core/behavior.py +++ b/flexbe_core/src/flexbe_core/behavior.py @@ -1,14 +1,7 @@ #!/usr/bin/env python -import rospy +from flexbe_core.logger import Logger +from flexbe_core.core import PreemptableState, OperatableStateMachine, LockableStateMachine -from flexbe_core import OperatableStateMachine, LockableStateMachine -from flexbe_core.core import PreemptableState - -''' -Created on 20.05.2013 - -@author: Philipp Schillinger -''' class Behavior(object): ''' @@ -22,38 +15,37 @@ def __init__(self): self._state_machine = None self.name = "unnamed behavior" self.id = 0 - + self.contains = {} self._behaviors = {} - + self._autonomy_level = 3 self._debug = False self.requested_state_path = None - - + # Please implement those: - + def create(self): """ Should create the state machine for this behavior and return it. - It is called immediately before executing the behavior, so used parameters will have their final value when called. - + It is called immediately before executing the behavior, + so used parameters will have their final value when called. + @return The complete state machine for this behavior. """ pass - - + # Use those if you need them: - + def add_parameter(self, name, default): """ Adds a parameter to this behavior. The parameter should be declared in the behavior manifest. - + @type name: string @param name: The name of the parameter. - + @type default: object @param default: The default value of this parameter. Be sure to set it to the right type. """ @@ -63,51 +55,52 @@ def add_behavior(self, behavior_class, behavior_id): """ Adds another behavior as part of this behavior. This other behavior should be declared as contained in the behavior manifest. - + @type behavior_class: class @param behavior_class: The class implementing the other behavior. - + @type behavior_id: string @param behavior_id: Unique identifier for this behavior instance. """ if not hasattr(self, 'contains'): - rospy.logerr('Behavior was not initialized! Please call superclass constructor.') - + Logger.logerr('Behavior was not initialized! Please call superclass constructor.') instance = behavior_class() self.contains[behavior_id] = instance - + def use_behavior(self, behavior_class, behavior_id, default_keys=None, parameters=None): """ Creates a state machine implementing the given behavior to use it in the behavior state machine. Behavior has to be added first. - + @type behavior_class: class @param behavior_class: The class implementing the other behavior. - + @type behavior_id: string @param behavior_id: Same identifier as used for adding. - + @type default_keys: list - @param default_keys: List of input keys of the behavior which should be ignored and instead use the default values as given by the behavior. - + @param default_keys: List of input keys of the behavior which should be ignored + and instead use the default values as given by the behavior. + @type parameters: dict - @param parameters: Optional assignment of values to behavior parameters. Any assigned parameter will be ignored for runtime customization, i.e., cannot be overwritten by a user who runs the behavior. + @param parameters: Optional assignment of values to behavior parameters. + Any assigned parameter will be ignored for runtime customization, + i.e., cannot be overwritten by a user who runs the behavior. """ if behavior_id not in self.contains: - rospy.logerr('Tried to use not added behavior!') + Logger.logerr('Tried to use a behavior without adding it!') return None if parameters is not None: for parameter, value in parameters.items(): setattr(self.contains[behavior_id], parameter, value) - + state_machine = self.contains[behavior_id]._get_state_machine() if default_keys is not None: state_machine._input_keys = list(set(state_machine._input_keys) - set(default_keys)) - - return state_machine + return state_machine # Lifecycle @@ -118,16 +111,13 @@ def prepare_for_execution(self, input_data=None): OperatableStateMachine.autonomy_level = self._autonomy_level self._state_machine = self.create() - self._state_machine._input_keys = {} - self._state_machine._output_keys = {} if input_data is None: input_data = dict() for k, v in input_data.items(): - if k in self._state_machine.userdata: + if k in self._state_machine.userdata: self._state_machine.userdata[k] = v - def set_parameter(self, name, value): """ Set the given parameter of this behavior or any sub-behavior. @@ -155,7 +145,6 @@ def set_parameter(self, name, value): found = True return found - def confirm(self): """ Confirms that this behavior is ready for execution. @@ -163,49 +152,44 @@ def confirm(self): LockableStateMachine.path_for_switch = self.requested_state_path self._state_machine.confirm(self.name, self.id) - - + def execute(self): """ Called when the behavior is executed. Need to call self.execute_behavior when ready to start the state machine and return its result. - + @return: A string containing the execution result such as finished or failed. """ - PreemptableState.switching = False result = self._state_machine.spin() - self._state_machine.destroy() - return result - def prepare_for_switch(self, state): """ Prepares the behavior for being executed after a behavior switch. - + @type name: string @param name: The name of this behavior. """ - states = self._get_states_of_path(state._get_path(), self._state_machine) + 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!") state_container = state._parent + state_container.remove_state(state) # remove from old state machine for sm in states[1:]: - sm.set_initial_state([sm._initial_state_label], state_container.userdata) - #rospy.loginfo("Set userdata for %s from %s; (keys: %d): %s", str(sm.name), str(state_container.name), len(state_container.userdata._data), str(state_container.userdata._data)) + # update userdata in new state machine + sm.replace_userdata(state_container.userdata) state_container = state_container._parent - states[1].replace(state) - - self.requested_state_path = state._get_path() - + states[1].replace_state(state) # add to new state machine + self.requested_state_path = state.path # set start after switch def get_current_state(self): - return self._state_machine._get_deep_state() + return self._state_machine.get_deep_state() def get_locked_state(self): - state = self._state_machine._get_deep_state() - while not state is None: + state = self._state_machine.get_deep_state() + while state is not None: if state.is_locked(): return state else: @@ -215,30 +199,12 @@ def get_locked_state(self): def preempt(self): PreemptableState.preempt = True - def preempt_for_switch(self): - PreemptableState.switching = True - PreemptableState.preempt = True - - # For internal use only - + def _get_state_machine(self): if self._state_machine is None: self._state_machine = self.create() return self._state_machine - - def _get_muted_state_machine(self): - if self._state_machine is None: - self._state_machine = self.create() - self._mute_state_machine(self._state_machine) - return self._state_machine - - def _mute_state_machine(self, sm): - for state in sm._ordered_states: - if isinstance(state, OperatableStateMachine): - self._mute_state_machine(state) - else: - state._is_controlled = False def _collect_contained(self, obj, path): contain_list = {path+"/"+key: value for (key, value) in getattr(obj, 'contains', {}).items()} @@ -274,44 +240,15 @@ def set_up(self, id, autonomy_level, debug): def _get_states_of_path(self, path, container): path_elements = path.split('/') if len(path_elements) < 2: - return [container] + return [container] # actually a state in this case state_label = path_elements[1] new_path = "/".join(path_elements[1:]) - - if state_label in container.get_children(): - childlist = self._get_states_of_path(new_path, container.get_children()[state_label]) + # collect along the path and append self + if state_label in container: + childlist = self._get_states_of_path(new_path, container[state_label]) if childlist is None: return None childlist.append(container) return childlist else: return None - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/flexbe_core/src/flexbe_core/behavior_library.py b/flexbe_core/src/flexbe_core/behavior_library.py index 560dbd57..6e9b1a58 100644 --- a/flexbe_core/src/flexbe_core/behavior_library.py +++ b/flexbe_core/src/flexbe_core/behavior_library.py @@ -1,160 +1,130 @@ #!/usr/bin/env python import os -import rospy from rospkg import RosPack import xml.etree.ElementTree as ET import zlib +from flexbe_core.logger import Logger -''' -Created on 10.01.2017 - -@author: Philipp Schillinger -''' class BehaviorLibrary(object): - ''' - Provides access to all known behaviors. - ''' - - def __init__(self): - self._rp = RosPack() - self._behavior_lib = dict() - self.parse_packages() - - - def parse_packages(self): - """ - Parses all ROS packages to update the internal behavior library. - """ - self._behavior_lib = dict() - for pkg in self._rp.list(): - for export in self._rp._load_manifest(pkg).exports: - if export.tag == "flexbe_behaviors": - self._add_behavior_manifests(self._rp.get_path(pkg), pkg) - - - def _add_behavior_manifests(self, path, pkg=None): - """ - Recursively add all behavior manifests in the given folder to the internal library. - If a package name is specified, only manifests referring to this package are added. - - @type path: string - @param path: Path of the folder to be traversed. - - @type pkg: string - @param pkg: Optional name of a package to only add manifests referring to this package. - """ - for entry in os.listdir(path): - 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("#"): - m = ET.parse(entry_path).getroot() - # structure sanity check - if m.tag != "behavior" \ - or len(m.findall(".//executable")) == 0 \ - or m.find("executable").get("package_path") is None \ - or len(m.find("executable").get("package_path").split(".")) != 2: - continue; - e = m.find("executable") - if pkg is not None and e.get("package_path").split(".")[0] != pkg: - continue # ignore if manifest not in specified package - be_id = zlib.adler32(e.get("package_path")) - self._behavior_lib[be_id] = { - "name": m.get("name"), - "package": e.get("package_path").split(".")[0], - "file": e.get("package_path").split(".")[1], - "class": e.get("class") - } - - - def get_behavior(self, be_id): - """ - Provides the library entry corresponding to the given ID. - - @type be_id: int - @param be_id: Behavior ID to look up. - - @return Corresponding library entry or None if not found. - """ - try: - return self._behavior_lib[be_id] - except KeyError: - rospy.logwarn("Did not find ID %d in libary, updating..." % be_id) - self.parse_packages() - return self._behavior_lib.get(be_id, None) - - - def find_behavior(self, be_name): - """ - Searches for a behavior with the given name and returns it along with its ID. - - @type be_name: string - @param be_name: Behavior ID to look up. - - @return Tuple (be_id, be_entry) corresponding to the name or (None, None) if not found. - """ - find = lambda: next((id, be) for (id, be) in self._behavior_lib.items() if be["name"] == be_name) - try: - return find() - except StopIteration: - rospy.logwarn("Did not find behavior '%s' in libary, updating..." % be_name) - self.parse_packages() - return find() - - - def count_behaviors(self): - """ - Counts the available behaviors. - - @return Number of behaviors. - """ - return len(self._behavior_lib) - - - def get_sourcecode_filepath(self, be_id, add_tmp=False): - """ - Constructs a file path to the source code of corresponding to the given ID. - - @type be_id: int - @param be_id: Behavior ID to look up. - - @type add_tmp: bool - @param add_tmp: Append "_tmp" to the file to consider a temporary version. - - @return String containing the absolute path to the source code file. - """ - be_entry = self.get_behavior(be_id) - if be_entry is None: - # rely on get_behavior to handle/log missing package - return None - try: - module_path = __import__(be_entry["package"]).__path__[-1] - except ImportError: - rospy.logwarn("Cannot import behavior package '%s', using 'rospack find' instead" % be_entry["package"]) - # rp can find it because otherwise, the above entry would not exist - module_path = os.path.join(self._rp.get_path(be_entry["package"]), "src", be_entry["package"]) - filename = be_entry["file"] + '.py' if not add_tmp else '_tmp.py' - return os.path.join(module_path, filename) - - - - - - - - - - - - - - - - - - - - - + ''' + Provides access to all known behaviors. + ''' + + def __init__(self): + self._rp = RosPack() + self._behavior_lib = dict() + self.parse_packages() + + def parse_packages(self): + """ + Parses all ROS packages to update the internal behavior library. + """ + self._behavior_lib = dict() + for pkg in self._rp.list(): + for export in self._rp._load_manifest(pkg).exports: + if export.tag == "flexbe_behaviors": + self._add_behavior_manifests(self._rp.get_path(pkg), pkg) + + def _add_behavior_manifests(self, path, pkg=None): + """ + Recursively add all behavior manifests in the given folder to the internal library. + If a package name is specified, only manifests referring to this package are added. + + @type path: string + @param path: Path of the folder to be traversed. + + @type pkg: string + @param pkg: Optional name of a package to only add manifests referring to this package. + """ + for entry in os.listdir(path): + 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("#"): + m = ET.parse(entry_path).getroot() + # structure sanity check + if (m.tag != "behavior" + or len(m.findall(".//executable")) == 0 + or m.find("executable").get("package_path") is None + or len(m.find("executable").get("package_path").split(".")) < 2): + continue + e = m.find("executable") + if pkg is not None and e.get("package_path").split(".")[0] != pkg: + continue # ignore if manifest not in specified package + be_id = zlib.adler32(e.get("package_path")) + self._behavior_lib[be_id] = { + "name": m.get("name"), + "package": ".".join(e.get("package_path").split(".")[:-1]), + "file": e.get("package_path").split(".")[-1], + "class": e.get("class") + } + + def get_behavior(self, be_id): + """ + Provides the library entry corresponding to the given ID. + + @type be_id: int + @param be_id: Behavior ID to look up. + + @return Corresponding library entry or None if not found. + """ + try: + return self._behavior_lib[be_id] + except KeyError: + Logger.logwarn("Did not find ID %d in libary, updating..." % be_id) + self.parse_packages() + return self._behavior_lib.get(be_id, None) + + def find_behavior(self, be_name): + """ + Searches for a behavior with the given name and returns it along with its ID. + + @type be_name: string + @param be_name: Behavior ID to look up. + + @return Tuple (be_id, be_entry) corresponding to the name or (None, None) if not found. + """ + find = lambda: next((id, be) for (id, be) # noqa: E731 (allow lambda, only used locally here) + in self._behavior_lib.items() + if be["name"] == be_name) + try: + return find() + except StopIteration: + Logger.logwarn("Did not find behavior '%s' in libary, updating..." % be_name) + self.parse_packages() + return find() + + def count_behaviors(self): + """ + Counts the available behaviors. + + @return Number of behaviors. + """ + return len(self._behavior_lib) + + def get_sourcecode_filepath(self, be_id, add_tmp=False): + """ + Constructs a file path to the source code of corresponding to the given ID. + + @type be_id: int + @param be_id: Behavior ID to look up. + + @type add_tmp: bool + @param add_tmp: Append "_tmp" to the file to consider a temporary version. + + @return String containing the absolute path to the source code file. + """ + be_entry = self.get_behavior(be_id) + if be_entry is None: + # rely on get_behavior to handle/log missing package + return None + try: + module_path = __import__(be_entry["package"]).__path__[-1] + except ImportError: + Logger.logwarn("Cannot import behavior package '%s', using 'rospack find' instead" % be_entry["package"]) + # rp can find it because otherwise, the above entry would not exist + module_path = os.path.join(self._rp.get_path(be_entry["package"]), "src", be_entry["package"]) + filename = be_entry["file"] + '.py' if not add_tmp else '_tmp.py' + return os.path.join(module_path, filename) diff --git a/flexbe_core/src/flexbe_core/core/__init__.py b/flexbe_core/src/flexbe_core/core/__init__.py index 71102b3a..db0ada98 100644 --- a/flexbe_core/src/flexbe_core/core/__init__.py +++ b/flexbe_core/src/flexbe_core/core/__init__.py @@ -1,15 +1,12 @@ -import roslib; roslib.load_manifest('flexbe_core') - from .preemptable_state_machine import PreemptableStateMachine -from .silent_state_machine import SilentStateMachine -from .jumpable_state_machine import JumpableStateMachine from .operatable_state_machine import OperatableStateMachine from .lockable_state_machine import LockableStateMachine from .concurrency_container import ConcurrencyContainer from .priority_container import PriorityContainer -from .preemptable_state import PreemptableState from .manually_transitionable_state import ManuallyTransitionableState +from .lockable_state import LockableState +from .preemptable_state import PreemptableState from .operatable_state import OperatableState from .event_state import EventState diff --git a/flexbe_core/src/flexbe_core/core/concurrency_container.py b/flexbe_core/src/flexbe_core/core/concurrency_container.py index 97aaaef8..fe25b2d7 100644 --- a/flexbe_core/src/flexbe_core/core/concurrency_container.py +++ b/flexbe_core/src/flexbe_core/core/concurrency_container.py @@ -1,13 +1,11 @@ #!/usr/bin/env python import traceback -import rospy +from flexbe_core.core.user_data import UserData from flexbe_core.core.event_state import EventState -from flexbe_core.core.operatable_state_machine import OperatableStateMachine -from flexbe_core.core.preemptable_state import PreemptableState from flexbe_core.core.priority_container import PriorityContainer -from .user_data import UserData +from flexbe_core.core.operatable_state_machine import OperatableStateMachine class ConcurrencyContainer(OperatableStateMachine): @@ -20,88 +18,74 @@ def __init__(self, conditions=dict(), *args, **kwargs): super(ConcurrencyContainer, self).__init__(*args, **kwargs) self._conditions = conditions self._returned_outcomes = dict() - self._do_rate_sleep = False self._sleep_dur = None - self.__execute = self.execute - self.execute = self._concurrency_execute - - - def _concurrency_execute(self, *args, **kwargs): - # use state machine execute, not state execute - return OperatableStateMachine.execute(self, *args, **kwargs) - - - def _build_msg(self, *args, **kwargs): - # use state machine _build_msg, not state execute - return OperatableStateMachine._build_msg(self, *args, **kwargs) - def sleep(self): if self._sleep_dur is not None: - rospy.sleep(self._sleep_dur) + self.wait(seconds=self._sleep_dur) self._sleep_dur = None @property def sleep_duration(self): return self._sleep_dur or 0. - def _update_once(self): + def _execute_current_state(self): + # execute all states that are done with sleeping and determine next sleep duration sleep_dur = None for state in self._states: - if state.name in list(self._returned_outcomes.keys()) and self._returned_outcomes[state.name] != self._loopback_name and self._returned_outcomes[state.name] is not None: - continue + if state.name in list(self._returned_outcomes.keys()) and self._returned_outcomes[state.name] is not None: + continue # already done with executing if (PriorityContainer.active_container is not None and not all(a == s for a, s in zip(PriorityContainer.active_container.split('/'), - state._get_path().split('/')))): + state.path.split('/')))): if isinstance(state, EventState): state._notify_skipped() - elif state._get_deep_state() is not None: - state._get_deep_state()._notify_skipped() - continue - state_sleep_dur = state.sleep_duration - if state_sleep_dur <= 0: - sleep_dur = 0 - self._returned_outcomes[state.name] = self._execute_state(state) + elif state.get_deep_state() is not None: + state.get_deep_state()._notify_skipped() + continue # other state has priority + if state.sleep_duration <= 0: # ready to execute + self._returned_outcomes[state.name] = self._execute_single_state(state) # check again in case the sleep has already been handled by the child - if state.sleep_duration < 0: + if state.sleep_duration <= 0: # this sleep returns immediately since sleep duration is negative, # but is required here to reset the sleep time after executing state.sleep() - else: - if sleep_dur is None: - sleep_dur = state_sleep_dur - else: - sleep_dur = min(sleep_dur, state_sleep_dur) + sleep_dur = state.sleep_duration if sleep_dur is None else min(sleep_dur, state.sleep_duration) if sleep_dur > 0: self._sleep_dur = sleep_dur # Determine outcome - outcome = self._loopback_name + outcome = None + if any(self._returned_outcomes[state.name] == state._preempted_name + for state in self._states if state.name in self._returned_outcomes): + return self._preempted_name # handle preemption if required + # check conditions for item in self._conditions: (oc, cond) = item - if all(sn in self._returned_outcomes and self._returned_outcomes[sn] == o for sn,o in cond): + if all(sn in self._returned_outcomes and self._returned_outcomes[sn] == o for sn, o in cond): outcome = oc break - # preempt (?) - if outcome == self._loopback_name: + if outcome is None: return None if outcome in self.outcomes: - # Call termination callbacks - self.exit(self.userdata, states = [s for s in self._states if s.name not in list(self._returned_outcomes.keys()) or self._returned_outcomes[s.name] == self._loopback_name]) + # trigger on_exit for those states that are not done yet + self.on_exit(self.userdata, + states=[s for s in self._states if (s.name not in list(self._returned_outcomes.keys()) or + self._returned_outcomes[s.name] is None)]) self._returned_outcomes = dict() # right now, going out of a cc may break sync # thus, as a quick fix, explicitly sync again self._parent._inner_sync_request = True self._current_state = None - return outcome else: - raise ValueError("Outcome '%s' of state '%s' with transition target '%s' is neither a registered state nor a registered container outcome." % - (outcome, self.name, outcome)) + raise ValueError("Outcome '%s' of state '%s' with transition target '%s' " + "is neither a registered state nor a registered container outcome." % + (outcome, self.name, outcome)) - def _execute_state(self, state, force_exit=False): + def _execute_single_state(self, state, force_exit=False): result = None try: ud = UserData(reference=self.userdata, input_keys=state.input_keys, @@ -116,13 +100,6 @@ def _execute_state(self, state, force_exit=False): (state.name, state) + traceback.format_exc()) return result - def _notify_start(self): - for state in self._states: - if isinstance(state, EventState): - state.on_start() - if isinstance(state, OperatableStateMachine): - state._notify_start() - def _enable_ros_control(self): state = self._states[0] if isinstance(state, EventState): @@ -130,15 +107,6 @@ def _enable_ros_control(self): if isinstance(state, OperatableStateMachine): state._enable_ros_control() - def _notify_stop(self): - for state in self._states: - if isinstance(state, EventState): - state.on_stop() - if isinstance(state, OperatableStateMachine): - state._notify_stop() - if state._is_controlled: - state._disable_ros_control() - def _disable_ros_control(self): state = self._states[0] if isinstance(state, EventState): @@ -146,6 +114,8 @@ def _disable_ros_control(self): if isinstance(state, OperatableStateMachine): state._disable_ros_control() - def exit(self, userdata, states = None): + def on_exit(self, userdata, states=None): for state in self._states if states is None else states: - self._execute_state(state, force_exit=True) \ No newline at end of file + if state in self._returned_outcomes: + continue # skip states that already exited themselves + self._execute_single_state(state, force_exit=True) diff --git a/flexbe_core/src/flexbe_core/core/event_state.py b/flexbe_core/src/flexbe_core/core/event_state.py index 2f74d926..816feab3 100644 --- a/flexbe_core/src/flexbe_core/core/event_state.py +++ b/flexbe_core/src/flexbe_core/core/event_state.py @@ -1,15 +1,13 @@ #!/usr/bin/env python -import rospy - -from flexbe_core.core.operatable_state import OperatableState +from flexbe_core.logger import Logger +from flexbe_core.state_logger import StateLogger from flexbe_core.core.preemptable_state import PreemptableState from flexbe_core.core.priority_container import PriorityContainer -from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached from flexbe_msgs.msg import CommandFeedback from std_msgs.msg import Bool, Empty -from flexbe_core.state_logger import StateLogger +from flexbe_core.core.operatable_state import OperatableState @StateLogger.log_events('flexbe.events', @@ -21,15 +19,14 @@ class EventState(OperatableState): """ A state that allows implementing certain events. """ - + def __init__(self, *args, **kwargs): super(EventState, self).__init__(*args, **kwargs) - - self._entering = True - self._skipped = False self.__execute = self.execute self.execute = self._event_execute + self._entering = True + self._skipped = False self._paused = False self._last_active_container = None @@ -37,25 +34,27 @@ def __init__(self, *args, **kwargs): self._repeat_topic = 'flexbe/command/repeat' self._pause_topic = 'flexbe/command/pause' - self._pub = ProxyPublisher() - self._sub = ProxySubscriberCached() - - def _event_execute(self, *args, **kwargs): if self._is_controlled and self._sub.has_msg(self._pause_topic): msg = self._sub.get_last_msg(self._pause_topic) + self._sub.remove_last_msg(self._pause_topic) if msg.data: - self._sub.remove_last_msg(self._pause_topic) - rospy.loginfo("--> Pausing in state %s", self.name) + Logger.localinfo("--> Pausing in state %s", self.name) self._pub.publish(self._feedback_topic, CommandFeedback(command="pause")) self._last_active_container = PriorityContainer.active_container # claim priority to propagate pause event - PriorityContainer.active_container = self._get_path() + PriorityContainer.active_container = self.path self._paused = True + else: + Logger.localinfo("--> Resuming in state %s", self.name) + self._pub.publish(self._feedback_topic, CommandFeedback(command="resume")) + PriorityContainer.active_container = self._last_active_container + self._last_active_container = None + self._paused = False if self._paused and not PreemptableState.preempt: self._notify_skipped() - return self._loopback_name + return None if self._entering: self._entering = False @@ -63,39 +62,27 @@ def _event_execute(self, *args, **kwargs): if self._skipped and not PreemptableState.preempt: self._skipped = False self.on_resume(*args, **kwargs) - - execute_outcome = self.__execute(*args, **kwargs) + + outcome = self.__execute(*args, **kwargs) repeat = False if self._is_controlled and self._sub.has_msg(self._repeat_topic): - rospy.loginfo("--> Repeating state %s", self.name) + Logger.localinfo("--> Repeating state %s", self.name) self._sub.remove_last_msg(self._repeat_topic) self._pub.publish(self._feedback_topic, CommandFeedback(command="repeat")) repeat = True - - if execute_outcome != self._loopback_name and not PreemptableState.switching and not PreemptableState.preempt \ - or repeat: + + if repeat or outcome is not None and not PreemptableState.preempt: self._entering = True self.on_exit(*args, **kwargs) - - return execute_outcome + return outcome def _notify_skipped(self): if not self._skipped: self.on_pause() self._skipped = True - if self._is_controlled and self._sub.has_msg(self._pause_topic): - msg = self._sub.get_last_msg(self._pause_topic) - if not msg.data: - self._sub.remove_last_msg(self._pause_topic) - rospy.loginfo("--> Resuming in state %s", self.name) - self._pub.publish(self._feedback_topic, CommandFeedback(command="resume")) - PriorityContainer.active_container = self._last_active_container - self._last_active_container = None - self._paused = False super(EventState, self)._notify_skipped() - def _enable_ros_control(self): super(EventState, self)._enable_ros_control() self._pub.createPublisher(self._feedback_topic, CommandFeedback) @@ -109,8 +96,7 @@ def _disable_ros_control(self): self._last_active_container = None if self._paused: PriorityContainer.active_container = None - - + # Events # (just implement the ones you need) @@ -137,15 +123,15 @@ def on_resume(self, userdata): Will be executed each time this state is resumed. """ pass - + def on_enter(self, userdata): """ Will be executed each time the state is entered from any other state (but not from itself). """ pass - + def on_exit(self, userdata): """ Will be executed each time the state will be left to any other state (but not to itself). """ - pass \ No newline at end of file + pass diff --git a/flexbe_core/src/flexbe_core/core/jumpable_state_machine.py b/flexbe_core/src/flexbe_core/core/jumpable_state_machine.py deleted file mode 100644 index ce084b83..00000000 --- a/flexbe_core/src/flexbe_core/core/jumpable_state_machine.py +++ /dev/null @@ -1,193 +0,0 @@ -#!/usr/bin/env python -import rospy -import time - -from .state_machine import StateMachine - -from flexbe_msgs.msg import OutcomeRequest -from ..proxy import ProxySubscriberCached - -from .silent_state_machine import SilentStateMachine - - -class JumpableStateMachine(SilentStateMachine): - """ - A state machine that runs in background and does not report any transition. - It can jump to any of its states, executing all states on a random way to the target - (so make sure its states have no side effects). - """ - refresh = False - - def __init__(self, *args, **kwargs): - super(JumpableStateMachine, self).__init__(*args, **kwargs) - - self._sub = ProxySubscriberCached() - - - def jump_to(self, target_state_label): - """ - Jumps to the specified state. - - @type target_state_label: string - @param target_state_label: The name of the state to jump to. - Should be unique. If not, the first occurrence will be chosen. - """ - # set current state and current container - container = self - while isinstance(container._current_state, StateMachine): - container = container._current_state - current_state = container._current_state - - # determine path - path = self._search_state(current_state, target_state_label, container, [], '') - if path is None: - rospy.logerr('No path found from '+current_state.name+' to '+target_state_label) - return - - rospy.loginfo('Path: '+path) - if path == '': - JumpableStateMachine.refresh = True - return - - # traverse path - outcomes = path.split('/') - for i in range(len(outcomes)-1): - current_state = self._get_deep_state() - current_state._requested_outcome = outcomes[i] - if not self._wait_for_next_state(current_state, 1): - rospy.logerr('Unable to jump to '+target_state_label+': Stuck in state '+current_state.name) - return - - - def _search_state(self, origin_state, target_state_label, container, seen_states, path): - """ - Searches the given state to create an outcome path to it. - This is implemented by a hierarchical depth-first search. - - @type origin_state: State - @param origin_state: The current origin for this search. - - @type target_state_label: string - @param target_state_label: The name of the state to search. - - @type container: StateMachine - @param container: The current container inside which the search is currently done. - - @type seen_states: list of State - @param seen_states: A list of all states that already have been traversed. - - @type path: string - @param path: The outcome path to the current origin_state - - @return: The whole outcome path to the searched state. - """ - # saw this state - seen_states.append(origin_state) - - # look if we found our target - if origin_state.name == target_state_label: - return path - target_path = None - - # enter the state machine (next lower hierarchy level) - if isinstance(origin_state, StateMachine): - # do not need to enter, inital state is implicitly active - if origin_state._initial_state_label == target_state_label: - return path - # search inside this state machine - child_state = origin_state._states[origin_state._initial_state_label] - if seen_states.count(child_state) == 0: - target_path = self._search_state(child_state, target_state_label, origin_state, seen_states, path) - # found target inside, search finished - if target_path is not None: - return target_path - - # proceed with the neighbors - else: - for outcome in origin_state._outcomes: - next_state_label = origin_state.transitions[outcome] - if next_state_label == 'preempted' or next_state_label == 'preempted_mirror': continue # skip preempt outcomes - next_container = container - reached_exit = False - - # determine the next state - # (consider that it might be necessary to go to the next higher hierarchy level) - while next_state_label not in next_container._states: - if next_container._parent is None: - reached_exit = True - break - next_state_label = next_container.transitions[next_state_label] - next_container = next_container._parent - # reached one outcome of the whole state machine (no higher hierarchy level) - if reached_exit: break - - # search the neighbor if not already seen - next_state = next_container._states[next_state_label] - if seen_states.count(next_state) == 0: - target_path = self._search_state(next_state, target_state_label, next_container, seen_states, path+outcome+'/') - # found target in neighbors - if target_path is not None: - return target_path - - # unfortunately, no success - return None - - - def _wait_for_next_state(self, current_state, timeout, period=0.05): - """ - Sleeps until the state machine proceeded with the next state. - - @type current_state: State - @param current_state: The current state when this method is called. - - @type timeout: float - @param timeout: The time in seconds when this method should stop waiting when no transition is done. - - @type period: float - @param period: The time in seconds how often this method should check the current state. - - @return: True if waiting was successful, False if current_state is still the same (timeout). - """ - end_time = time.time() + timeout - while time.time() < end_time: - new_state = self._get_deep_state() - if new_state != current_state: - return True - time.sleep(period) - return False - - - def _transition_callback(self, msg): - """ - Forces the state machine to do a transition with a specified outcome. - - @type msg: OutcomeRequest - @param msg: A message containing all relevant information. - """ - state = self._get_deep_state() - - # wait if state machine is currently not active - if state is None: - if not self._wait_for_next_state(None, 5): - rospy.logwarn('Updating mirror while it is not running!') - return # not running - state = self._get_deep_state() - rospy.logwarn('Had to wait for mirror starting to request transition of state %s!', msg.target) - - # wait for the target state to not trigger a wrong transition - if msg.target != state.name: - self._wait_for_next_state(state, 2) - state = self._get_deep_state() - if msg.target != state.name: # waited for next state, so test current state again (but only once to not block anything) - rospy.logwarn('Wrong state name: requested %s but is %s!', msg.target, state.name) - return - rospy.logwarn('Had to wait for state %s to request transition!', msg.target) - - # trigger desired outcome or preempt - if msg.outcome == 255: - state._requested_outcome = 'preempted' - else: - state._requested_outcome = state._outcome_list[msg.outcome] - - def destroy(self): - pass \ No newline at end of file diff --git a/flexbe_core/src/flexbe_core/core/lockable_state.py b/flexbe_core/src/flexbe_core/core/lockable_state.py index d1339329..579c2d8c 100644 --- a/flexbe_core/src/flexbe_core/core/lockable_state.py +++ b/flexbe_core/src/flexbe_core/core/lockable_state.py @@ -1,12 +1,11 @@ #!/usr/bin/env python -import rospy +from flexbe_core.logger import Logger -from flexbe_core.core.manually_transitionable_state import ManuallyTransitionableState - -from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached from flexbe_msgs.msg import CommandFeedback from std_msgs.msg import String +from flexbe_core.core.manually_transitionable_state import ManuallyTransitionableState + class LockableState(ManuallyTransitionableState): """ @@ -15,24 +14,19 @@ class LockableState(ManuallyTransitionableState): However, if any outcome would be triggered, the outcome will be stored and the state won't be executed anymore until it is unlocked and the stored outcome is set. """ - + def __init__(self, *args, **kwargs): super(LockableState, self).__init__(*args, **kwargs) + self.__execute = self.execute + self.execute = self._lockable_execute self._locked = False self._stored_outcome = None - - self.__execute = self.execute - self.execute = self._lockable_execute self._feedback_topic = 'flexbe/command_feedback' self._lock_topic = 'flexbe/command/lock' self._unlock_topic = 'flexbe/command/unlock' - self._pub = ProxyPublisher() - self._sub = ProxySubscriberCached() - - def _lockable_execute(self, *args, **kwargs): if self._is_controlled and self._sub.has_msg(self._lock_topic): msg = self._sub.get_last_msg(self._lock_topic) @@ -44,13 +38,15 @@ def _lockable_execute(self, *args, **kwargs): self._sub.remove_last_msg(self._unlock_topic) self._execute_unlock(msg.data) + # locked, so execute until we want to transition if self._locked: if self._stored_outcome is None or self._stored_outcome == 'None': self._stored_outcome = self.__execute(*args, **kwargs) return None - - if not self._locked and not self._stored_outcome is None and not self._stored_outcome == 'None': - if self._parent.transition_allowed(self.name, self._stored_outcome): + + # not locked, but there still is a transition we want to trigger + if not self._locked and self._stored_outcome is not None and not self._stored_outcome == 'None': + if self.parent.transition_allowed(self.name, self._stored_outcome): outcome = self._stored_outcome self._stored_outcome = None return outcome @@ -62,42 +58,42 @@ def _lockable_execute(self, *args, **kwargs): if outcome is None or outcome == 'None': return None - if not self._parent is None and not self._parent.transition_allowed(self.name, outcome): + # not locked, but still, a parent could be locked so we need to ensure that we do not cause an outcome there + if self.parent is not None and not self.parent.transition_allowed(self.name, outcome): self._stored_outcome = outcome return None return outcome - def _execute_lock(self, target): - if target == self._get_path() or target == '': - target = self._get_path() + if target == self.path or target == '': + target = self.path found_target = True self._locked = True else: found_target = self._parent.lock(target) - - self._pub.publish(self._feedback_topic, CommandFeedback(command="lock", args=[target, target if found_target else ""])) + # provide feedback about lock + self._pub.publish(self._feedback_topic, CommandFeedback(command="lock", + args=[target, target if found_target else ""])) if not found_target: - rospy.logwarn("--> Wanted to lock %s, but could not find it in current path %s.", target, self._get_path()) + Logger.logwarn("Wanted to lock %s, but could not find it in current path %s." % (target, self.path)) else: - rospy.loginfo("--> Locking in state %s", target) - + Logger.localinfo("--> Locking in state %s" % target) def _execute_unlock(self, target): - if target == self._get_path() or (self._locked and target == ''): - target = self._get_path() + if target == self.path or (self._locked and target == ''): + target = self.path found_target = True self._locked = False else: found_target = self._parent.unlock(target) - - self._pub.publish(self._feedback_topic, CommandFeedback(command="unlock", args=[target, target if found_target else ""])) + # provide feedback about unlock + self._pub.publish(self._feedback_topic, CommandFeedback(command="unlock", + args=[target, target if found_target else ""])) if not found_target: - rospy.logwarn("--> Wanted to unlock %s, but could not find it in current path %s.", target, self._get_path()) + Logger.logwarn("Wanted to unlock %s, but could not find it in current path %s." % (target, self.path)) else: - rospy.loginfo("--> Unlocking in state %s", target) - + Logger.localinfo("--> Unlocking in state %s" % target) def _enable_ros_control(self): super(LockableState, self)._enable_ros_control() @@ -110,6 +106,5 @@ def _disable_ros_control(self): self._sub.unsubscribe_topic(self._lock_topic) self._sub.unsubscribe_topic(self._unlock_topic) - def is_locked(self): - return self._locked \ No newline at end of file + return self._locked diff --git a/flexbe_core/src/flexbe_core/core/lockable_state_machine.py b/flexbe_core/src/flexbe_core/core/lockable_state_machine.py index 453a2a9b..2b15328f 100644 --- a/flexbe_core/src/flexbe_core/core/lockable_state_machine.py +++ b/flexbe_core/src/flexbe_core/core/lockable_state_machine.py @@ -1,9 +1,8 @@ #!/usr/bin/env python -import rospy -from .state_machine import StateMachine +from .ros_state_machine import RosStateMachine -class LockableStateMachine(StateMachine): +class LockableStateMachine(RosStateMachine): """ A state machine that can be locked. When locked, no transition can be done regardless of the resulting outcome. @@ -11,54 +10,62 @@ class LockableStateMachine(StateMachine): and the state won't be executed anymore until it is unlocked and the stored outcome is set. """ path_for_switch = None - + def __init__(self, *args, **kwargs): super(LockableStateMachine, self).__init__(*args, **kwargs) - self._locked = False - - self._is_controlled = False - - def _get_deep_state(self): + def get_deep_state(self): """ Looks for the current state (traversing all state machines down to the real state). - + @return: The current state (not state machine) """ container = self - while isinstance(container._current_state, StateMachine): + while isinstance(container._current_state, LockableStateMachine): container = container._current_state return container._current_state - - def execute(self, userdata): - if LockableStateMachine.path_for_switch is not None and LockableStateMachine.path_for_switch.startswith(self._get_path()): - path_segments = LockableStateMachine.path_for_switch.replace(self._get_path(), "", 1).split("/") - wanted_state = path_segments[1] - self._current_state(wanted_state) - if len(path_segments) <= 2: - LockableStateMachine.path_for_switch = None - return super(LockableStateMachine, self).execute(userdata) - - def _is_internal_transition(self, transition_target): return transition_target in self._labels - def transition_allowed(self, state, outcome): if outcome is None or outcome == 'None': return True transition_target = self._transitions[state][outcome] - return self._is_internal_transition(transition_target) or (not self._locked and (self._parent is None or self._parent.transition_allowed(self.name, transition_target))) + return (self._is_internal_transition(transition_target) or + (not self._locked and (self.parent is None or + self.parent.transition_allowed(self.name, transition_target)))) + + # for switching + + def execute(self, userdata): + 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("/") + wanted_state = path_segments[1] + self._current_state = self._labels[wanted_state] + if len(path_segments) <= 2: + LockableStateMachine.path_for_switch = None + return super(LockableStateMachine, self).execute(userdata) + + def replace_userdata(self, userdata): + self._userdata = userdata + def replace_state(self, state): + 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 _get_path(self): - return "" if self._parent is None else self._parent._get_path() + "/" + self.name + def remove_state(self, state): + del self._labels[state.name] + self._states.remove(state) + # for locking def lock(self, path): - if path == self._get_path(): + if path == self.path: self._locked = True return True elif self._parent is not None: @@ -66,9 +73,8 @@ def lock(self, path): else: return False - def unlock(self, path): - if path == self._get_path(): + if path == self.path: self._locked = False return True elif self._parent is not None: @@ -76,35 +82,28 @@ def unlock(self, path): else: return False - def is_locked(self): return self._locked - def is_locked_inside(self): if self._locked: return True - for state in self._states: result = False - if isinstance(state, StateMachine): + if isinstance(state, LockableStateMachine): result = state.is_locked_inside() else: result = state.is_locked() if result is True: return True - return False - def get_locked_state(self): if self._locked: return self - for state in self._states: if state.is_locked(): return state - elif isinstance(state, StateMachine): + elif isinstance(state, LockableStateMachine): return state.get_locked_state() - return None diff --git a/flexbe_core/src/flexbe_core/core/loopback_state.py b/flexbe_core/src/flexbe_core/core/loopback_state.py deleted file mode 100644 index 453ab8ed..00000000 --- a/flexbe_core/src/flexbe_core/core/loopback_state.py +++ /dev/null @@ -1,46 +0,0 @@ -#!/usr/bin/env python -import rospy - -from flexbe_core.core.lockable_state import LockableState - -class LoopbackState(LockableState): - """ - A state that can refer back to itself. - It periodically transitions to itself while no other outcome is fulfilled. - """ - - _loopback_name = 'loopback' - def __init__(self, *args, **kwargs): - # add loopback outcome - if len(args) > 0 and type(args[0]) is list: - # need this ugly check for list type, because first argument in CBState is the callback - args[0].append(self._loopback_name) - else: - outcomes = kwargs.get('outcomes', []) - outcomes.append(self._loopback_name) - kwargs['outcomes'] = outcomes - - super(LoopbackState, self).__init__(*args, **kwargs) - self._rate = rospy.Rate(10) - self.__execute = self.execute - self.execute = self._loopback_execute - - def _loopback_execute(self, *args, **kwargs): - result = self.__execute(*args, **kwargs) - - if result is None or result == 'None': - result = self._loopback_name - return result - - def set_rate(self, rate): - """ - Set the execution rate of this state, - i.e., the rate with which the execute method is being called. - - Note: The rate is best-effort, - a rospy.Rate does not guarantee real-time properties. - - @type label: float - @param label: The desired rate in Hz. - """ - self._rate = rospy.Rate(rate) diff --git a/flexbe_core/src/flexbe_core/core/loopback_state_machine.py b/flexbe_core/src/flexbe_core/core/loopback_state_machine.py deleted file mode 100644 index 4b27baa1..00000000 --- a/flexbe_core/src/flexbe_core/core/loopback_state_machine.py +++ /dev/null @@ -1,23 +0,0 @@ -#!/usr/bin/env python -import rospy - -from flexbe_core.core.lockable_state_machine import LockableStateMachine - - -class LoopbackStateMachine(LockableStateMachine): - """ - A state machine that can loop back to itself. - """ - - _loopback_name = 'loopback' - - def __init__(self, *args, **kwargs): - # add loopback outcome - if len(args) > 0: - args[0].append(self._loopback_name) - else: - outcomes = kwargs.get('outcomes', []) - outcomes.append(self._loopback_name) - kwargs['outcomes'] = outcomes - - super(LoopbackStateMachine, self).__init__(*args, **kwargs) diff --git a/flexbe_core/src/flexbe_core/core/manually_transitionable_state.py b/flexbe_core/src/flexbe_core/core/manually_transitionable_state.py index 3a7da62d..cddf29e1 100644 --- a/flexbe_core/src/flexbe_core/core/manually_transitionable_state.py +++ b/flexbe_core/src/flexbe_core/core/manually_transitionable_state.py @@ -1,50 +1,44 @@ #!/usr/bin/env python -import rospy +from flexbe_core.logger import Logger -from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached from flexbe_msgs.msg import CommandFeedback, OutcomeRequest -from flexbe_core.core.monitoring_state import MonitoringState +from flexbe_core.core.ros_state import RosState -class ManuallyTransitionableState(MonitoringState): +class ManuallyTransitionableState(RosState): """ A state for that a desired outcome can be declared. If any outcome is declared, this outcome is forced. """ - + def __init__(self, *args, **kwargs): super(ManuallyTransitionableState, self).__init__(*args, **kwargs) - - self._force_transition = False - self.__execute = self.execute self.execute = self._manually_transitionable_execute + self._force_transition = False + self._feedback_topic = 'flexbe/command_feedback' self._transition_topic = 'flexbe/command/transition' - self._pub = ProxyPublisher() - self._sub = ProxySubscriberCached() - - def _manually_transitionable_execute(self, *args, **kwargs): if self._is_controlled and self._sub.has_buffered(self._transition_topic): command_msg = self._sub.get_from_buffer(self._transition_topic) - self._pub.publish(self._feedback_topic, CommandFeedback(command="transition", args=[command_msg.target, self.name])) + self._pub.publish(self._feedback_topic, CommandFeedback(command="transition", + args=[command_msg.target, self.name])) if command_msg.target != self.name: - rospy.logwarn("--> Requested outcome for state " + command_msg.target + " but active state is " + self.name) + Logger.logwarn("Requested outcome for state %s but active state is %s" % + (command_msg.target, self.name)) else: self._force_transition = True - outcome = self._outcome_list[command_msg.outcome] - rospy.loginfo("--> Manually triggered outcome " + outcome + " of state " + self.name) + outcome = self.outcomes[command_msg.outcome] + Logger.localinfo("--> Manually triggered outcome %s of state %s" % (outcome, self.name)) return outcome - - # return the normal outcome + # otherwise, return the normal outcome self._force_transition = False return self.__execute(*args, **kwargs) - def _enable_ros_control(self): super(ManuallyTransitionableState, self)._enable_ros_control() self._pub.createPublisher(self._feedback_topic, CommandFeedback) diff --git a/flexbe_core/src/flexbe_core/core/monitoring_state.py b/flexbe_core/src/flexbe_core/core/monitoring_state.py deleted file mode 100644 index 49b7c8ca..00000000 --- a/flexbe_core/src/flexbe_core/core/monitoring_state.py +++ /dev/null @@ -1,97 +0,0 @@ -#!/usr/bin/env python -import rospy - -from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached -from flexbe_msgs.msg import CommandFeedback, OutcomeRequest -from flexbe_core.logger import Logger - -from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus - -from .state import State - - -class MonitoringState(State): - """ - A state to monitor a custom set of conditions. - For each conditions, an outcome is added or mapped which will be returned if the condition is not met. - """ - - def __init__(self, *args, **kwargs): - super(MonitoringState, self).__init__(*args, **kwargs) - - outcomes = kwargs.get('outcomes', []) - self._outcome_list = list(outcomes) - self._outcome_list.remove('loopback') - - self._is_controlled = False - self._force_monitoring = False - - self._monitored_keys = dict() - self._sent_keys = list() - self._current_status = None - - self.__execute = self.execute - self.execute = self._monitoring_execute - - self._diagnostics_topic = 'diagnostics_agg' - self._sub = ProxySubscriberCached() - - - def _monitoring_execute(self, *args, **kwargs): - new_status = None - had_warning = False - if (self._force_monitoring or self._is_controlled) and self._sub.has_buffered(self._diagnostics_topic): - new_status = "" - diag_msg = self._sub.get_from_buffer(self._diagnostics_topic) - for status in diag_msg.status: - if not status.name in list(self._monitored_keys.keys()): continue - if status.level == DiagnosticStatus.WARN: - had_warning = True - if not status.name + "_warn" in self._sent_keys: - self._sent_keys.append(status.name + "_warn") - Logger.logwarn("%s: %s" % (status.name, status.message)) - if status.level == DiagnosticStatus.ERROR: - if not status.name + "_err" in self._sent_keys: - self._sent_keys.append(status.name + "_err") - Logger.logerr("%s: %s" % (status.name, status.message)) - new_status = status.name - - if new_status == "": - self._current_status = None - new_status = None - if not had_warning: - self._sent_keys = list() - - if new_status is None or self._current_status == new_status: - return self.__execute(*args, **kwargs) - - self._current_status = new_status - - return self._monitored_keys[self._current_status] - - - def monitor(self, key, outcome = None): - oc = outcome if not outcome is None else key - self._monitored_keys[key] = oc - if not oc in self._outcomes: - self.register_outcomes([oc]) - self._outcome_list.append(oc) - - def force_monitoring(self): - self._force_monitoring = True - if not self._is_controlled: - self._sub.subscribe(self._diagnostics_topic, DiagnosticArray) - self._sub.enable_buffer(self._diagnostics_topic) - - def _enable_ros_control(self): - self._is_controlled = True - self._sub.subscribe(self._diagnostics_topic, DiagnosticArray) - self._sub.enable_buffer(self._diagnostics_topic) - - def _disable_ros_control(self): - self._is_controlled = False - self._sub.unsubscribe_topic(self._diagnostics_topic) - - - def _get_path(self): - return self._parent._get_path() + "/" + self.name diff --git a/flexbe_core/src/flexbe_core/core/operatable_state.py b/flexbe_core/src/flexbe_core/core/operatable_state.py index a9614c15..ec804943 100644 --- a/flexbe_core/src/flexbe_core/core/operatable_state.py +++ b/flexbe_core/src/flexbe_core/core/operatable_state.py @@ -1,14 +1,11 @@ #!/usr/bin/env python -import rospy - -from flexbe_core.core.preemptable_state import PreemptableState -from flexbe_core.core.operatable_state_machine import OperatableStateMachine +from flexbe_core.logger import Logger +from flexbe_core.state_logger import StateLogger -from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached -from flexbe_msgs.msg import Container, OutcomeRequest +from flexbe_msgs.msg import OutcomeRequest from std_msgs.msg import UInt8, String -from flexbe_core.state_logger import StateLogger +from flexbe_core.core.preemptable_state import PreemptableState @StateLogger.log_outcomes('flexbe.outcomes') @@ -19,125 +16,55 @@ class OperatableState(PreemptableState): as it reports each transition and its initial structure. An additional method is provided to report custom status messages to the widget. """ - + def __init__(self, *args, **kwargs): super(OperatableState, self).__init__(*args, **kwargs) - self.transitions = None - self.autonomy = None - - self._mute = False # is set to true when used in silent state machine (don't report transitions) + self.__execute = self.execute + self.execute = self._operatable_execute + self._last_requested_outcome = None - + self._outcome_topic = 'flexbe/mirror/outcome' self._request_topic = 'flexbe/outcome_request' self._debug_topic = 'flexbe/debug/current_state' - self._pub = ProxyPublisher() - - self.__execute = self.execute - self.execute = self._operatable_execute - - - def _build_msg(self, prefix, msg): - """ - Adds this state to the initial structure message. - - @type prefix: string - @param prefix: A path consisting of the container hierarchy containing this state. - - @type msg: ContainerStructure - @param msg: The message that will finally contain the structure message. - """ - - # set path - name = prefix + self.name - - # no children - children = None - - # set outcomes - outcomes = self._outcome_list - - # set transitions and autonomy levels - transitions = [] - autonomy = [] - for i in range(len(outcomes)): - outcome = outcomes[i] - if outcome == 'preempted': # set preempt transition - transitions.append('preempted') - autonomy.append(-1) - else: - transitions.append(str(self.transitions[outcome])) - autonomy.append(self.autonomy[outcome]) - - # add to message - msg.containers.append(Container(name, children, outcomes, transitions, autonomy)) - def _operatable_execute(self, *args, **kwargs): outcome = self.__execute(*args, **kwargs) - - if self._is_controlled: + if self._is_controlled: # reset previously requested outcome if applicable - if self._last_requested_outcome is not None and outcome == OperatableState._loopback_name: - self._pub.publish(self._request_topic, OutcomeRequest(outcome=255, target=self._parent._get_path() + "/" + self.name)) + if self._last_requested_outcome is not None and outcome is None: + self._pub.publish(self._request_topic, OutcomeRequest(outcome=255, target=self.path)) self._last_requested_outcome = None # request outcome because autonomy level is too low - if not self._force_transition and (outcome in self.autonomy and self.autonomy[outcome] >= OperatableStateMachine.autonomy_level or - outcome != OperatableState._loopback_name and - self._get_path() in rospy.get_param('/flexbe/breakpoints', [])): + if not self._force_transition and (not self.parent.is_transition_allowed(self.name, outcome) or + outcome is not None and self.is_breakpoint): if outcome != self._last_requested_outcome: - self._pub.publish(self._request_topic, OutcomeRequest(outcome=self._outcome_list.index(outcome), target=self._parent._get_path() + "/" + self.name)) - rospy.loginfo("<-- Want result: %s > %s", self.name, outcome) + self._pub.publish(self._request_topic, OutcomeRequest(outcome=self.outcomes.index(outcome), + target=self.path)) + Logger.localinfo("<-- Want result: %s > %s" % (self.name, outcome)) StateLogger.log('flexbe.operator', self, type='request', request=outcome, - autonomy=OperatableStateMachine.autonomy_level, - required=self.autonomy[outcome]) + autonomy=self.parent.autonomy_level, + required=self.parent.get_required_autonomy(outcome)) self._last_requested_outcome = outcome - outcome = OperatableState._loopback_name - + outcome = None + # autonomy level is high enough, report the executed transition - elif outcome != OperatableState._loopback_name: - rospy.loginfo("State result: %s > %s", self.name, outcome) - self._pub.publish(self._outcome_topic, UInt8(self._outcome_list.index(outcome))) - self._pub.publish(self._debug_topic, String("%s > %s" % (self._get_path(), outcome))) + elif outcome is not None and outcome != self._preempted_name: + Logger.localinfo("State result: %s > %s" % (self.name, outcome)) + self._pub.publish(self._outcome_topic, UInt8(self.outcomes.index(outcome))) + self._pub.publish(self._debug_topic, String("%s > %s" % (self.path, outcome))) if self._force_transition: StateLogger.log('flexbe.operator', self, type='forced', forced=outcome, requested=self._last_requested_outcome) self._last_requested_outcome = None self._force_transition = False - return outcome - def _notify_skipped(self): - super(OperatableState, self)._notify_skipped() - - def _enable_ros_control(self): super(OperatableState, self)._enable_ros_control() self._pub.createPublisher(self._outcome_topic, UInt8) self._pub.createPublisher(self._debug_topic, String) self._pub.createPublisher(self._request_topic, OutcomeRequest) - - - - - - - - - - - - - - - - - - - - - - diff --git a/flexbe_core/src/flexbe_core/core/operatable_state_machine.py b/flexbe_core/src/flexbe_core/core/operatable_state_machine.py index b862bd95..f633bc48 100644 --- a/flexbe_core/src/flexbe_core/core/operatable_state_machine.py +++ b/flexbe_core/src/flexbe_core/core/operatable_state_machine.py @@ -1,21 +1,14 @@ #!/usr/bin/env python -import rospy import zlib +from flexbe_core.core.user_data import UserData +from flexbe_core.logger import Logger +from flexbe_core.state_logger import StateLogger +from flexbe_core.core.operatable_state import OperatableState -from rospy.exceptions import ROSInterruptException - -from flexbe_core.core.preemptable_state_machine import PreemptableStateMachine -from flexbe_core.core.lockable_state_machine import LockableStateMachine - -from .state_machine import StateMachine -from .user_data import UserData - -from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached -from flexbe_msgs.msg import Container, ContainerStructure, OutcomeRequest, BehaviorSync, CommandFeedback, BehaviorLog +from flexbe_msgs.msg import Container, ContainerStructure, BehaviorSync, CommandFeedback from std_msgs.msg import Empty, UInt8, Int32 -from flexbe_core.core.loopback_state import LoopbackState -from flexbe_core.state_logger import StateLogger +from flexbe_core.core.preemptable_state_machine import PreemptableStateMachine class OperatableStateMachine(PreemptableStateMachine): @@ -23,74 +16,100 @@ class OperatableStateMachine(PreemptableStateMachine): A state machine that can be operated. It synchronizes its current state with the mirror and supports some control mechanisms. """ - + autonomy_level = 3 - silent_mode = False - + def __init__(self, *args, **kwargs): super(OperatableStateMachine, self).__init__(*args, **kwargs) - self._message = None - self._rate = rospy.Rate(10) - self._do_rate_sleep = True - self.id = None - self.autonomy = None - self._autonomy = {} - self._ordered_states = [] self._inner_sync_request = False - - self._pub = ProxyPublisher() - - self._sub = ProxySubscriberCached() + # construction @staticmethod - def add(label, state, transitions = None, autonomy = None, remapping = None): + def add(label, state, transitions, autonomy=None, remapping=None): """ Add a state to the opened state machine. - + @type label: string @param label: The label of the state being added. - + @param state: An instance of a class implementing the L{State} interface. - + @param transitions: A dictionary mapping state outcomes to other state labels or container outcomes. - + @param autonomy: A dictionary mapping state outcomes to their required autonomy level @param remapping: A dictionary mapping local userdata keys to userdata keys in the container. """ - self = StateMachine._currently_opened_container - - # add loopback transition to loopback states - if isinstance(state, LoopbackState): - transitions[LoopbackState._loopback_name] = label - autonomy[LoopbackState._loopback_name] = -1 - if isinstance(state, OperatableStateMachine): - transitions[OperatableStateMachine._loopback_name] = label - autonomy[OperatableStateMachine._loopback_name] = -1 - - self._ordered_states.append(state) - state.transitions = transitions - state.autonomy = autonomy - - StateMachine.add(label, state, transitions, remapping) + self = OperatableStateMachine.get_opened_container() + PreemptableStateMachine.add(label, state, transitions, remapping) self._autonomy[label] = autonomy - def replace(self, new_state): - old_state = self._states[new_state.name] - new_state.transitions = old_state.transitions - new_state.autonomy = old_state.autonomy - new_state._parent = old_state._parent + def _build_structure_msg(self): + """ + Creates a message to describe the structure of this state machine. + """ + structure_msg = ContainerStructure() + container_msg = self._add_to_structure_msg(structure_msg) + container_msg.outcomes = self.outcomes + structure_msg.behavior_id = self.id + return structure_msg + + def _add_to_structure_msg(self, structure_msg): + """ + Adds this state machine and all children to the structure message. + + @type structure_msg: ContainerStructure + @param structure_msg: The message that will finally contain the structure message. + """ + # add self to message + container_msg = Container() + container_msg.path = self.path + container_msg.children = [state.name for state in self._states] + structure_msg.containers.append(container_msg) + # add children to message + for state in self._states: + # create and add children + if isinstance(state, OperatableStateMachine): + state_msg = state._add_to_structure_msg(structure_msg) + else: + state_msg = Container(path=state.path) + structure_msg.containers.append(state_msg) + # complete structure info for children + state_msg.outcomes = state.outcomes + 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] + return container_msg + + # execution + + def _execute_current_state(self): + outcome = super(OperatableStateMachine, self)._execute_current_state() + # provide explicit sync as back-up functionality + # should be used only if there is no other choice + # since it requires additional 8 byte + header update bandwith and time to restart mirror + if self._inner_sync_request and self.get_deep_state() is not None: + self._inner_sync_request = False + if self.id is None: + self.parent._inner_sync_request = True + else: + msg = BehaviorSync() + msg.behavior_id = self.id + msg.current_state_checksum = zlib.adler32(self.get_deep_state().path) + self._pub.publish('flexbe/mirror/sync', msg) + return outcome + + def is_transition_allowed(self, label, outcome): + return self._autonomy[label].get(outcome, -1) < OperatableStateMachine.autonomy_level + + def get_required_autonomy(self, outcome): + return self._autonomy[self.current_state_label][outcome] - self._ordered_states[self._ordered_states.index(old_state)] = new_state - self._states[new_state.name] = new_state - - def destroy(self): self._notify_stop() self._disable_ros_control() @@ -99,25 +118,25 @@ def destroy(self): self._sub.unsubscribe_topic('flexbe/command/attach') self._sub.unsubscribe_topic('flexbe/request_mirror_structure') StateLogger.shutdown() - - + def confirm(self, name, id): """ Confirms the state machine and triggers the creation of the structural message. It is mandatory to call this function at the top-level state machine between building it and starting its execution. - + @type name: string @param name: The name of this state machine to identify it. """ self.set_name(name) self.id = id - self._pub.createPublisher('flexbe/mirror/sync', BehaviorSync, _latch = True) # Update mirror with currently active state (high bandwidth mode) - self._pub.createPublisher('flexbe/mirror/preempt', Empty, _latch = True) # Preempts the mirror - self._pub.createPublisher('flexbe/mirror/structure', ContainerStructure) # Sends the current structure to the mirror - self._pub.createPublisher('flexbe/log', BehaviorLog) # Topic for logs to the GUI - self._pub.createPublisher('flexbe/command_feedback', CommandFeedback) # Gives feedback about executed commands to the GUI + # Update mirror with currently active state (high bandwidth mode) + self._pub.createPublisher('flexbe/mirror/sync', BehaviorSync) + # Sends the current structure to the mirror + self._pub.createPublisher('flexbe/mirror/structure', ContainerStructure) + # Gives feedback about executed commands to the GUI + self._pub.createPublisher('flexbe/command_feedback', CommandFeedback) self._sub.subscribe('flexbe/command/autonomy', UInt8, self._set_autonomy_level) self._sub.subscribe('flexbe/command/sync', Empty, self._sync_callback) @@ -129,37 +148,34 @@ def confirm(self, name, id): if OperatableStateMachine.autonomy_level != 255: self._enable_ros_control() - rospy.sleep(0.5) # no clean way to wait for publisher to be ready... + self.wait(seconds=0.2) # no clean way to wait for publisher to be ready... self._notify_start() - + # operator callbacks + def _set_autonomy_level(self, msg): """ Sets the current autonomy level. """ if OperatableStateMachine.autonomy_level != msg.data: - rospy.loginfo('--> Autonomy changed to %d', msg.data) - + Logger.localinfo('--> Autonomy changed to %d' % msg.data) if msg.data < 0: self.preempt() else: OperatableStateMachine.autonomy_level = msg.data - self._pub.publish('flexbe/command_feedback', CommandFeedback(command="autonomy", args=[])) - def _sync_callback(self, msg): - rospy.loginfo("--> Synchronization requested...") + Logger.localinfo("--> Synchronization requested...") msg = BehaviorSync() msg.behavior_id = self.id - while self._get_deep_state() is None: - rospy.sleep(0.1) - msg.current_state_checksum = zlib.adler32(self._get_deep_state()._get_path()) + # make sure we are already executing + self.wait(condition=lambda: self.get_deep_state() is not None) + msg.current_state_checksum = zlib.adler32(self.get_deep_state().path) self._pub.publish('flexbe/mirror/sync', msg) self._pub.publish('flexbe/command_feedback', CommandFeedback(command="sync", args=[])) - rospy.loginfo("<-- Sent synchronization message for mirror.") - + Logger.localinfo("<-- Sent synchronization message for mirror.") def _attach_callback(self, msg): - rospy.loginfo("--> Enabling control...") + Logger.localinfo("--> Enabling control...") # set autonomy level OperatableStateMachine.autonomy_level = msg.data # enable control of states @@ -169,102 +185,29 @@ def _attach_callback(self, msg): cfb = CommandFeedback(command="attach") cfb.args.append(self.name) self._pub.publish('flexbe/command_feedback', cfb) - rospy.loginfo("<-- Sent attach confirm.") - + Logger.localinfo("<-- Sent attach confirm.") def _mirror_structure_callback(self, msg): - rospy.loginfo("--> Creating behavior structure for mirror...") - msg = self._build_msg('') - msg.behavior_id = self.id - self._pub.publish('flexbe/mirror/structure', msg) - rospy.loginfo("<-- Sent behavior structure for mirror.") - - - def _transition_allowed(self, label, outcome): - return self._autonomy[label][outcome] < OperatableStateMachine.autonomy_level - - - def _build_msg(self, prefix, msg = None): - """ - Adds this state machine to the initial structure message. - - @type prefix: string - @param prefix: A path consisting of the container hierarchy containing this state. - - @type msg: ContainerStructure - @param msg: The message that will finally contain the structure message. - """ - # set children - children = [] - for state in self._ordered_states: - children.append(str(state.name)) - - # set name - name = prefix + (self.name if self.id is None else '') - - if msg is None: - # top-level state machine (has no transitions) - self._message = ContainerStructure() - outcomes = list(self._outcomes) - transitions = None - autonomy = None - else: - # lower-level state machine - self._message = msg - outcomes = list(self.transitions) - # set transitions and autonomy - transitions = [] - autonomy = [] - for i in range(len(self.transitions)): - outcome = outcomes[i] - if outcome == 'preempted': # set preempt transition - transitions.append('preempted') - autonomy.append(-1) - else: - transitions.append(str(self.transitions[outcome])) - autonomy.append(self.autonomy[outcome]) - - # add to message - self._message.containers.append(Container(name, children, outcomes, transitions, autonomy)) - - # build message for children - for state in self._ordered_states: - state._build_msg(name+'/', self._message) - - # top-level state machine returns the message - if msg is None: - return self._message + Logger.localinfo("--> Creating behavior structure for mirror...") + self._pub.publish('flexbe/mirror/structure', self._build_structure_msg()) + Logger.localinfo("<-- Sent behavior structure for mirror.") + # handle state events def _notify_start(self): - for state in self._ordered_states: - if isinstance(state, LoopbackState): + for state in self._states: + if isinstance(state, OperatableState): state.on_start() if isinstance(state, OperatableStateMachine): state._notify_start() - def _enable_ros_control(self): - self._is_controlled = True - for state in self._ordered_states: - if isinstance(state, LoopbackState): - state._enable_ros_control() - if isinstance(state, OperatableStateMachine): - state._enable_ros_control() - def _notify_stop(self): - for state in self._ordered_states: - if isinstance(state, LoopbackState): + for state in self._states: + if isinstance(state, OperatableState): state.on_stop() - state._disable_ros_control() if isinstance(state, OperatableStateMachine): state._notify_stop() - - def _disable_ros_control(self): - self._is_controlled = False - for state in self._ordered_states: - if isinstance(state, LoopbackState): - state._disable_ros_control() - if isinstance(state, OperatableStateMachine): + if state._is_controlled: state._disable_ros_control() def on_exit(self, userdata): diff --git a/flexbe_core/src/flexbe_core/core/preemptable_state.py b/flexbe_core/src/flexbe_core/core/preemptable_state.py index 66dc659f..462e1c57 100644 --- a/flexbe_core/src/flexbe_core/core/preemptable_state.py +++ b/flexbe_core/src/flexbe_core/core/preemptable_state.py @@ -1,67 +1,46 @@ #!/usr/bin/env python -import rospy +from flexbe_core.logger import Logger -from flexbe_core.core.loopback_state import LoopbackState - -from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached from flexbe_msgs.msg import CommandFeedback from std_msgs.msg import Empty +from flexbe_core.core.lockable_state import LockableState + -class PreemptableState(LoopbackState): +class PreemptableState(LockableState): """ A state that can be preempted. If preempted, the state will not be executed anymore and return the outcome preempted. """ - + _preempted_name = 'preempted' preempt = False - switching = False def __init__(self, *args, **kwargs): - # add preempted outcome - if len(args) > 0 and type(args[0]) is list: - # need this ugly check for list type, because first argument in CBState is the callback - args[0].append(self._preempted_name) - else: - outcomes = kwargs.get('outcomes', []) - outcomes.append(self._preempted_name) - kwargs['outcomes'] = outcomes - super(PreemptableState, self).__init__(*args, **kwargs) self.__execute = self.execute self.execute = self._preemptable_execute - self._feedback_topic = 'flexbe/command_feedback' - self._preempt_topic = 'flexbe/command/preempt' - - self._pub = ProxyPublisher() - self._sub = ProxySubscriberCached() - PreemptableState.preempt = False + self._feedback_topic = 'flexbe/command_feedback' + self._preempt_topic = 'flexbe/command/preempt' def _preemptable_execute(self, *args, **kwargs): - preempting = False if self._is_controlled and self._sub.has_msg(self._preempt_topic): self._sub.remove_last_msg(self._preempt_topic) self._pub.publish(self._feedback_topic, CommandFeedback(command="preempt")) - preempting = True PreemptableState.preempt = True - rospy.loginfo("--> Behavior will be preempted") + Logger.localinfo("--> Behavior will be preempted") if PreemptableState.preempt: - preempting = True - rospy.loginfo("Behavior will be preempted") - - if preempting: - self.service_preempt() + if not self._is_controlled: + Logger.localinfo("Behavior will be preempted") self._force_transition = True return self._preempted_name return self.__execute(*args, **kwargs) - 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(self._preempt_topic): @@ -69,7 +48,6 @@ def _notify_skipped(self): self._pub.publish(self._feedback_topic, CommandFeedback(command="preempt")) PreemptableState.preempt = True - def _enable_ros_control(self): super(PreemptableState, self)._enable_ros_control() self._pub.createPublisher(self._feedback_topic, CommandFeedback) @@ -78,4 +56,4 @@ def _enable_ros_control(self): def _disable_ros_control(self): super(PreemptableState, self)._disable_ros_control() - self._sub.unsubscribe_topic(self._preempt_topic) \ No newline at end of file + self._sub.unsubscribe_topic(self._preempt_topic) diff --git a/flexbe_core/src/flexbe_core/core/preemptable_state_machine.py b/flexbe_core/src/flexbe_core/core/preemptable_state_machine.py index 5876c3f1..3fb32c5b 100644 --- a/flexbe_core/src/flexbe_core/core/preemptable_state_machine.py +++ b/flexbe_core/src/flexbe_core/core/preemptable_state_machine.py @@ -1,38 +1,32 @@ #!/usr/bin/env python -import rospy - -from flexbe_core.core.loopback_state_machine import LoopbackStateMachine from flexbe_core.core.preemptable_state import PreemptableState - from flexbe_core.proxy import ProxySubscriberCached + from std_msgs.msg import Empty +from flexbe_core.core.lockable_state_machine import LockableStateMachine -class PreemptableStateMachine(LoopbackStateMachine): + +class PreemptableStateMachine(LockableStateMachine): """ A state machine that can be preempted. If preempted, the state machine will return the outcome preempted. """ - + _preempted_name = 'preempted' - + def __init__(self, *args, **kwargs): - # add preempted outcome - if len(args) > 0: - args[0].append(self._preempted_name) - else: - outcomes = kwargs.get('outcomes', []) - outcomes.append(self._preempted_name) - kwargs['outcomes'] = outcomes - super(PreemptableStateMachine, self).__init__(*args, **kwargs) - # always listen to preempt so that the behavior can be stopped even if unsupervised self._preempt_topic = 'flexbe/command/preempt' self._sub = ProxySubscriberCached({self._preempt_topic: Empty}) self._sub.set_callback(self._preempt_topic, self._preempt_cb) - def _preempt_cb(self, msg): if not self._is_controlled: - PreemptableState.preempt = True \ No newline at end of file + PreemptableState.preempt = True + + @staticmethod + def add(label, state, transitions=None, remapping=None): + transitions[PreemptableState._preempted_name] = PreemptableStateMachine._preempted_name + LockableStateMachine.add(label, state, transitions, remapping) diff --git a/flexbe_core/src/flexbe_core/core/priority_container.py b/flexbe_core/src/flexbe_core/core/priority_container.py index b18bf0c4..583783ab 100644 --- a/flexbe_core/src/flexbe_core/core/priority_container.py +++ b/flexbe_core/src/flexbe_core/core/priority_container.py @@ -1,6 +1,4 @@ #!/usr/bin/env python -import traceback - from flexbe_core.core.operatable_state_machine import OperatableStateMachine @@ -8,23 +6,23 @@ class PriorityContainer(OperatableStateMachine): """ A state machine that is always executed alone when becoming active. """ + active_container = None - + def __init__(self, conditions=dict(), *args, **kwargs): super(PriorityContainer, self).__init__(*args, **kwargs) - self._parent_active_container = None def execute(self, *args, **kwargs): if (PriorityContainer.active_container is None or not all(p == PriorityContainer.active_container.split('/')[i] - for i, p in enumerate(self._get_path().split('/')))): + for i, p in enumerate(self.path.split('/')))): self._parent_active_container = PriorityContainer.active_container - PriorityContainer.active_container = self._get_path() + PriorityContainer.active_container = self.path outcome = OperatableStateMachine.execute(self, *args, **kwargs) - if outcome != self._loopback_name and outcome is not None: + if outcome is not None: PriorityContainer.active_container = self._parent_active_container return outcome diff --git a/flexbe_core/src/flexbe_core/core/ros_state.py b/flexbe_core/src/flexbe_core/core/ros_state.py new file mode 100644 index 00000000..7a8b78a1 --- /dev/null +++ b/flexbe_core/src/flexbe_core/core/ros_state.py @@ -0,0 +1,49 @@ +#!/usr/bin/env python +import rospy +from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached + +from .state import State + + +class RosState(State): + """ + A state to interface with ROS. + """ + + def __init__(self, *args, **kwargs): + super(RosState, self).__init__(*args, **kwargs) + self._rate = rospy.Rate(10) + self._is_controlled = False + + self._pub = ProxyPublisher() + self._sub = ProxySubscriberCached() + + def sleep(self): + self._rate.sleep() + + @property + def sleep_duration(self): + return self._rate.remaining().to_sec() + + def set_rate(self, rate): + """ + Set the execution rate of this state, + i.e., the rate with which the execute method is being called. + + Note: The rate is best-effort, + a rospy.Rate does not guarantee real-time properties. + + @type label: float + @param label: The desired rate in Hz. + """ + self._rate = rospy.Rate(rate) + + def _enable_ros_control(self): + self._is_controlled = True + + def _disable_ros_control(self): + self._is_controlled = False + + @property + def is_breakpoint(self): + return self.path in rospy.get_param('/flexbe/breakpoints', []) diff --git a/flexbe_core/src/flexbe_core/core/ros_state_machine.py b/flexbe_core/src/flexbe_core/core/ros_state_machine.py new file mode 100644 index 00000000..1eec7ae3 --- /dev/null +++ b/flexbe_core/src/flexbe_core/core/ros_state_machine.py @@ -0,0 +1,38 @@ +#!/usr/bin/env python +import rospy +from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached + +from .state_machine import StateMachine + + +class RosStateMachine(StateMachine): + """ + A state machine to interface with ROS. + """ + + def __init__(self, *args, **kwargs): + super(RosStateMachine, self).__init__(*args, **kwargs) + self._is_controlled = False + + self._pub = ProxyPublisher() + self._sub = ProxySubscriberCached() + + def wait(self, seconds=None, condition=None): + if seconds is not None: + rospy.sleep(seconds) + if condition is not None: + rate = rospy.Rate(100) + while not rospy.is_shutdown(): + if condition(): + break + rate.sleep() + + def _enable_ros_control(self): + self._is_controlled = True + for state in self._states: + state._enable_ros_control() + + def _disable_ros_control(self): + self._is_controlled = False + for state in self._states: + state._disable_ros_control() diff --git a/flexbe_core/src/flexbe_core/core/silent_state_machine.py b/flexbe_core/src/flexbe_core/core/silent_state_machine.py deleted file mode 100644 index d0d55ba7..00000000 --- a/flexbe_core/src/flexbe_core/core/silent_state_machine.py +++ /dev/null @@ -1,52 +0,0 @@ -#!/usr/bin/env python - -from .state_machine import StateMachine - -from .preemptable_state_machine import PreemptableStateMachine -from flexbe_core.core.loopback_state import LoopbackState -import rospy - - -class SilentStateMachine(PreemptableStateMachine): - """ - A state machine that runs in background and does not report any transition. - """ - - def __init__(self, *args, **kwargs): - super(SilentStateMachine, self).__init__(*args, **kwargs) - self.transitions = None - self.autonomy = None - - @staticmethod - def add(label, state, transitions = None, autonomy = None, remapping = None): - """ - Add a state to the opened state machine. - - @type label: string - @param label: The label of the state being added. - - @param state: An instance of a class implementing the L{State} interface. - - @param transitions: A dictionary mapping state outcomes to other state - labels or container outcomes. - - @param autonomy: A dictionary mapping state outcomes to their required - autonomy level. Not relevant for this class. - - @param remapping: A dictrionary mapping local userdata keys to userdata - keys in the container. - """ - self = StateMachine._currently_opened_container - - # add loopback transition to loopback states - if isinstance(state, LoopbackState): - transitions[LoopbackState._loopback_name] = label - - StateMachine.add(label, state, transitions, remapping) - - state.transitions = transitions - state.autonomy = None - state._mute = True - - def destroy(self): - pass diff --git a/flexbe_core/src/flexbe_core/core/state.py b/flexbe_core/src/flexbe_core/core/state.py index d6e2e7c8..9badbf98 100644 --- a/flexbe_core/src/flexbe_core/core/state.py +++ b/flexbe_core/src/flexbe_core/core/state.py @@ -4,11 +4,10 @@ class State(object): def __init__(self, *args, **kwargs): - self._outcomes = set(kwargs.get('outcomes', [])) + self._outcomes = list(set(kwargs.get('outcomes', []))) io_keys = kwargs.get('io_keys', []) - self._input_keys = set(kwargs.get('input_keys', []) + io_keys) - self._output_keys = set(kwargs.get('output_keys', []) + io_keys) - self._rate = None + self._input_keys = list(set(kwargs.get('input_keys', []) + io_keys)) + self._output_keys = list(set(kwargs.get('output_keys', []) + io_keys)) # properties of instances of a state machine self._name = None self._parent = None @@ -17,8 +16,23 @@ def execute(self, userdata): pass def sleep(self): - if self._rate is not None: - self._rate.sleep() + pass + + @property + def sleep_duration(self): + return 0. + + @property + def outcomes(self): + return self._outcomes + + @property + def input_keys(self): + return self._input_keys + + @property + def output_keys(self): + return self._output_keys # instance properties @@ -43,20 +57,5 @@ def set_parent(self, value): self._parent = value @property - def outcomes(self): - return self._outcomes - - @property - def input_keys(self): - return self._input_keys - - @property - def output_keys(self): - return self._output_keys - - @property - def sleep_duration(self): - if self._rate is not None: - return self._rate.remaining().to_sec() - else: - return 0. + def path(self): + return "" if self.parent is None else self.parent.path + "/" + self.name diff --git a/flexbe_core/src/flexbe_core/core/state_machine.py b/flexbe_core/src/flexbe_core/core/state_machine.py index 455969dd..e3b35802 100644 --- a/flexbe_core/src/flexbe_core/core/state_machine.py +++ b/flexbe_core/src/flexbe_core/core/state_machine.py @@ -25,11 +25,20 @@ def __exit__(self, *args): StateMachine._currently_opened_container = self._previously_opened_container self._previously_opened_container = None + def __contains__(self, label): + return label in self._labels + + def __getitem__(self, label): + return self._labels[label] + + def __iter__(self): + return iter(state.name for state in self._states) + # construction @staticmethod - def add(label, state, transitions, remappings): - self = StateMachine._currently_opened_container + def add(label, state, transitions, remapping=None): + self = StateMachine.get_opened_container() if label in self._labels: raise ValueError("The label %s has already been added to this state machine!" % label) if label in self._outcomes: @@ -38,11 +47,15 @@ def add(label, state, transitions, remappings): self._states.append(state) self._labels[label] = state self._transitions[label] = transitions - self._remappings[label] = remappings + self._remappings[label] = remapping or dict() # update state instance state.set_name(label) state.set_parent(self) + @staticmethod + def get_opened_container(): + return StateMachine._currently_opened_container + # execution def spin(self, userdata=None): @@ -59,14 +72,14 @@ def execute(self, userdata): if self._current_state is None: self._current_state = self.initial_state self._userdata = userdata - outcome = self._update_once() + outcome = self._execute_current_state() return outcome def sleep(self): if self._current_state is not None: self._current_state.sleep() - def _update_once(self): + def _execute_current_state(self): outcome = self._current_state.execute( UserData(reference=self.userdata, input_keys=self._current_state.input_keys, output_keys=self._current_state.output_keys, remap=self._remappings[self._current_state.name]) @@ -74,7 +87,7 @@ def _update_once(self): if outcome is not None: target = self._transitions[self._current_state.name][outcome] self._current_state = self._labels.get(target) - if self._current_state is None and target in self._outcomes: + if self._current_state is None: return target # properties diff --git a/flexbe_core/src/flexbe_core/logger.py b/flexbe_core/src/flexbe_core/logger.py index fc9c6b4d..98e0e3c5 100644 --- a/flexbe_core/src/flexbe_core/logger.py +++ b/flexbe_core/src/flexbe_core/logger.py @@ -26,12 +26,16 @@ def initialize(): def log(text, severity): if Logger._pub is None: Logger.initialize() - + # send message with logged text msg = BehaviorLog() msg.text = str(text) msg.status_code = severity Logger._pub.publish(msg) + # also log locally + Logger.local(text, severity) + @staticmethod + def local(text, severity): if severity == Logger.REPORT_INFO: rospy.loginfo(text) elif severity == Logger.REPORT_WARN: @@ -46,21 +50,29 @@ def log(text, severity): rospy.logdebug(text + ' (unknown log level %s)' % str(severity)) @staticmethod - def logdebug(text): - Logger.log(text, Logger.REPORT_DEBUG) + def logdebug(text, *args): + Logger.log(text % args, Logger.REPORT_DEBUG) + + @staticmethod + def loginfo(text, *args): + Logger.log(text % args, Logger.REPORT_INFO) + + @staticmethod + def logwarn(text, *args): + Logger.log(text % args, Logger.REPORT_WARN) @staticmethod - def loginfo(text): - Logger.log(text, Logger.REPORT_INFO) + def loghint(text, *args): + Logger.log(text % args, Logger.REPORT_HINT) @staticmethod - def logwarn(text): - Logger.log(text, Logger.REPORT_WARN) + def logerr(text, *args): + Logger.log(text % args, Logger.REPORT_ERROR) @staticmethod - def loghint(text): - Logger.log(text, Logger.REPORT_HINT) + def localdebug(text, *args): + Logger.local(text % args, Logger.REPORT_DEBUG) @staticmethod - def logerr(text): - Logger.log(text, Logger.REPORT_ERROR) + def localinfo(text, *args): + Logger.local(text % args, Logger.REPORT_INFO) diff --git a/flexbe_core/src/flexbe_core/proxy.py b/flexbe_core/src/flexbe_core/proxy.py deleted file mode 100644 index 6d57f2c8..00000000 --- a/flexbe_core/src/flexbe_core/proxy.py +++ /dev/null @@ -1,5 +0,0 @@ -#!/usr/bin/env python - -import roslib; roslib.load_manifest('flexbe_core') - -from .proxy import ProxySubscriberCached, ProxyPublisher, ProxyServiceCaller, ProxyTransformListener diff --git a/flexbe_core/src/flexbe_core/state_logger.py b/flexbe_core/src/flexbe_core/state_logger.py index 71f438e4..4b73ce28 100644 --- a/flexbe_core/src/flexbe_core/state_logger.py +++ b/flexbe_core/src/flexbe_core/state_logger.py @@ -131,7 +131,7 @@ def execute_wrapper(*args, **kwargs): outcome = execute_method(*args, **kwargs) return outcome finally: - if StateLogger.enabled and outcome != cls._loopback_name: + if StateLogger.enabled and outcome is not None: StateLogger.get(name).info(dict( StateLogger._basic(self), outcome=outcome)) @@ -188,7 +188,7 @@ def _basic(state): result.update({ 'name': state.name, 'state': state.__class__.__name__, - 'path': state._get_path() + 'path': state.path }) return result diff --git a/flexbe_mirror/src/flexbe_mirror/flexbe_mirror.py b/flexbe_mirror/src/flexbe_mirror/flexbe_mirror.py index 03a49c87..4e269026 100644 --- a/flexbe_mirror/src/flexbe_mirror/flexbe_mirror.py +++ b/flexbe_mirror/src/flexbe_mirror/flexbe_mirror.py @@ -1,12 +1,9 @@ #!/usr/bin/env python - -import roslib; roslib.load_manifest('flexbe_mirror') import rospy import threading import zlib -from flexbe_core import JumpableStateMachine, LockableStateMachine -from flexbe_core.core import PreemptableState +from flexbe_core.core import PreemptableState, PreemptableStateMachine, LockableStateMachine from .mirror_state import MirrorState from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached @@ -29,12 +26,12 @@ def __init__(self): Constructor ''' self._sm = None - + # set up proxys for sm <--> GUI communication # publish topics self._pub = ProxyPublisher({'flexbe/behavior_update': String, 'flexbe/request_mirror_structure': Int32}) - + self._running = False self._stopping = False self._active_id = 0 @@ -111,7 +108,7 @@ def _start_mirror(self, msg): self._struct_buffer = self._struct_buffer[1:] if struct.behavior_id == self._active_id: self._mirror_state_machine(struct) - rospy.loginfo('Mirror built.') + rospy.loginfo('Mirror built for checksum %s.' % self._active_id) else: rospy.logwarn('Discarded mismatching buffered structure for checksum %s' % str(struct.behavior_id)) @@ -211,8 +208,6 @@ def _execute_mirror(self): rospy.loginfo('Caught exception on preempt:\n%s' % str(e)) result = 'preempted' - if self._sm is not None: - self._sm.destroy() self._running = False rospy.loginfo('Mirror finished with result ' + result) @@ -252,38 +247,22 @@ def _add_node(self, msg, path): if len(container.children) > 0: sm_outcomes = [] for outcome in container.outcomes: sm_outcomes.append(outcome + '_mirror') - sm = JumpableStateMachine(outcomes=sm_outcomes) + sm = PreemptableStateMachine(outcomes=sm_outcomes) with sm: for child in container.children: self._add_node(msg, path+'/'+child) if len(transitions) > 0: container_transitions = {} for i in range(len(container.transitions)): container_transitions[sm_outcomes[i]] = transitions[container.outcomes[i]] - JumpableStateMachine.add(container_name + '_mirror', sm, transitions=container_transitions) + PreemptableStateMachine.add(container_name + '_mirror', sm, transitions=container_transitions) else: self._sm = sm else: - JumpableStateMachine.add(container_name + '_mirror', MirrorState(container_name, path, container.outcomes, container.autonomy), transitions=transitions) - - - def _jump_callback(self, msg): - start_time = rospy.Time.now() - current_elapsed = 0 - jumpable = True - while self._sm is None or not self._sm.is_running(): - current_elapsed = rospy.Time.now() - start_time - if current_elapsed > rospy.Duration.from_sec(10): - jumpable = False - break - rospy.sleep(0.05) - if jumpable: - self._sm.jump_to(msg.stateName) - else: - rospy.logwarn('Could not jump in mirror: Mirror does not exist or is not running!') + PreemptableStateMachine.add(container_name + '_mirror', MirrorState(container_name, path, container.outcomes, container.autonomy), transitions=transitions) def _preempt_callback(self, msg): - if self._sm is not None and self._sm.is_running(): + if self._sm is not None: rospy.logwarn('Explicit preempting is currently ignored, mirror should be preempted by onboard behavior.') else: rospy.logwarn('Could not preempt mirror because it seems not to be running!') diff --git a/flexbe_mirror/src/flexbe_mirror/mirror_state.py b/flexbe_mirror/src/flexbe_mirror/mirror_state.py index aa672893..bd1f12c0 100644 --- a/flexbe_mirror/src/flexbe_mirror/mirror_state.py +++ b/flexbe_mirror/src/flexbe_mirror/mirror_state.py @@ -1,69 +1,45 @@ #!/usr/bin/env python - -import roslib; roslib.load_manifest('flexbe_mirror') import rospy from rospy.exceptions import ROSInterruptException -from flexbe_core import EventState, JumpableStateMachine +from flexbe_core import EventState from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached from std_msgs.msg import String, UInt8 -''' -Created on 07.05.2013 - -@author: Philipp Schillinger -''' class MirrorState(EventState): ''' - This state will display its possible outcomes as buttons in the GUI and is designed in a way to be created dynamically. + This state will display its possible outcomes as buttons in the GUI + and is designed in a way to be created dynamically. ''' - def __init__(self, target_name, target_path, given_outcomes, outcome_autonomy): ''' Constructor ''' super(MirrorState, self).__init__(outcomes=given_outcomes) - self._rate = rospy.Rate(100) - self._given_outcomes = given_outcomes - self._outcome_autonomy = outcome_autonomy + self.set_rate(100) self._target_name = target_name self._target_path = target_path - + self._outcome_topic = 'flexbe/mirror/outcome' - self._pub = ProxyPublisher() #{'flexbe/behavior_update': String} + self._pub = ProxyPublisher() self._sub = ProxySubscriberCached({self._outcome_topic: UInt8}) - - + def execute(self, userdata): ''' Execute this state ''' - if JumpableStateMachine.refresh: - JumpableStateMachine.refresh = False - self.on_enter(userdata) - if self._sub.has_buffered(self._outcome_topic): msg = self._sub.get_from_buffer(self._outcome_topic) - if msg.data < len(self._given_outcomes): - rospy.loginfo("State update: %s > %s", self._target_name, self._given_outcomes[msg.data]) - return self._given_outcomes[msg.data] - + if msg.data < len(self.outcomes): + rospy.loginfo("State update: %s > %s", self._target_name, self.outcomes[msg.data]) + return self.outcomes[msg.data] try: - self._rate.sleep() + self.sleep() except ROSInterruptException: print('Interrupted mirror sleep.') - - + def on_enter(self, userdata): - #rospy.loginfo("Mirror entering %s", self._target_path) self._pub.publish('flexbe/behavior_update', String("/" + "/".join(self._target_path.split("/")[1:]))) - - - - - - - diff --git a/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py b/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py index 55095aef..191704aa 100644 --- a/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py +++ b/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py @@ -111,7 +111,7 @@ def _behavior_execution(self, msg): return # stop the rest rospy.loginfo('Preempting current behavior version...') - self.be.preempt_for_switch() + self.be.preempt() # execute the behavior with self._run_lock: diff --git a/flexbe_states/src/flexbe_states/decision_state.py b/flexbe_states/src/flexbe_states/decision_state.py index 9abb58c3..a7cbb2f2 100644 --- a/flexbe_states/src/flexbe_states/decision_state.py +++ b/flexbe_states/src/flexbe_states/decision_state.py @@ -41,12 +41,12 @@ def execute(self, userdata): ''' if self._conditions is not None: - outcome = DecisionState._loopback_name + outcome = None try: outcome = str(self._conditions(userdata.input_value)) except Exception as e: Logger.logwarn('Passed no function as predicate!\n%s' % str(e)) - outcome = DecisionState._loopback_name + outcome = None if outcome is not None: return outcome diff --git a/flexbe_testing/src/flexbe_testing/test_interface.py b/flexbe_testing/src/flexbe_testing/test_interface.py index 3f3763d2..c77597e6 100644 --- a/flexbe_testing/src/flexbe_testing/test_interface.py +++ b/flexbe_testing/src/flexbe_testing/test_interface.py @@ -60,10 +60,10 @@ def execute(self, userdata, spin_cb=None): def _execute_state(self, userdata, spin_cb): self._instance.on_start() - outcome = EventState._loopback_name - while outcome == EventState._loopback_name and not rospy.is_shutdown(): + outcome = None + while outcome is None and not rospy.is_shutdown(): outcome = self._instance.execute(userdata) - self._instance._rate.sleep() + self._instance.sleep() spin_cb() self._instance.on_stop() return outcome @@ -73,12 +73,11 @@ def _execute_behavior(self, userdata, spin_cb): self._instance.confirm() # do not execute behavior directly, instead explicitly spin its state machine # this is required here for spinning ROS and processing roslaunch context callbacks - outcome = EventState._loopback_name + outcome = None sm = self._instance._state_machine - sm._set_current_state(sm._initial_state_label) - while outcome == EventState._loopback_name and not rospy.is_shutdown(): - outcome = sm._async_execute(userdata) - sm._rate.sleep() + while outcome is None and not rospy.is_shutdown(): + outcome = sm.execute(userdata) + sm.sleep() spin_cb() userdata.update(sm.userdata) return outcome From e386ea0cacac86b2b4793f039f90b3942a7dd3cd Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Fri, 3 Apr 2020 14:10:12 +0200 Subject: [PATCH 23/57] [flexbe_core] Add test cases for state capabilities --- flexbe_core/CMakeLists.txt | 1 + flexbe_core/test/flexbe_core.test | 5 + flexbe_core/test/test_core.py | 418 ++++++++++++++++++++++++++++++ 3 files changed, 424 insertions(+) create mode 100644 flexbe_core/test/flexbe_core.test create mode 100755 flexbe_core/test/test_core.py diff --git a/flexbe_core/CMakeLists.txt b/flexbe_core/CMakeLists.txt index 6d33d524..a980f8ed 100644 --- a/flexbe_core/CMakeLists.txt +++ b/flexbe_core/CMakeLists.txt @@ -29,4 +29,5 @@ catkin_package( if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) add_rostest(test/flexbe_proxies.test) + add_rostest(test/flexbe_core.test) endif() diff --git a/flexbe_core/test/flexbe_core.test b/flexbe_core/test/flexbe_core.test new file mode 100644 index 00000000..819a6570 --- /dev/null +++ b/flexbe_core/test/flexbe_core.test @@ -0,0 +1,5 @@ + + + + + diff --git a/flexbe_core/test/test_core.py b/flexbe_core/test/test_core.py new file mode 100755 index 00000000..34592cc3 --- /dev/null +++ b/flexbe_core/test/test_core.py @@ -0,0 +1,418 @@ +#!/usr/bin/env python +import unittest +import rospy + +from flexbe_core import EventState, OperatableStateMachine, ConcurrencyContainer +from flexbe_core.core import PreemptableState +from flexbe_core.proxy import ProxySubscriberCached + +from std_msgs.msg import Bool, Empty, UInt8, String +from flexbe_msgs.msg import CommandFeedback, OutcomeRequest + + +class TestSubjectState(EventState): + + def __init__(self): + super(TestSubjectState, self).__init__(outcomes=['done', 'error']) + self.result = None + self.last_events = [] + self.count = 0 + + def execute(self, userdata): + self.count += 1 + return self.result + + def on_enter(self, userdata): + self.last_events.append('on_enter') + + def on_exit(self, userdata): + self.last_events.append('on_exit') + + def on_start(self): + self.last_events.append('on_start') + + def on_stop(self): + self.last_events.append('on_stop') + + def on_pause(self): + self.last_events.append('on_pause') + + def on_resume(self, userdata): + self.last_events.append('on_resume') + + +class TestCore(unittest.TestCase): + + def _create(self): + state = TestSubjectState() + state._enable_ros_control() + with OperatableStateMachine(outcomes=['done', 'error']): + OperatableStateMachine.add('subject', state, + transitions={'done': 'done', 'error': 'error'}, + autonomy={'done': 1, 'error': 2}) + return state + + def _execute(self, state): + state.last_events = [] + return state.parent.execute(None) + + def assertMessage(self, sub, topic, msg, timeout=1): + rate = rospy.Rate(100) + for i in range(int(timeout * 100)): + if sub.has_msg(topic): + received = sub.get_last_msg(topic) + sub.remove_last_msg(topic) + break + rate.sleep() + else: + raise AssertionError('Did not receive message on topic %s, expected:\n%s' + % (topic, str(msg))) + 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) + if isinstance(expected, list): + self.assertListEqual(expected, actual, error) + else: + self.assertEqual(expected, actual, error) + + def assertNoMessage(self, sub, topic, timeout=1): + rate = rospy.Rate(100) + for i in range(int(timeout * 100)): + if sub.has_msg(topic): + received = sub.get_last_msg(topic) + sub.remove_last_msg(topic) + raise AssertionError('Should not receive message on topic %s, but got:\n%s' + % (topic, str(received))) + rate.sleep() + + # Test Cases + + def test_event_state(self): + state = self._create() + fb_topic = 'flexbe/command_feedback' + sub = ProxySubscriberCached({fb_topic: CommandFeedback}) + rospy.sleep(0.2) # wait for pub/sub + + # enter during first execute + self._execute(state) + self.assertListEqual(['on_enter'], state.last_events) + self._execute(state) + self.assertListEqual([], state.last_events) + + # pause and resume as commanded + state._sub._callback(Bool(True), 'flexbe/command/pause') + self._execute(state) + self.assertListEqual(['on_pause'], state.last_events) + self.assertMessage(sub, fb_topic, CommandFeedback(command="pause")) + state.result = 'error' + outcome = self._execute(state) + state.result = None + self.assertIsNone(outcome) + state._sub._callback(Bool(False), 'flexbe/command/pause') + self._execute(state) + self.assertListEqual(['on_resume'], state.last_events) + self.assertMessage(sub, fb_topic, CommandFeedback(command="resume")) + + # repeat triggers exit and enter again + state._sub._callback(Empty(), 'flexbe/command/repeat') + self._execute(state) + self.assertListEqual(['on_exit'], state.last_events) + self.assertMessage(sub, fb_topic, CommandFeedback(command="repeat")) + self._execute(state) + self.assertListEqual(['on_enter'], state.last_events) + self._execute(state) + self.assertListEqual([], state.last_events) + + # exit during last execute when returning an outcome + state.result = 'done' + outcome = self._execute(state) + self.assertListEqual(['on_exit'], state.last_events) + self.assertEqual('done', outcome) + + def test_operatable_state(self): + state = self._create() + out_topic = 'flexbe/mirror/outcome' + req_topic = 'flexbe/outcome_request' + sub = ProxySubscriberCached({out_topic: UInt8, req_topic: OutcomeRequest}) + rospy.sleep(0.2) # wait for pub/sub + + # return outcome in full autonomy, no request + state.result = 'error' + self._execute(state) + self.assertNoMessage(sub, req_topic) + self.assertMessage(sub, out_topic, UInt8(1)) + + # request outcome on same autnomy and clear request on loopback + OperatableStateMachine.autonomy_level = 2 + self._execute(state) + self.assertNoMessage(sub, out_topic) + self.assertMessage(sub, req_topic, OutcomeRequest(outcome=1, target='/subject')) + state.result = None + self._execute(state) + self.assertMessage(sub, req_topic, OutcomeRequest(outcome=255, target='/subject')) + + # still return other outcomes + state.result = 'done' + self._execute(state) + self.assertNoMessage(sub, req_topic) + self.assertMessage(sub, out_topic, UInt8(0)) + + # request outcome on lower autonomy, return outcome after level increase + 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._execute(state) + self.assertMessage(sub, out_topic, UInt8(0)) + + def test_preemptable_state(self): + state = self._create() + fb_topic = 'flexbe/command_feedback' + sub = ProxySubscriberCached({fb_topic: CommandFeedback}) + rospy.sleep(0.2) # wait for pub/sub + + # preempt when trigger variable is set + PreemptableState.preempt = True + outcome = self._execute(state) + self.assertEqual(outcome, PreemptableState._preempted_name) + self.assertRaises(ValueError, lambda: state.parent.current_state) + PreemptableState.preempt = False + outcome = self._execute(state) + self.assertIsNone(outcome) + + # preempt when command is received + state._sub._callback(Empty(), 'flexbe/command/preempt') + outcome = self._execute(state) + self.assertEqual(outcome, PreemptableState._preempted_name) + self.assertRaises(ValueError, lambda: state.parent.current_state) + self.assertMessage(sub, fb_topic, CommandFeedback(command='preempt')) + PreemptableState.preempt = False + + def test_lockable_state(self): + state = self._create() + fb_topic = 'flexbe/command_feedback' + sub = ProxySubscriberCached({fb_topic: CommandFeedback}) + rospy.sleep(0.2) # wait for pub/sub + + # lock and unlock as commanded, return outcome after unlock + state._sub._callback(String('/subject'), 'flexbe/command/lock') + state.result = 'done' + outcome = self._execute(state) + self.assertIsNone(outcome) + self.assertTrue(state._locked) + self.assertMessage(sub, fb_topic, CommandFeedback(command='lock', args=['/subject', '/subject'])) + state.result = None + state._sub._callback(String('/subject'), 'flexbe/command/unlock') + outcome = self._execute(state) + self.assertEqual(outcome, 'done') + self.assertMessage(sub, fb_topic, CommandFeedback(command='unlock', args=['/subject', '/subject'])) + + # lock and unlock without target + state._sub._callback(String(''), 'flexbe/command/lock') + state.result = 'done' + outcome = self._execute(state) + self.assertIsNone(outcome) + self.assertMessage(sub, fb_topic, CommandFeedback(command='lock', args=['/subject', '/subject'])) + state._sub._callback(String(''), 'flexbe/command/unlock') + outcome = self._execute(state) + self.assertEqual(outcome, 'done') + self.assertMessage(sub, fb_topic, CommandFeedback(command='unlock', args=['/subject', '/subject'])) + + # reject invalid lock command + state._sub._callback(String('/invalid'), 'flexbe/command/lock') + 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 + state._sub._callback(String(''), 'flexbe/command/unlock') + self._execute(state) + self.assertMessage(sub, fb_topic, CommandFeedback(command='unlock', args=['', ''])) + + # do not transition out of locked container + state.parent._locked = True + outcome = self._execute(state) + self.assertIsNone(outcome) + state.parent._locked = False + state.result = None + outcome = self._execute(state) + self.assertEqual(outcome, 'done') + + def test_manually_transitionable_state(self): + state = self._create() + fb_topic = 'flexbe/command_feedback' + sub = ProxySubscriberCached({fb_topic: CommandFeedback}) + rospy.sleep(0.2) # wait for pub/sub + + # return requested outcome + state._sub._callback(OutcomeRequest(target='subject', outcome=1), 'flexbe/command/transition') + outcome = self._execute(state) + self.assertEqual(outcome, 'error') + self.assertMessage(sub, fb_topic, CommandFeedback(command='transition', args=['subject', 'subject'])) + + # reject outcome request for different target + state._sub._callback(OutcomeRequest(target='invalid', outcome=1), 'flexbe/command/transition') + outcome = self._execute(state) + self.assertIsNone(outcome) + self.assertMessage(sub, fb_topic, CommandFeedback(command='transition', args=['invalid', 'subject'])) + + def test_ros_state(self): + state = self._create() + + # default rate is 10Hz + start = rospy.get_time() + for i in range(10): + state.sleep() + duration = rospy.get_time() - start + self.assertAlmostEqual(duration, 1., places=2) + self.assertAlmostEqual(state.sleep_duration, .1, places=2) + + # change of rate works as expected + state.set_rate(1) + start = rospy.get_time() + state.sleep() + duration = rospy.get_time() - start + self.assertAlmostEqual(duration, 1., places=2) + self.assertAlmostEqual(state.sleep_duration, 1., places=2) + + def test_cross_combinations(self): + state = self._create() + + # manual transition works on low autonomy + OperatableStateMachine.autonomy_level = 0 + state.result = 'error' + outcome = self._execute(state) + self.assertIsNone(outcome) + state._sub._callback(OutcomeRequest(target='subject', outcome=0), 'flexbe/command/transition') + outcome = self._execute(state) + self.assertEqual(outcome, 'done') + OperatableStateMachine.autonomy_level = 3 + state.result = None + + # manual transition blocked by lock + state._sub._callback(String('/subject'), 'flexbe/command/lock') + outcome = self._execute(state) + self.assertIsNone(outcome) + state._sub._callback(OutcomeRequest(target='subject', outcome=1), 'flexbe/command/transition') + outcome = self._execute(state) + self.assertIsNone(outcome) + state._sub._callback(String('/subject'), 'flexbe/command/unlock') + outcome = self._execute(state) + self.assertEqual(outcome, 'error') + + # preempt works on low autonomy + OperatableStateMachine.autonomy_level = 0 + state.result = 'error' + outcome = self._execute(state) + self.assertIsNone(outcome) + state._sub._callback(Empty(), 'flexbe/command/preempt') + outcome = self._execute(state) + self.assertEqual(outcome, PreemptableState._preempted_name) + PreemptableState.preempt = False + OperatableStateMachine.autonomy_level = 3 + state.result = None + + # preempt also works when locked + state._sub._callback(String('/subject'), 'flexbe/command/lock') + outcome = self._execute(state) + self.assertIsNone(outcome) + state._sub._callback(Empty(), 'flexbe/command/preempt') + outcome = self._execute(state) + self.assertEqual(outcome, PreemptableState._preempted_name) + PreemptableState.preempt = False + state._sub._callback(String('/subject'), 'flexbe/command/unlock') + outcome = self._execute(state) + self.assertIsNone(outcome) + + def test_concurrency_container(self): + cc = ConcurrencyContainer(outcomes=['done', 'error'], + conditions=[ + ('error', [('main', 'error')]), + ('error', [('side', 'error')]), + ('done', [('main', 'done'), ('side', 'done')]) + ]) + with cc: + OperatableStateMachine.add('main', TestSubjectState(), + transitions={'done': 'done', 'error': 'error'}, + autonomy={'done': 1, 'error': 2}) + OperatableStateMachine.add('side', TestSubjectState(), + transitions={'done': 'done', 'error': 'error'}, + autonomy={'done': 1, 'error': 2}) + with OperatableStateMachine(outcomes=['done', 'error']): + OperatableStateMachine.add('cc', cc, + transitions={'done': 'done', 'error': 'error'}, + autonomy={'done': 1, 'error': 2}) + + class FakeRate(object): + + def remaining(self): + return rospy.Duration(0) + + def sleep(self): + pass + + # all states are called with their correct rate + cc.execute(None) + cc.sleep() + cc.execute(None) + self.assertAlmostEqual(cc.sleep_duration, .1, places=2) + cc.sleep() + cc['main'].set_rate(15) + cc['side'].set_rate(10) + cc['main'].count = 0 + cc['side'].count = 0 + start = rospy.get_time() + cc_count = 0 + while rospy.get_time() - start <= 1.: + cc_count += 1 + cc.execute(None) + self.assertLessEqual(cc.sleep_duration, .1) + cc.sleep() + self.assertIn(cc['main'].count, [14, 15, 16]) + self.assertIn(cc['side'].count, [9, 10, 11]) + self.assertLessEqual(cc_count, 27) + + # verify ROS properties and disable sleep + cc._enable_ros_control() + self.assertTrue(cc['main']._is_controlled) + self.assertFalse(cc['side']._is_controlled) + cc['main']._rate = FakeRate() + cc['side']._rate = FakeRate() + + # return outcome when all return done or any returns error + outcome = cc.execute(None) + self.assertIsNone(outcome) + cc['main'].result = 'error' + outcome = cc.execute(None) + self.assertEqual(outcome, 'error') + cc['main'].result = None + cc['side'].result = 'error' + outcome = cc.execute(None) + self.assertEqual(outcome, 'error') + cc['side'].result = 'done' + outcome = cc.execute(None) + self.assertIsNone(outcome) + cc['main'].result = 'done' + outcome = cc.execute(None) + self.assertEqual(outcome, 'done') + cc['main'].result = None + cc['side'].result = None + + # always call on_exit exactly once when returning an outcome + outcome = cc.execute(None) + self.assertIsNone(outcome) + cc['main'].last_events = [] + cc['side'].last_events = [] + cc['main'].result = 'error' + outcome = cc.execute(None) + self.assertEqual(outcome, 'error') + self.assertListEqual(cc['main'].last_events, ['on_exit']) + self.assertListEqual(cc['side'].last_events, ['on_exit']) + + +if __name__ == '__main__': + rospy.init_node('test_flexbe_cores') + import rostest + rostest.rosrun('flexbe_core', 'test_flexbe_core', TestCore) From c8c3684625c4a054e0e9d773fbc055ab05df6882 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Fri, 3 Apr 2020 15:18:52 +0200 Subject: [PATCH 24/57] [flexbe_core] Add checks and specific exception types --- flexbe_core/CMakeLists.txt | 1 + flexbe_core/src/flexbe_core/__init__.py | 37 ++---- flexbe_core/src/flexbe_core/core/__init__.py | 26 +++-- .../flexbe_core/core/concurrency_container.py | 35 +++--- .../src/flexbe_core/core/exceptions.py | 13 +++ .../core/lockable_state_machine.py | 4 +- .../src/flexbe_core/core/operatable_state.py | 2 +- .../core/operatable_state_machine.py | 10 +- .../core/preemptable_state_machine.py | 4 + flexbe_core/src/flexbe_core/core/ros_state.py | 2 +- .../src/flexbe_core/core/ros_state_machine.py | 2 +- flexbe_core/src/flexbe_core/core/state.py | 5 +- .../src/flexbe_core/core/state_machine.py | 35 ++++-- flexbe_core/src/flexbe_core/core/user_data.py | 7 +- flexbe_core/src/flexbe_core/proxy/__init__.py | 10 +- flexbe_core/test/flexbe_exceptions.test | 5 + flexbe_core/test/test_core.py | 7 +- flexbe_core/test/test_exceptions.py | 106 ++++++++++++++++++ 18 files changed, 230 insertions(+), 81 deletions(-) create mode 100644 flexbe_core/src/flexbe_core/core/exceptions.py create mode 100644 flexbe_core/test/flexbe_exceptions.test create mode 100755 flexbe_core/test/test_exceptions.py diff --git a/flexbe_core/CMakeLists.txt b/flexbe_core/CMakeLists.txt index a980f8ed..4f6c3596 100644 --- a/flexbe_core/CMakeLists.txt +++ b/flexbe_core/CMakeLists.txt @@ -30,4 +30,5 @@ if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) add_rostest(test/flexbe_proxies.test) add_rostest(test/flexbe_core.test) + add_rostest(test/flexbe_exceptions.test) endif() diff --git a/flexbe_core/src/flexbe_core/__init__.py b/flexbe_core/src/flexbe_core/__init__.py index 56c380dc..b2ceab99 100644 --- a/flexbe_core/src/flexbe_core/__init__.py +++ b/flexbe_core/src/flexbe_core/__init__.py @@ -1,34 +1,32 @@ #!/usr/bin/env python - # # Please use EventState as parent class for new states because it extends all other parent classes. # For a behavior, choose OperatableStateMachine as state machine. -# For each state machine in the background, choose SilentStateMachine or JumpableStateMachine (only do this if you know what you are doing). # -from .core import EventState -from .core import OperatableStateMachine, ConcurrencyContainer, PriorityContainer +from .core import EventState # noqa: F401 +from .core import OperatableStateMachine, ConcurrencyContainer, PriorityContainer # noqa: F401 -from .behavior import Behavior +from .behavior import Behavior # noqa: F401 -from .behavior_library import BehaviorLibrary +from .behavior_library import BehaviorLibrary # noqa: F401 -from .logger import Logger -from .state_logger import StateLogger +from .logger import Logger # noqa: F401 +from .state_logger import StateLogger # noqa: F401 class Autonomy: """ Provides constants for the available required Autonomy Levels. """ - + Inherit = 0 """ Use this whenever you want to rely on an already defined level of autonomy Typical situations: Outcomes of a contained state machine or behavior, outcome of the input request state """ - + Off = 0 """ Use this when no level of autonomy is required, the Autonomy Level needs to be at least 'Off'. @@ -39,10 +37,11 @@ class Autonomy: """ Use this for reliable decisions that only need validation in the 'low' autonomy mode. """ - + High = 2 """ - Use this for more important or high level decisions that will only be executed autonomously when in full autonomy and need validation in the 'high' autonomy mode. + Use this for more important or high level decisions that will only be executed autonomously + when in full autonomy and need validation in the 'high' autonomy mode. """ Full = 3 @@ -50,17 +49,3 @@ class Autonomy: Use this for outcomes that always need an operator input. A use of this level is not recommended. """ - - - - - - - - - - - - - - diff --git a/flexbe_core/src/flexbe_core/core/__init__.py b/flexbe_core/src/flexbe_core/core/__init__.py index db0ada98..8f58e408 100644 --- a/flexbe_core/src/flexbe_core/core/__init__.py +++ b/flexbe_core/src/flexbe_core/core/__init__.py @@ -1,12 +1,18 @@ -from .preemptable_state_machine import PreemptableStateMachine -from .operatable_state_machine import OperatableStateMachine -from .lockable_state_machine import LockableStateMachine +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 -from .concurrency_container import ConcurrencyContainer -from .priority_container import PriorityContainer +from .concurrency_container import ConcurrencyContainer # noqa: F401 +from .priority_container import PriorityContainer # noqa: F401 -from .manually_transitionable_state import ManuallyTransitionableState -from .lockable_state import LockableState -from .preemptable_state import PreemptableState -from .operatable_state import OperatableState -from .event_state import EventState +from .state import State # noqa: F401 +from .ros_state import RosState # noqa: F401 +from .manually_transitionable_state import ManuallyTransitionableState # noqa: F401 +from .lockable_state import LockableState # noqa: F401 +from .preemptable_state import PreemptableState # noqa: F401 +from .operatable_state import OperatableState # noqa: F401 +from .event_state import EventState # noqa: F401 + +from .user_data import UserData # noqa: F401 diff --git a/flexbe_core/src/flexbe_core/core/concurrency_container.py b/flexbe_core/src/flexbe_core/core/concurrency_container.py index fe25b2d7..4ba7d8c1 100644 --- a/flexbe_core/src/flexbe_core/core/concurrency_container.py +++ b/flexbe_core/src/flexbe_core/core/concurrency_container.py @@ -1,6 +1,5 @@ #!/usr/bin/env python -import traceback - +from flexbe_core.logger import Logger from flexbe_core.core.user_data import UserData from flexbe_core.core.event_state import EventState from flexbe_core.core.priority_container import PriorityContainer @@ -69,21 +68,16 @@ def _execute_current_state(self): if outcome is None: return None - if outcome in self.outcomes: - # trigger on_exit for those states that are not done yet - self.on_exit(self.userdata, - states=[s for s in self._states if (s.name not in list(self._returned_outcomes.keys()) or - self._returned_outcomes[s.name] is None)]) - self._returned_outcomes = dict() - # right now, going out of a cc may break sync - # thus, as a quick fix, explicitly sync again - self._parent._inner_sync_request = True - self._current_state = None - return outcome - else: - raise ValueError("Outcome '%s' of state '%s' with transition target '%s' " - "is neither a registered state nor a registered container outcome." % - (outcome, self.name, outcome)) + # trigger on_exit for those states that are not done yet + self.on_exit(self.userdata, + states=[s for s in self._states if (s.name not in list(self._returned_outcomes.keys()) or + self._returned_outcomes[s.name] is None)]) + self._returned_outcomes = dict() + # right now, going out of a cc may break sync + # thus, as a quick fix, explicitly sync again + self._parent._inner_sync_request = True + self._current_state = None + return outcome def _execute_single_state(self, state, force_exit=False): result = None @@ -95,9 +89,10 @@ def _execute_single_state(self, state, force_exit=False): state.on_exit(ud) else: result = state.execute(ud) - except Exception: - raise RuntimeError("Could not execute state '%s' of type '%s': " % - (state.name, state) + traceback.format_exc()) + except Exception as e: + result = None + self._last_exception = e + Logger.logerr('Failed to execute state %s:\n%s' % (self.current_state_label, str(e))) return result def _enable_ros_control(self): diff --git a/flexbe_core/src/flexbe_core/core/exceptions.py b/flexbe_core/src/flexbe_core/core/exceptions.py new file mode 100644 index 00000000..700f2231 --- /dev/null +++ b/flexbe_core/src/flexbe_core/core/exceptions.py @@ -0,0 +1,13 @@ +#!/usr/bin/env python + + +class StateError(Exception): + pass + + +class StateMachineError(Exception): + pass + + +class UserDataError(Exception): + pass diff --git a/flexbe_core/src/flexbe_core/core/lockable_state_machine.py b/flexbe_core/src/flexbe_core/core/lockable_state_machine.py index 2b15328f..c09bc041 100644 --- a/flexbe_core/src/flexbe_core/core/lockable_state_machine.py +++ b/flexbe_core/src/flexbe_core/core/lockable_state_machine.py @@ -1,5 +1,5 @@ #!/usr/bin/env python -from .ros_state_machine import RosStateMachine +from flexbe_core.core.ros_state_machine import RosStateMachine class LockableStateMachine(RosStateMachine): @@ -32,7 +32,7 @@ def _is_internal_transition(self, transition_target): def transition_allowed(self, state, outcome): if outcome is None or outcome == 'None': return True - transition_target = self._transitions[state][outcome] + transition_target = self._transitions[state].get(outcome) return (self._is_internal_transition(transition_target) or (not self._locked and (self.parent is None or self.parent.transition_allowed(self.name, transition_target)))) diff --git a/flexbe_core/src/flexbe_core/core/operatable_state.py b/flexbe_core/src/flexbe_core/core/operatable_state.py index ec804943..3f4af3af 100644 --- a/flexbe_core/src/flexbe_core/core/operatable_state.py +++ b/flexbe_core/src/flexbe_core/core/operatable_state.py @@ -51,7 +51,7 @@ def _operatable_execute(self, *args, **kwargs): outcome = None # autonomy level is high enough, report the executed transition - elif outcome is not None and outcome != self._preempted_name: + elif outcome is not None and outcome in self.outcomes: Logger.localinfo("State result: %s > %s" % (self.name, outcome)) self._pub.publish(self._outcome_topic, UInt8(self.outcomes.index(outcome))) self._pub.publish(self._debug_topic, String("%s > %s" % (self.path, outcome))) diff --git a/flexbe_core/src/flexbe_core/core/operatable_state_machine.py b/flexbe_core/src/flexbe_core/core/operatable_state_machine.py index f633bc48..ed3f0c28 100644 --- a/flexbe_core/src/flexbe_core/core/operatable_state_machine.py +++ b/flexbe_core/src/flexbe_core/core/operatable_state_machine.py @@ -24,6 +24,7 @@ def __init__(self, *args, **kwargs): self.id = None self._autonomy = {} self._inner_sync_request = False + self._last_exception = None # construction @@ -89,7 +90,14 @@ def _add_to_structure_msg(self, structure_msg): # execution def _execute_current_state(self): - outcome = super(OperatableStateMachine, self)._execute_current_state() + # catch any exception and keep state active to let operator intervene + try: + outcome = super(OperatableStateMachine, self)._execute_current_state() + self._last_exception = None + except Exception as e: + outcome = None + self._last_exception = e + Logger.logerr('Failed to execute state %s:\n%s' % (self.current_state_label, str(e))) # provide explicit sync as back-up functionality # should be used only if there is no other choice # since it requires additional 8 byte + header update bandwith and time to restart mirror diff --git a/flexbe_core/src/flexbe_core/core/preemptable_state_machine.py b/flexbe_core/src/flexbe_core/core/preemptable_state_machine.py index 3fb32c5b..ad355c33 100644 --- a/flexbe_core/src/flexbe_core/core/preemptable_state_machine.py +++ b/flexbe_core/src/flexbe_core/core/preemptable_state_machine.py @@ -30,3 +30,7 @@ def _preempt_cb(self, msg): def add(label, state, transitions=None, remapping=None): transitions[PreemptableState._preempted_name] = PreemptableStateMachine._preempted_name LockableStateMachine.add(label, state, transitions, remapping) + + @property + def _valid_targets(self): + return super(PreemptableStateMachine, self)._valid_targets + [PreemptableStateMachine._preempted_name] diff --git a/flexbe_core/src/flexbe_core/core/ros_state.py b/flexbe_core/src/flexbe_core/core/ros_state.py index 7a8b78a1..29df6bbe 100644 --- a/flexbe_core/src/flexbe_core/core/ros_state.py +++ b/flexbe_core/src/flexbe_core/core/ros_state.py @@ -2,7 +2,7 @@ import rospy from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached -from .state import State +from flexbe_core.core.state import State class RosState(State): diff --git a/flexbe_core/src/flexbe_core/core/ros_state_machine.py b/flexbe_core/src/flexbe_core/core/ros_state_machine.py index 1eec7ae3..19a5a93e 100644 --- a/flexbe_core/src/flexbe_core/core/ros_state_machine.py +++ b/flexbe_core/src/flexbe_core/core/ros_state_machine.py @@ -2,7 +2,7 @@ import rospy from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached -from .state_machine import StateMachine +from flexbe_core.core.state_machine import StateMachine class RosStateMachine(StateMachine): diff --git a/flexbe_core/src/flexbe_core/core/state.py b/flexbe_core/src/flexbe_core/core/state.py index 9badbf98..425fc249 100644 --- a/flexbe_core/src/flexbe_core/core/state.py +++ b/flexbe_core/src/flexbe_core/core/state.py @@ -1,4 +1,5 @@ #!/usr/bin/env python +from flexbe_core.core.exceptions import StateError class State(object): @@ -42,7 +43,7 @@ def name(self): def set_name(self, value): if self._name is not None: - raise ValueError("Cannot change the name of a state!") + raise StateError("Cannot change the name of a state!") else: self._name = value @@ -52,7 +53,7 @@ def parent(self): def set_parent(self, value): if self._parent is not None: - raise ValueError("Cannot change the parent of a state!") + raise StateError("Cannot change the parent of a state!") else: self._parent = value diff --git a/flexbe_core/src/flexbe_core/core/state_machine.py b/flexbe_core/src/flexbe_core/core/state_machine.py index e3b35802..e6c1e9e8 100644 --- a/flexbe_core/src/flexbe_core/core/state_machine.py +++ b/flexbe_core/src/flexbe_core/core/state_machine.py @@ -1,6 +1,7 @@ #!/usr/bin/env python -from .state import State -from .user_data import UserData +from flexbe_core.core.state import State +from flexbe_core.core.user_data import UserData +from flexbe_core.core.exceptions import StateError, StateMachineError class StateMachine(State): @@ -39,10 +40,12 @@ def __iter__(self): @staticmethod def add(label, state, transitions, remapping=None): self = StateMachine.get_opened_container() + if self is None: + raise StateMachineError("No container openend, activate one first by: 'with state_machine:'") if label in self._labels: - raise ValueError("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 ValueError("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 @@ -70,6 +73,7 @@ def spin(self, userdata=None): def execute(self, userdata): if self._current_state is None: + self.assert_consistent_transitions() self._current_state = self.initial_state self._userdata = userdata outcome = self._execute_current_state() @@ -85,7 +89,11 @@ def _execute_current_state(self): output_keys=self._current_state.output_keys, remap=self._remappings[self._current_state.name]) ) if outcome is not None: - target = self._transitions[self._current_state.name][outcome] + try: + target = self._transitions[self._current_state.name][outcome] + except KeyError as e: + outcome = None + raise StateError("Returned outcome '%s' is not registered as transition!" % str(e)) self._current_state = self._labels.get(target) if self._current_state is None: return target @@ -101,7 +109,7 @@ def current_state(self): if self._current_state is not None: return self._current_state else: - raise ValueError("No state active!") + raise StateMachineError("No state active!") @property def current_state_label(self): @@ -112,7 +120,7 @@ def initial_state(self): if len(self._states) > 0: return self._states[0] else: - raise ValueError("No states added yet!") + raise StateMachineError("No states added yet!") @property def initial_state_label(self): @@ -124,3 +132,16 @@ def sleep_duration(self): return self._current_state.sleep_duration else: return 0. + + # consistency checks + + @property + def _valid_targets(self): + return self._labels.keys() + self.outcomes + + def assert_consistent_transitions(self): + for transitions in self._transitions.values(): + for transition_target in transitions.values(): + if transition_target not in self._valid_targets: + raise StateMachineError("Transition target '%s' missing in %s" % + (transition_target, str(self._valid_targets))) diff --git a/flexbe_core/src/flexbe_core/core/user_data.py b/flexbe_core/src/flexbe_core/core/user_data.py index 4a8ffb0e..20cfa228 100644 --- a/flexbe_core/src/flexbe_core/core/user_data.py +++ b/flexbe_core/src/flexbe_core/core/user_data.py @@ -1,4 +1,5 @@ #!/user/bin/env python +from flexbe_core.core.exceptions import UserDataError class UserData(object): @@ -17,12 +18,14 @@ def __contains__(self, key): def __getitem__(self, key): if key not in self: - raise KeyError("Key %s not contained, make sure it is defined as input key." % key) + raise UserDataError("Key '%s' cannot be accessed, declare it as input key for read access." % key + if self._input_keys is not None and key not in self._input_keys else + "No data found for declared input key '%s'" % key) return self._data[self._remap.get(key, key)] def __setitem__(self, key, value): if self._output_keys is not None and key not in self._output_keys: - raise KeyError("Key %s cannot be set, make sure it is defined as output key." % key) + raise UserDataError("Key '%s' cannot be set, declare it as output key for write access." % key) self._data[self._remap.get(key, key)] = value def __getattr__(self, key): diff --git a/flexbe_core/src/flexbe_core/proxy/__init__.py b/flexbe_core/src/flexbe_core/proxy/__init__.py index f9f390a4..d7e4da99 100644 --- a/flexbe_core/src/flexbe_core/proxy/__init__.py +++ b/flexbe_core/src/flexbe_core/proxy/__init__.py @@ -1,5 +1,5 @@ -from .proxy_subscriber_cached import ProxySubscriberCached -from .proxy_publisher import ProxyPublisher -from .proxy_service_caller import ProxyServiceCaller -from .proxy_action_client import ProxyActionClient -from .proxy_transform_listener import ProxyTransformListener +from .proxy_subscriber_cached import ProxySubscriberCached # noqa: F401 +from .proxy_publisher import ProxyPublisher # noqa: F401 +from .proxy_service_caller import ProxyServiceCaller # noqa: F401 +from .proxy_action_client import ProxyActionClient # noqa: F401 +from .proxy_transform_listener import ProxyTransformListener # noqa: F401 diff --git a/flexbe_core/test/flexbe_exceptions.test b/flexbe_core/test/flexbe_exceptions.test new file mode 100644 index 00000000..a83a4df8 --- /dev/null +++ b/flexbe_core/test/flexbe_exceptions.test @@ -0,0 +1,5 @@ + + + + + diff --git a/flexbe_core/test/test_core.py b/flexbe_core/test/test_core.py index 34592cc3..867e2b76 100755 --- a/flexbe_core/test/test_core.py +++ b/flexbe_core/test/test_core.py @@ -5,6 +5,7 @@ from flexbe_core import EventState, OperatableStateMachine, ConcurrencyContainer from flexbe_core.core import PreemptableState from flexbe_core.proxy import ProxySubscriberCached +from flexbe_core.core.exceptions import StateMachineError from std_msgs.msg import Bool, Empty, UInt8, String from flexbe_msgs.msg import CommandFeedback, OutcomeRequest @@ -177,7 +178,7 @@ def test_preemptable_state(self): PreemptableState.preempt = True outcome = self._execute(state) self.assertEqual(outcome, PreemptableState._preempted_name) - self.assertRaises(ValueError, lambda: state.parent.current_state) + self.assertRaises(StateMachineError, lambda: state.parent.current_state) PreemptableState.preempt = False outcome = self._execute(state) self.assertIsNone(outcome) @@ -186,7 +187,7 @@ def test_preemptable_state(self): state._sub._callback(Empty(), 'flexbe/command/preempt') outcome = self._execute(state) self.assertEqual(outcome, PreemptableState._preempted_name) - self.assertRaises(ValueError, lambda: state.parent.current_state) + self.assertRaises(StateMachineError, lambda: state.parent.current_state) self.assertMessage(sub, fb_topic, CommandFeedback(command='preempt')) PreemptableState.preempt = False @@ -413,6 +414,6 @@ def sleep(self): if __name__ == '__main__': - rospy.init_node('test_flexbe_cores') + rospy.init_node('test_flexbe_core') import rostest rostest.rosrun('flexbe_core', 'test_flexbe_core', TestCore) diff --git a/flexbe_core/test/test_exceptions.py b/flexbe_core/test/test_exceptions.py new file mode 100755 index 00000000..cdb6e985 --- /dev/null +++ b/flexbe_core/test/test_exceptions.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python +import unittest +import rospy + +from flexbe_core import EventState, OperatableStateMachine +from flexbe_core.core.exceptions import StateError, StateMachineError, UserDataError + + +class TestExceptions(unittest.TestCase): + + def test_invalid_outcome(self): + class ReturnInvalidOutcome(EventState): + + def __init__(self): + super(ReturnInvalidOutcome, self).__init__(outcomes=['done']) + + def execute(self, userdata): + return 'invalid' + + sm = OperatableStateMachine(outcomes=['done']) + with sm: + OperatableStateMachine.add('state', ReturnInvalidOutcome(), transitions={'done': 'done'}) + + outcome = sm.execute(None) + self.assertIsNone(outcome) + self.assertIsInstance(sm._last_exception, StateError) + + def test_invalid_transition(self): + class ReturnDone(EventState): + + def __init__(self): + super(ReturnDone, self).__init__(outcomes=['done']) + + def execute(self, userdata): + return 'done' + + inner_sm = OperatableStateMachine(outcomes=['done']) + with inner_sm: + OperatableStateMachine.add('state', ReturnDone(), transitions={'done': 'invalid'}) + sm = OperatableStateMachine(outcomes=['done']) + with sm: + OperatableStateMachine.add('inner', inner_sm, transitions={'done': 'done'}) + + outcome = sm.execute(None) + self.assertIsNone(outcome) + self.assertIsInstance(sm._last_exception, StateMachineError) + + def test_invalid_userdata_input(self): + class AccessInvalidInput(EventState): + + def __init__(self): + super(AccessInvalidInput, self).__init__(outcomes=['done'], input_keys=['input']) + + def execute(self, userdata): + print(userdata.invalid) + return 'done' + + sm = OperatableStateMachine(outcomes=['done']) + with sm: + OperatableStateMachine.add('state', AccessInvalidInput(), transitions={'done': 'done'}) + + outcome = sm.execute(None) + self.assertIsNone(outcome) + self.assertIsInstance(sm._last_exception, UserDataError) + + def test_invalid_userdata_output(self): + class SetInvalidOutput(EventState): + + def __init__(self): + super(SetInvalidOutput, self).__init__(outcomes=['done'], output_keys=['output']) + + def execute(self, userdata): + userdata.invalid = False + return 'done' + + sm = OperatableStateMachine(outcomes=['done']) + with sm: + OperatableStateMachine.add('state', SetInvalidOutput(), transitions={'done': 'done'}) + + outcome = sm.execute(None) + self.assertIsNone(outcome) + self.assertIsInstance(sm._last_exception, UserDataError) + + def test_missing_userdata(self): + class AccessValidInput(EventState): + + def __init__(self): + super(AccessValidInput, self).__init__(outcomes=['done'], input_keys=['missing']) + + def execute(self, userdata): + print(userdata.missing) + return 'done' + + sm = OperatableStateMachine(outcomes=['done']) + with sm: + OperatableStateMachine.add('state', AccessValidInput(), transitions={'done': 'done'}) + + outcome = sm.execute(None) + self.assertIsNone(outcome) + self.assertIsInstance(sm._last_exception, UserDataError) + + +if __name__ == '__main__': + rospy.init_node('test_flexbe_exceptions') + import rostest + rostest.rosrun('flexbe_core', 'test_flexbe_exceptions', TestExceptions) From 9c47ebfd18f4290340d39d6ab3b7be8c52389027 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Fri, 10 Apr 2020 20:36:39 +0200 Subject: [PATCH 25/57] [flexbe_core] Add userdata tests and improvements --- .../flexbe_core/core/concurrency_container.py | 14 ++-- .../src/flexbe_core/core/state_machine.py | 18 ++--- flexbe_core/src/flexbe_core/core/user_data.py | 60 +++++++++++++-- flexbe_core/test/test_core.py | 75 +++++++++++++++++++ flexbe_core/test/test_exceptions.py | 19 +++++ 5 files changed, 162 insertions(+), 24 deletions(-) diff --git a/flexbe_core/src/flexbe_core/core/concurrency_container.py b/flexbe_core/src/flexbe_core/core/concurrency_container.py index 4ba7d8c1..4f7ee87a 100644 --- a/flexbe_core/src/flexbe_core/core/concurrency_container.py +++ b/flexbe_core/src/flexbe_core/core/concurrency_container.py @@ -82,13 +82,13 @@ def _execute_current_state(self): def _execute_single_state(self, state, force_exit=False): result = None try: - ud = UserData(reference=self.userdata, input_keys=state.input_keys, - output_keys=state.output_keys, remap=self._remappings[state.name]) - if force_exit: - state._entering = True - state.on_exit(ud) - else: - result = state.execute(ud) + with UserData(reference=self.userdata, remap=self._remappings[state.name], + input_keys=state.input_keys, output_keys=state.output_keys) as userdata: + if force_exit: + state._entering = True + state.on_exit(userdata) + else: + result = state.execute(userdata) except Exception as e: result = None self._last_exception = e diff --git a/flexbe_core/src/flexbe_core/core/state_machine.py b/flexbe_core/src/flexbe_core/core/state_machine.py index e6c1e9e8..52b24bf5 100644 --- a/flexbe_core/src/flexbe_core/core/state_machine.py +++ b/flexbe_core/src/flexbe_core/core/state_machine.py @@ -15,7 +15,8 @@ def __init__(self, *args, **kwargs): self._transitions = dict() self._remappings = dict() self._current_state = None - self._userdata = UserData() + self._own_userdata = UserData() + self._userdata = None self._previously_opened_container = None def __enter__(self): @@ -62,10 +63,9 @@ def get_opened_container(): # execution def spin(self, userdata=None): - self._userdata = userdata or self._userdata outcome = None while True: - outcome = self.execute(self._userdata) + outcome = self.execute(userdata) if outcome is not None: break self.sleep() @@ -75,7 +75,7 @@ def execute(self, userdata): if self._current_state is None: self.assert_consistent_transitions() self._current_state = self.initial_state - self._userdata = userdata + self._userdata = (userdata or UserData()) + self._own_userdata outcome = self._execute_current_state() return outcome @@ -84,10 +84,10 @@ def sleep(self): self._current_state.sleep() def _execute_current_state(self): - outcome = self._current_state.execute( - UserData(reference=self.userdata, input_keys=self._current_state.input_keys, - output_keys=self._current_state.output_keys, remap=self._remappings[self._current_state.name]) - ) + 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: + outcome = self._current_state.execute(userdata) if outcome is not None: try: target = self._transitions[self._current_state.name][outcome] @@ -102,7 +102,7 @@ def _execute_current_state(self): @property def userdata(self): - return self._userdata + return self._own_userdata @property def current_state(self): diff --git a/flexbe_core/src/flexbe_core/core/user_data.py b/flexbe_core/src/flexbe_core/core/user_data.py index 20cfa228..4d49c5ae 100644 --- a/flexbe_core/src/flexbe_core/core/user_data.py +++ b/flexbe_core/src/flexbe_core/core/user_data.py @@ -5,28 +5,46 @@ class UserData(object): def __init__(self, reference=None, input_keys=None, output_keys=None, remap=None): - self._data = reference._data if reference is not None else dict() + self._data = dict() + self._reference = reference or dict() self._input_keys = input_keys self._output_keys = output_keys self._remap = remap or dict() + self._hashes = dict() + + def __enter__(self): + return self + + def __exit__(self, *args): + for key, value in self._hashes.items(): + if value != hash(repr(self._data[key])): + raise UserDataError("Illegally modified input-only key '%s', declare it as output." % key) def __contains__(self, key): - if self._input_keys is not None and key not in self._input_keys: + if key in self._data: + return True + elif self._input_keys is not None and key not in self._input_keys: return False else: - return self._remap.get(key, key) in self._data + return self._remap.get(key, key) in self._reference def __getitem__(self, key): + if key in self._data: + return self._data[key] if key not in self: raise UserDataError("Key '%s' cannot be accessed, declare it as input key for read access." % key if self._input_keys is not None and key not in self._input_keys else - "No data found for declared input key '%s'" % key) - return self._data[self._remap.get(key, key)] + "No data found for key '%s'" % key) + value = self._reference[self._remap.get(key, 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)) + return value def __setitem__(self, key, value): - if self._output_keys is not None and key not in self._output_keys: - raise UserDataError("Key '%s' cannot be set, declare it as output key for write access." % key) - self._data[self._remap.get(key, key)] = value + if self._output_keys is not None and key in self._output_keys: + self._reference[self._remap.get(key, key)] = value + self._data[key] = value def __getattr__(self, key): if key.startswith('_'): @@ -36,4 +54,30 @@ def __getattr__(self, key): def __setattr__(self, key, value): if key.startswith('_'): return object.__setattr__(self, key, value) + if self._output_keys is not None and key not in self._output_keys: + raise UserDataError("Key '%s' cannot be set, declare it as output key for write access." % key) self[key] = value + + def __call__(self, reference=None): + self._reference = reference or self._reference + return self + + def __add__(self, other): + if not isinstance(other, UserData): + raise UserDataError("Can only add other userdata, not %s" % str(other)) + for key, value in other._data.items(): + self._data[key] = value + return self + + def __len__(self): + return len(self._data) + len(self._reference) + + def __str__(self): + 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" + % (len(self), str(self._input_keys), str(self._output_keys), str(self._data), + str(self._remap), data_str)) diff --git a/flexbe_core/test/test_core.py b/flexbe_core/test/test_core.py index 867e2b76..beef515d 100755 --- a/flexbe_core/test/test_core.py +++ b/flexbe_core/test/test_core.py @@ -412,6 +412,81 @@ def sleep(self): self.assertListEqual(cc['main'].last_events, ['on_exit']) self.assertListEqual(cc['side'].last_events, ['on_exit']) + def test_user_data(self): + class TestUserdata(EventState): + + def __init__(self, out_content='test_data'): + super(TestUserdata, self).__init__(outcomes=['done'], input_keys=['data_in'], output_keys=['data_out']) + self.data = None + self._out_content = out_content + + def execute(self, userdata): + rospy.logwarn('\033[0m%s\n%s' % (self.path, str(userdata))) # log for manual inspection + self.data = userdata.data_in + userdata.data_out = self._out_content + return 'done' + + inner_sm = OperatableStateMachine(outcomes=['done'], input_keys=['sm_in'], output_keys=['sm_out']) + inner_sm.userdata.own = 'own_data' + with inner_sm: + OperatableStateMachine.add('own_state', TestUserdata('inner_data'), transitions={'done': 'outside_state'}, + remapping={'data_in': 'own', 'data_out': 'sm_out'}) + OperatableStateMachine.add('outside_state', TestUserdata(), transitions={'done': 'internal_state'}, + remapping={'data_in': 'sm_in', 'data_out': 'data_in'}) + OperatableStateMachine.add('internal_state', TestUserdata(), transitions={'done': 'done'}, + remapping={}) + + sm = OperatableStateMachine(outcomes=['done']) + sm.userdata.outside = 'outside_data' + with sm: + OperatableStateMachine.add('before_state', TestUserdata(), transitions={'done': 'inner_sm'}, + remapping={'data_in': 'outside'}) + OperatableStateMachine.add('inner_sm', inner_sm, transitions={'done': 'after_state'}, + remapping={'sm_in': 'outside'}) + OperatableStateMachine.add('after_state', TestUserdata('last_data'), transitions={'done': 'modify_state'}, + remapping={'data_in': 'sm_out'}) + OperatableStateMachine.add('modify_state', TestUserdata(), transitions={'done': 'final_state'}, + remapping={'data_out': 'outside', 'data_in': 'outside'}) + OperatableStateMachine.add('final_state', TestUserdata(), transitions={'done': 'done'}, + remapping={'data_in': 'data_out'}) + + # can pass userdata to state and get it from state + sm.execute(None) + self.assertEqual(sm['before_state'].data, 'outside_data') + self.assertEqual(sm._userdata.data_out, 'test_data') + + # sub-state machine can set its own local userdata + sm.execute(None) + self.assertEqual(sm['inner_sm']['own_state'].data, 'own_data') + self.assertNotIn('own', sm._userdata) # transparent to outer sm + + # sub-state machine can read data from parent state machine + sm.execute(None) + self.assertEqual(sm['inner_sm']['outside_state'].data, 'outside_data') + + # sub-state machine can pass along its local userdata + self.assertIn('data_in', sm['inner_sm']._userdata) + sm.execute(None) + self.assertEqual(sm['inner_sm']['internal_state'].data, 'test_data') + self.assertNotIn('data_in', sm._userdata) # transparent to outer sm + + # sub-state machine userdata is wrote back to the parent + self.assertEqual(sm._userdata.sm_out, 'inner_data') + + # outer state machine can read data set by inner state machine + sm.execute(None) + self.assertEqual(sm['after_state'].data, 'inner_data') + + # can remap different keys to achieve read-write access + sm.execute(None) + self.assertEqual(sm['modify_state'].data, 'outside_data') + self.assertEqual(sm._userdata.outside, 'test_data') + + # one state can read data set by another one + outcome = sm.execute(None) + self.assertEqual(sm['final_state'].data, 'last_data') + self.assertEqual(outcome, 'done') + if __name__ == '__main__': rospy.init_node('test_flexbe_core') diff --git a/flexbe_core/test/test_exceptions.py b/flexbe_core/test/test_exceptions.py index cdb6e985..34026d5f 100755 --- a/flexbe_core/test/test_exceptions.py +++ b/flexbe_core/test/test_exceptions.py @@ -99,6 +99,25 @@ def execute(self, userdata): self.assertIsNone(outcome) self.assertIsInstance(sm._last_exception, UserDataError) + def test_modify_input_key(self): + class ModifyInputKey(EventState): + + def __init__(self): + super(ModifyInputKey, self).__init__(outcomes=['done'], input_keys=['only_input']) + + def execute(self, userdata): + userdata.only_input['new'] = 'not_allowed' + return 'done' + + sm = OperatableStateMachine(outcomes=['done']) + sm.userdata.only_input = {'existing': 'is_allowed'} + with sm: + OperatableStateMachine.add('state', ModifyInputKey(), transitions={'done': 'done'}) + + outcome = sm.execute(None) + self.assertIsNone(outcome) + self.assertIsInstance(sm._last_exception, UserDataError) + if __name__ == '__main__': rospy.init_node('test_flexbe_exceptions') From cba554eb413d6dab71ca1819a2e67f45f1c283af Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Fri, 10 Apr 2020 21:04:48 +0200 Subject: [PATCH 26/57] [flexbe_testing] Fix check of userdata output --- flexbe_testing/src/flexbe_testing/tester.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/flexbe_testing/src/flexbe_testing/tester.py b/flexbe_testing/src/flexbe_testing/tester.py index 0ae6d967..ae15d192 100644 --- a/flexbe_testing/src/flexbe_testing/tester.py +++ b/flexbe_testing/src/flexbe_testing/tester.py @@ -1,5 +1,4 @@ #!/usr/bin/env python -import rospy import rosunit import unittest import re @@ -104,7 +103,7 @@ def run_test(self, name, config): # evaluate output output_ok = True for expected_key, expected_value in list(expected.items()): - if expected_key in list(userdata.keys()): + if expected_key in userdata: equals = userdata[expected_key] == expected_value self._tests['test_%s_output_%s' % (name, expected_key)] = \ self._test_output(userdata[expected_key], expected_value) From 9acc3c859822290b29249abda32a2a7a59b1bbb4 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Fri, 17 Apr 2020 11:37:36 +0200 Subject: [PATCH 27/57] [flexbe_core] Use more explicit userdata update operations --- .../src/flexbe_core/core/state_machine.py | 3 ++- flexbe_core/src/flexbe_core/core/user_data.py | 17 ++++++++--------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/flexbe_core/src/flexbe_core/core/state_machine.py b/flexbe_core/src/flexbe_core/core/state_machine.py index 52b24bf5..23b2ee39 100644 --- a/flexbe_core/src/flexbe_core/core/state_machine.py +++ b/flexbe_core/src/flexbe_core/core/state_machine.py @@ -75,7 +75,8 @@ def execute(self, userdata): if self._current_state is None: self.assert_consistent_transitions() self._current_state = self.initial_state - self._userdata = (userdata or UserData()) + self._own_userdata + self._userdata = userdata or UserData() + self._userdata(add_from=self._own_userdata) outcome = self._execute_current_state() return outcome diff --git a/flexbe_core/src/flexbe_core/core/user_data.py b/flexbe_core/src/flexbe_core/core/user_data.py index 4d49c5ae..5df87c10 100644 --- a/flexbe_core/src/flexbe_core/core/user_data.py +++ b/flexbe_core/src/flexbe_core/core/user_data.py @@ -58,16 +58,15 @@ def __setattr__(self, key, value): raise UserDataError("Key '%s' cannot be set, declare it as output key for write access." % key) self[key] = value - def __call__(self, reference=None): + def __call__(self, reference=None, add_from=None, update_from=None): self._reference = reference or self._reference - return self - - def __add__(self, other): - if not isinstance(other, UserData): - raise UserDataError("Can only add other userdata, not %s" % str(other)) - for key, value in other._data.items(): - self._data[key] = value - return self + if isinstance(add_from, UserData): + for key, value in add_from._data.items(): + if key not in self._data: + self._data[key] = value + if isinstance(update_from, UserData): + for key, value in update_from._data.items(): + self._data[key] = value def __len__(self): return len(self._data) + len(self._reference) From 46f9b8101ecfea788548a6b8420ada3b4aeef474 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Fri, 17 Apr 2020 11:37:59 +0200 Subject: [PATCH 28/57] [flexbe_testing] Add a behavior to the self-test --- .../flexbe_testing/test/selftest_behavior.xml | 27 +++++++ .../test/selftest_behavior_sm.py | 81 +++++++++++++++++++ .../src/flexbe_testing/test_interface.py | 1 - flexbe_testing/test/res/behavior.test | 13 +++ flexbe_testing/test/selftest.launch | 1 + flexbe_testing/test/selftest.test | 1 + 6 files changed, 123 insertions(+), 1 deletion(-) create mode 100644 flexbe_testing/src/flexbe_testing/test/selftest_behavior.xml create mode 100644 flexbe_testing/src/flexbe_testing/test/selftest_behavior_sm.py create mode 100644 flexbe_testing/test/res/behavior.test diff --git a/flexbe_testing/src/flexbe_testing/test/selftest_behavior.xml b/flexbe_testing/src/flexbe_testing/test/selftest_behavior.xml new file mode 100644 index 00000000..2e8d98eb --- /dev/null +++ b/flexbe_testing/src/flexbe_testing/test/selftest_behavior.xml @@ -0,0 +1,27 @@ + + + + + + test + Philipp Schillinger + Fri Apr 17 2020 + + Simple behavior for the flexbe_testing self-test of behaviors. + + + + + + + + + + + + + \ No newline at end of file diff --git a/flexbe_testing/src/flexbe_testing/test/selftest_behavior_sm.py b/flexbe_testing/src/flexbe_testing/test/selftest_behavior_sm.py new file mode 100644 index 00000000..dd56e544 --- /dev/null +++ b/flexbe_testing/src/flexbe_testing/test/selftest_behavior_sm.py @@ -0,0 +1,81 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +########################################################### +# WARNING: Generated code! # +# ************************** # +# Manual changes may get lost if file is generated again. # +# Only code inside the [MANUAL] tags will be kept. # +########################################################### + +from flexbe_core import Behavior, Autonomy, OperatableStateMachine, ConcurrencyContainer, PriorityContainer, Logger +from flexbe_states.calculation_state import CalculationState +from flexbe_states.decision_state import DecisionState +# Additional imports can be added inside the following tags +# [MANUAL_IMPORT] + +# [/MANUAL_IMPORT] + + +''' +Created on Fri Apr 17 2020 +@author: Philipp Schillinger +''' +class SelftestBehaviorSM(Behavior): + ''' + Simple behavior for the flexbe_testing self-test of behaviors. + ''' + + + def __init__(self): + super(SelftestBehaviorSM, self).__init__() + self.name = 'Selftest Behavior' + + # parameters of this behavior + self.add_parameter('value', 'wrong') + + # references to used behaviors + + # Additional initialization code can be added inside the following tags + # [MANUAL_INIT] + + # [/MANUAL_INIT] + + # Behavior comments: + + + + def create(self): + # x:30 y:365, x:130 y:365 + _state_machine = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['data'], output_keys=['result']) + _state_machine.userdata.data = None + _state_machine.userdata.result = None + + # Additional creation code can be added inside the following tags + # [MANUAL_CREATE] + + # [/MANUAL_CREATE] + + + with _state_machine: + # x:40 y:73 + OperatableStateMachine.add('Modify Data', + CalculationState(calculation=lambda x: x * 2), + transitions={'done': 'Decide Param'}, + autonomy={'done': Autonomy.Off}, + remapping={'input_value': 'data', 'output_value': 'result'}) + + # x:37 y:201 + OperatableStateMachine.add('Decide Param', + DecisionState(outcomes=['finished', 'failed'], conditions=lambda x: 'finished' if self.value == 'correct' else 'failed'), + transitions={'finished': 'finished', 'failed': 'failed'}, + autonomy={'finished': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'input_value': 'data'}) + + + return _state_machine + + + # Private functions can be added inside the following tags + # [MANUAL_FUNC] + + # [/MANUAL_FUNC] diff --git a/flexbe_testing/src/flexbe_testing/test_interface.py b/flexbe_testing/src/flexbe_testing/test_interface.py index c77597e6..78331aaa 100644 --- a/flexbe_testing/src/flexbe_testing/test_interface.py +++ b/flexbe_testing/src/flexbe_testing/test_interface.py @@ -79,5 +79,4 @@ def _execute_behavior(self, userdata, spin_cb): outcome = sm.execute(userdata) sm.sleep() spin_cb() - userdata.update(sm.userdata) return outcome diff --git a/flexbe_testing/test/res/behavior.test b/flexbe_testing/test/res/behavior.test new file mode 100644 index 00000000..edfe7c74 --- /dev/null +++ b/flexbe_testing/test/res/behavior.test @@ -0,0 +1,13 @@ +path: flexbe_testing.test.selftest_behavior_sm +class: SelftestBehaviorSM + +params: + value: correct + +input: + data: 3 + +output: + result: 6 + +outcome: finished diff --git a/flexbe_testing/test/selftest.launch b/flexbe_testing/test/selftest.launch index ffb97933..a675a206 100644 --- a/flexbe_testing/test/selftest.launch +++ b/flexbe_testing/test/selftest.launch @@ -13,6 +13,7 @@ $(arg path)/test_add.test $(arg path)/test_add_bagfile.test $(arg path)/sub_unavailable.test + $(arg path)/behavior.test " /> diff --git a/flexbe_testing/test/selftest.test b/flexbe_testing/test/selftest.test index d916cc04..0a2b5772 100644 --- a/flexbe_testing/test/selftest.test +++ b/flexbe_testing/test/selftest.test @@ -13,6 +13,7 @@ $(arg path)/test_add.test $(arg path)/test_add_bagfile.test $(arg path)/sub_unavailable.test + $(arg path)/behavior.test " /> From dd5fc2d6c01459bd1e06e05eb861fd209e839a96 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Fri, 17 Apr 2020 13:26:50 +0200 Subject: [PATCH 29/57] [flexbe_core] Un-import behavior-specific imports after execution --- .../src/flexbe_onboard/flexbe_onboard.py | 30 +++++++++++++++---- 1 file changed, 24 insertions(+), 6 deletions(-) diff --git a/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py b/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py index 90fe6a1e..74290e70 100644 --- a/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py +++ b/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py @@ -8,6 +8,7 @@ import time import smach import zlib +import contextlib from ast import literal_eval as cast from flexbe_core import Logger, BehaviorLibrary @@ -25,6 +26,7 @@ class FlexbeOnboard(object): def __init__(self): self.be = None Logger.initialize() + self._tracked_imports = list() # hide SMACH transition log spamming smach.set_loggers(rospy.logdebug, rospy.logwarn, rospy.logdebug, rospy.logerr) @@ -146,6 +148,8 @@ def _behavior_execution(self, msg): # done, remove left-overs like the temporary behavior file try: + if not self._switching: + self._clear_imports() self._cleanup_behavior(msg.behavior_checksum) except Exception as e: rospy.logerr('Failed to clean up behavior:\n%s' % str(e)) @@ -215,11 +219,12 @@ def _prepare_behavior(self, msg): # import temp class file and initialize behavior try: - package = __import__("tmp_%d" % msg.behavior_checksum, fromlist=["tmp_%d" % msg.behavior_checksum]) - clsmembers = inspect.getmembers(package, lambda member: (inspect.isclass(member) and - member.__module__ == package.__name__)) - beclass = clsmembers[0][1] - be = beclass() + with self._track_imports(): + package = __import__("tmp_%d" % msg.behavior_checksum, fromlist=["tmp_%d" % msg.behavior_checksum]) + clsmembers = inspect.getmembers(package, lambda member: (inspect.isclass(member) and + member.__module__ == package.__name__)) + beclass = clsmembers[0][1] + be = beclass() rospy.loginfo('Behavior ' + be.name + ' created.') except Exception as e: Logger.logerr('Exception caught in behavior definition:\n%s' % str(e)) @@ -275,7 +280,6 @@ def _is_switchable(self, be): return True def _cleanup_behavior(self, behavior_checksum): - del(sys.modules["tmp_%d" % behavior_checksum]) file_path = os.path.join(self._tmp_folder, 'tmp_%d.pyc' % behavior_checksum) try: os.remove(file_path) @@ -286,6 +290,12 @@ def _cleanup_behavior(self, behavior_checksum): except OSError: pass + def _clear_imports(self): + for module in self._tracked_imports: + if module in sys.modules: + del sys.modules[module] + self._tracked_imports = list() + def _cleanup_tempdir(self): try: os.remove(self._tmp_folder) @@ -329,3 +339,11 @@ def _convert_dict(self, o): class _attr_dict(dict): __getattr__ = dict.__getitem__ + + @contextlib.contextmanager + def _track_imports(self): + previous_modules = set(sys.modules.keys()) + try: + yield + finally: + self._tracked_imports.extend(set(sys.modules.keys()) - previous_modules) From 412ce627050b7b1060dd389852702f9d9ed66965 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Fri, 17 Apr 2020 13:53:01 +0200 Subject: [PATCH 30/57] [flexbe_core] Consider a running mirror as being controlled (see #123) --- flexbe_core/src/flexbe_core/core/operatable_state_machine.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/flexbe_core/src/flexbe_core/core/operatable_state_machine.py b/flexbe_core/src/flexbe_core/core/operatable_state_machine.py index b73b1de6..80501ac7 100644 --- a/flexbe_core/src/flexbe_core/core/operatable_state_machine.py +++ b/flexbe_core/src/flexbe_core/core/operatable_state_machine.py @@ -273,6 +273,8 @@ def _mirror_structure_callback(self, msg): msg.behavior_id = self.id self._pub.publish('flexbe/mirror/structure', msg) rospy.loginfo("<-- Sent behavior structure for mirror.") + # enable control of states since a mirror is listening + self._enable_ros_control() def _transition_allowed(self, label, outcome): From 0838aae2278730ecc28beb7363e61e455eb8f44c Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Fri, 17 Apr 2020 15:26:06 +0200 Subject: [PATCH 31/57] [flexbe_mirror] Minor cleanup of mirror --- flexbe_mirror/bin/behavior_mirror_sm | 8 +-- .../src/flexbe_mirror/flexbe_mirror.py | 65 +++++++------------ .../src/flexbe_mirror/mirror_state.py | 6 -- 3 files changed, 25 insertions(+), 54 deletions(-) diff --git a/flexbe_mirror/bin/behavior_mirror_sm b/flexbe_mirror/bin/behavior_mirror_sm index 7b05abb9..b5ec3230 100755 --- a/flexbe_mirror/bin/behavior_mirror_sm +++ b/flexbe_mirror/bin/behavior_mirror_sm @@ -1,16 +1,14 @@ #!/usr/bin/env python - -import roslib; roslib.load_manifest('flexbe_mirror') import rospy from flexbe_core.proxy import ProxySubscriberCached -from flexbe_mirror.flexbe_mirror import VigirBehaviorMirror +from flexbe_mirror.flexbe_mirror import FlexbeMirror if __name__ == '__main__': rospy.init_node('flexbe_mirror') - - VigirBehaviorMirror() + + FlexbeMirror() # Wait for ctrl-c to stop the application rospy.spin() diff --git a/flexbe_mirror/src/flexbe_mirror/flexbe_mirror.py b/flexbe_mirror/src/flexbe_mirror/flexbe_mirror.py index 4e269026..a03fcf32 100644 --- a/flexbe_mirror/src/flexbe_mirror/flexbe_mirror.py +++ b/flexbe_mirror/src/flexbe_mirror/flexbe_mirror.py @@ -10,21 +10,10 @@ from flexbe_msgs.msg import ContainerStructure, BehaviorSync, BEStatus from std_msgs.msg import Empty, String, Int32, UInt8 -''' -Created on 08.05.2013 - -@author: Philipp Schillinger -''' -class VigirBehaviorMirror(object): - ''' - classdocs - ''' +class FlexbeMirror(object): def __init__(self): - ''' - Constructor - ''' self._sm = None # set up proxys for sm <--> GUI communication @@ -37,12 +26,11 @@ def __init__(self): self._active_id = 0 self._starting_path = None self._current_struct = None + self._struct_buffer = list() self._sync_lock = threading.Lock() self._outcome_topic = 'flexbe/mirror/outcome' - self._struct_buffer = list() - # listen for mirror message self._sub = ProxySubscriberCached() self._sub.subscribe(self._outcome_topic, UInt8) @@ -52,17 +40,18 @@ def __init__(self): self._sub.subscribe('flexbe/status', BEStatus, self._status_callback) self._sub.subscribe('flexbe/mirror/sync', BehaviorSync, self._sync_callback) self._sub.subscribe('flexbe/mirror/preempt', Empty, self._preempt_callback) - - + def _mirror_callback(self, msg): rate = rospy.Rate(10) while self._stopping: rate.sleep() - + if self._running: - rospy.logwarn('Received a new mirror structure while mirror is already running, adding to buffer (checksum: %s).' % str(msg.behavior_id)) + rospy.logwarn('Received a new mirror structure while mirror is already running, ' + 'adding to buffer (checksum: %s).' % str(msg.behavior_id)) elif self._active_id != 0 and msg.behavior_id != self._active_id: - rospy.logwarn('checksum of received mirror structure (%s) does not match expected checksum (%s), will ignore.' % (str(msg.behavior_id), str(self._active_id))) + rospy.logwarn('Checksum of received mirror structure (%s) does not match expected (%s), ' + 'will ignore.' % (str(msg.behavior_id), str(self._active_id))) return else: rospy.loginfo('Received a new mirror structure for checksum %s' % str(msg.behavior_id)) @@ -76,7 +65,6 @@ def _mirror_callback(self, msg): self._execute_mirror() - def _status_callback(self, msg): if msg.code == BEStatus.STARTED: thread = threading.Thread(target=self._start_mirror, args=[msg]) @@ -87,7 +75,6 @@ def _status_callback(self, msg): thread.daemon = True thread.start() - def _start_mirror(self, msg): with self._sync_lock: rate = rospy.Rate(10) @@ -114,14 +101,13 @@ def _start_mirror(self, msg): if self._sm is None: rospy.logwarn('Missing correct mirror structure, requesting...') - rospy.sleep(0.2) # no clean wayacquire to wait for publisher to be ready... + rospy.sleep(0.2) # no clean wayacquire to wait for publisher to be ready... self._pub.publish('flexbe/request_mirror_structure', Int32(msg.behavior_id)) self._active_id = msg.behavior_id return self._execute_mirror() - def _stop_mirror(self, msg): with self._sync_lock: self._stopping = True @@ -156,7 +142,6 @@ def _stop_mirror(self, msg): self._stopping = False - def _sync_callback(self, msg): if msg.behavior_id == self._active_id: thread = threading.Thread(target=self._restart_mirror, args=[msg]) @@ -168,7 +153,6 @@ def _sync_callback(self, msg): thread.daemon = True thread.start() - def _restart_mirror(self, msg): with self._sync_lock: rospy.loginfo('Restarting mirror for synchronization...') @@ -190,7 +174,6 @@ def _restart_mirror(self, msg): rospy.loginfo('Stopping synchronization because behavior has stopped.') self._execute_mirror() - def _execute_mirror(self): self._running = True @@ -211,8 +194,7 @@ def _execute_mirror(self): self._running = False rospy.loginfo('Mirror finished with result ' + result) - - + def _mirror_state_machine(self, msg): self._current_struct = msg self._state_checksums = dict() @@ -226,47 +208,44 @@ def _mirror_state_machine(self, msg): for con_msg in msg.containers: if con_msg.path.find('/') != -1: self._state_checksums[zlib.adler32(con_msg.path)] = con_msg.path - - + def _add_node(self, msg, path): - #rospy.loginfo('Add node: '+path) container = None for con_msg in msg.containers: if con_msg.path == path: container = con_msg break - + transitions = None if container.transitions is not None: transitions = {} for i in range(len(container.transitions)): transitions[container.outcomes[i]] = container.transitions[i] + '_mirror' - + path_frags = path.split('/') container_name = path_frags[len(path_frags)-1] if len(container.children) > 0: sm_outcomes = [] - for outcome in container.outcomes: sm_outcomes.append(outcome + '_mirror') + for outcome in container.outcomes: + sm_outcomes.append(outcome + '_mirror') sm = PreemptableStateMachine(outcomes=sm_outcomes) with sm: - for child in container.children: + for child in container.children: self._add_node(msg, path+'/'+child) - if len(transitions) > 0: + if len(transitions) > 0: container_transitions = {} - for i in range(len(container.transitions)): container_transitions[sm_outcomes[i]] = transitions[container.outcomes[i]] + for i in range(len(container.transitions)): + container_transitions[sm_outcomes[i]] = transitions[container.outcomes[i]] PreemptableStateMachine.add(container_name + '_mirror', sm, transitions=container_transitions) else: self._sm = sm else: - PreemptableStateMachine.add(container_name + '_mirror', MirrorState(container_name, path, container.outcomes, container.autonomy), transitions=transitions) - + PreemptableStateMachine.add(container_name + '_mirror', + MirrorState(container_name, path, container.outcomes, container.autonomy), + transitions=transitions) def _preempt_callback(self, msg): if self._sm is not None: rospy.logwarn('Explicit preempting is currently ignored, mirror should be preempted by onboard behavior.') else: rospy.logwarn('Could not preempt mirror because it seems not to be running!') - - - - diff --git a/flexbe_mirror/src/flexbe_mirror/mirror_state.py b/flexbe_mirror/src/flexbe_mirror/mirror_state.py index bd1f12c0..6d5b111f 100644 --- a/flexbe_mirror/src/flexbe_mirror/mirror_state.py +++ b/flexbe_mirror/src/flexbe_mirror/mirror_state.py @@ -14,9 +14,6 @@ class MirrorState(EventState): ''' def __init__(self, target_name, target_path, given_outcomes, outcome_autonomy): - ''' - Constructor - ''' super(MirrorState, self).__init__(outcomes=given_outcomes) self.set_rate(100) self._target_name = target_name @@ -28,9 +25,6 @@ def __init__(self, target_name, target_path, given_outcomes, outcome_autonomy): self._sub = ProxySubscriberCached({self._outcome_topic: UInt8}) def execute(self, userdata): - ''' - Execute this state - ''' if self._sub.has_buffered(self._outcome_topic): msg = self._sub.get_from_buffer(self._outcome_topic) if msg.data < len(self.outcomes): From d0a3c7c884bfa5c5a4d6f2cc513996d0c682e9d9 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Fri, 17 Apr 2020 15:26:27 +0200 Subject: [PATCH 32/57] [flexbe_states] Cleanup of states --- .../src/flexbe_states/calculation_state.py | 91 +++++------- .../flexbe_states/check_condition_state.py | 69 ++++----- .../src/flexbe_states/decision_state.py | 82 ++++------- .../flexible_calculation_state.py | 91 +++++------- .../flexible_check_condition_state.py | 86 +++++------ .../src/flexbe_states/input_state.py | 12 -- .../src/flexbe_states/log_key_state.py | 64 ++++----- flexbe_states/src/flexbe_states/log_state.py | 49 +++---- .../flexbe_states/operator_decision_state.py | 67 ++++----- .../src/flexbe_states/publisher_bool_state.py | 25 +--- .../flexbe_states/publisher_empty_state.py | 25 +--- .../flexbe_states/publisher_string_state.py | 25 +--- .../src/flexbe_states/subscriber_state.py | 133 +++++++----------- flexbe_states/src/flexbe_states/wait_state.py | 48 ++----- 14 files changed, 319 insertions(+), 548 deletions(-) diff --git a/flexbe_states/src/flexbe_states/calculation_state.py b/flexbe_states/src/flexbe_states/calculation_state.py index 1b317c89..5bf16ea1 100644 --- a/flexbe_states/src/flexbe_states/calculation_state.py +++ b/flexbe_states/src/flexbe_states/calculation_state.py @@ -1,60 +1,41 @@ #!/usr/bin/env python - -import rospy from flexbe_core import EventState, Logger -''' -Created on 29.08.2013 - -@author: Philipp Schillinger -''' class CalculationState(EventState): - ''' - Implements a state that can perform a calculation based on userdata. - 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. - 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. - - #> output_value object The result of the calculation. - - <= done Indicates completion of the calculation. - - ''' - - - def __init__(self, calculation): - ''' - Constructor - ''' - super(CalculationState, self).__init__(outcomes=['done'], - input_keys=['input_value'], - output_keys=['output_value']) - - self._calculation = calculation - self._calculation_result = None - - - def execute(self, userdata): - '''Execute this state''' - - userdata.output_value = self._calculation_result - - # nothing to check - return 'done' - - - def on_enter(self, userdata): - - if self._calculation is not None: - try: - self._calculation_result = self._calculation(userdata.input_value) - except Exception as e: - Logger.logwarn('Failed to execute calculation function!\n%s' % str(e)) - else: - Logger.logwarn('Passed no calculation!') + ''' + Implements a state that can perform a calculation based on userdata. + 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. + 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. + + #> output_value object The result of the calculation. + + <= done Indicates completion of the calculation. + ''' + + def __init__(self, calculation): + super(CalculationState, self).__init__(outcomes=['done'], + input_keys=['input_value'], + output_keys=['output_value']) + self._calculation = calculation + self._calculation_result = None + + def execute(self, userdata): + userdata.output_value = self._calculation_result + # nothing to check + return 'done' + + def on_enter(self, userdata): + if self._calculation is not None: + try: + self._calculation_result = self._calculation(userdata.input_value) + except Exception as e: + Logger.logwarn('Failed to execute calculation function!\n%s' % str(e)) + else: + Logger.logwarn('Passed no calculation!') diff --git a/flexbe_states/src/flexbe_states/check_condition_state.py b/flexbe_states/src/flexbe_states/check_condition_state.py index 1a9ae465..da8735ec 100644 --- a/flexbe_states/src/flexbe_states/check_condition_state.py +++ b/flexbe_states/src/flexbe_states/check_condition_state.py @@ -1,47 +1,32 @@ #!/usr/bin/env python - -import rospy from flexbe_core import EventState, Logger -''' -Created on 02/24/2015 - -@author: Philipp Schillinger -''' class CheckConditionState(EventState): - ''' - Checks if the given condition is true and returns the corresponding outcome. - This state can be used if the further control flow of the behavior depends on a simple condition. - - -- predicate function The condition whose truth value will be evaluated. - Has to expect one parameter which will be set to input_value and return a boolean. - - ># input_value object Input to the predicate function. - - <= true Returned if the condition evaluates to True - <= false Returned if the condition evaluates to False - - ''' - - - def __init__(self, predicate): - '''Constructor''' - super(CheckConditionState, self).__init__(outcomes=['true', 'false'], - input_keys=['input_value']) - - self._predicate = predicate - self._outcome = 'false' - - - def execute(self, userdata): - '''Execute this state''' - - return self._outcome - - - def on_enter(self, userdata): - try: - self._outcome = 'true' if self._predicate(userdata.input_value) else 'false' - except Exception as e: - Logger.logwarn('Failed to execute condition function!\n%s' % str(e)) + ''' + Checks if the given condition is true and returns the corresponding outcome. + This state can be used if the further control flow of the behavior depends on a simple condition. + + -- predicate function The condition whose truth value will be evaluated. + Has to expect one parameter which will be set to input_value and return a boolean. + + ># input_value object Input to the predicate function. + + <= true Returned if the condition evaluates to True + <= false Returned if the condition evaluates to False + ''' + + def __init__(self, predicate): + super(CheckConditionState, self).__init__(outcomes=['true', 'false'], + input_keys=['input_value']) + self._predicate = predicate + self._outcome = 'false' + + def execute(self, userdata): + return self._outcome + + def on_enter(self, userdata): + try: + self._outcome = 'true' if self._predicate(userdata.input_value) else 'false' + except Exception as e: + Logger.logwarn('Failed to execute condition function!\n%s' % str(e)) diff --git a/flexbe_states/src/flexbe_states/decision_state.py b/flexbe_states/src/flexbe_states/decision_state.py index a7cbb2f2..5b1d6e82 100644 --- a/flexbe_states/src/flexbe_states/decision_state.py +++ b/flexbe_states/src/flexbe_states/decision_state.py @@ -1,58 +1,34 @@ #!/usr/bin/env python - -import rospy from flexbe_core import EventState, Logger -from rospy.exceptions import ROSInterruptException - -''' -Created on 11.06.2013 -@author: Philipp Schillinger -''' class DecisionState(EventState): - ''' - Evaluates a condition function in order to return one of the specified outcomes. - 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. - Has to expect one parameter which will be set to input_value. - - ># input_value object Input to the condition function. - - ''' - - - def __init__(self, outcomes, conditions): - ''' - Constructor - ''' - super(DecisionState, self).__init__(outcomes=outcomes, - input_keys=['input_value']) - - self._conditions = conditions - self._my_outcomes = outcomes - - - def execute(self, userdata): - ''' - Execute this state - ''' - - if self._conditions is not None: - outcome = None - try: - outcome = str(self._conditions(userdata.input_value)) - except Exception as e: - Logger.logwarn('Passed no function as predicate!\n%s' % str(e)) - outcome = None - if outcome is not None: - return outcome - - - def on_enter(self, userdata): - try: - rospy.sleep(0.2) # TODO: check why this has been added - except ROSInterruptException: - rospy.logwarn('Skipped sleep.') + ''' + Evaluates a condition function in order to return one of the specified outcomes. + 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. + Has to expect one parameter which will be set to input_value. + + ># input_value object Input to the condition function. + ''' + + def __init__(self, outcomes, conditions): + ''' + Constructor + ''' + super(DecisionState, self).__init__(outcomes=outcomes, + input_keys=['input_value']) + self._conditions = conditions + + def execute(self, userdata): + if self._conditions is not None: + outcome = None + try: + outcome = str(self._conditions(userdata.input_value)) + except Exception as e: + Logger.logwarn('Passed no function as predicate!\n%s' % str(e)) + outcome = None + if outcome is not None and outcome in self._outcomes: + return outcome diff --git a/flexbe_states/src/flexbe_states/flexible_calculation_state.py b/flexbe_states/src/flexbe_states/flexible_calculation_state.py index 524de066..aa33d1a1 100644 --- a/flexbe_states/src/flexbe_states/flexible_calculation_state.py +++ b/flexbe_states/src/flexbe_states/flexible_calculation_state.py @@ -1,59 +1,42 @@ #!/usr/bin/env python - -import rospy from flexbe_core import EventState, Logger -''' -Created on 29.08.2013 - -@author: Philipp Schillinger -''' class FlexibleCalculationState(EventState): - ''' - Implements a state that can perform a calculation based on multiple userdata inputs provided as a list to the calculation function. - - -- 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[0]^2 + x[1]^2). - -- input_keys string[] List of available input keys. - - ># input_keys object[] Input(s) to the calculation function as a list of userdata. - The individual inputs can be accessed as list elements (see lambda expression example). - - #> output_value object The result of the calculation. - - <= done Indicates completion of the calculation. - - ''' - - - def __init__(self, calculation, input_keys): - '''Constructor''' - super(FlexibleCalculationState, self).__init__(outcomes=['done'], - input_keys=input_keys, - output_keys=['output_value']) - - self._calculation = calculation - self._calculation_result = None - self._input_keys = input_keys - - - def execute(self, userdata): - '''Execute this state''' - - userdata.output_value = self._calculation_result - - # nothing to check - return 'done' - - - def on_enter(self, userdata): - - if self._calculation is not None: - try: - self._calculation_result = self._calculation([userdata[key] for key in self._input_keys]) - except Exception as e: - Logger.logwarn('Failed to execute calculation function!\n%s' % str(e)) - else: - Logger.logwarn('Passed no calculation!') + ''' + Implements a state that can perform a calculation based on multiple userdata inputs + provided as a list to the calculation function. + + -- 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[0]^2 + x[1]^2). + -- input_keys string[] List of available input keys. + + ># input_keys object[] Input(s) to the calculation function as a list of userdata. + The individual inputs can be accessed as list elements (see lambda expression example). + + #> output_value object The result of the calculation. + + <= done Indicates completion of the calculation. + ''' + + def __init__(self, calculation, input_keys): + super(FlexibleCalculationState, self).__init__(outcomes=['done'], + input_keys=input_keys, + output_keys=['output_value']) + self._calculation = calculation + self._calculation_result = None + + def execute(self, userdata): + userdata.output_value = self._calculation_result + # nothing to check + return 'done' + + def on_enter(self, userdata): + if self._calculation is not None: + try: + self._calculation_result = self._calculation([userdata[key] for key in self._input_keys]) + except Exception as e: + Logger.logwarn('Failed to execute calculation function!\n%s' % str(e)) + else: + Logger.logwarn('Passed no calculation!') diff --git a/flexbe_states/src/flexbe_states/flexible_check_condition_state.py b/flexbe_states/src/flexbe_states/flexible_check_condition_state.py index eb010b55..0e5c1b79 100644 --- a/flexbe_states/src/flexbe_states/flexible_check_condition_state.py +++ b/flexbe_states/src/flexbe_states/flexible_check_condition_state.py @@ -1,56 +1,40 @@ #!/usr/bin/env python - -import rospy from flexbe_core import EventState, Logger -''' -Created on 08.11.2016 - -@author: Alberto Romay -''' class FlexibleCheckConditionState(EventState): - ''' - Implements a state that checks if the given condition is true based on multiple userdata inputs provided as a list to the calculation function - and returns the corresponding outcome. - This state can be used if the further control flow of the behavior depends on a simple condition. - - -- predicate function The condition whose truth value will be evaluated. - 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[0]^2 + x[1]^2). - -- input_keys string[] List of available input keys. - - ># input_keys object[] Input(s) to the calculation function as a list of userdata. - The individual inputs can be accessed as list elements (see lambda expression example). - - <= true Returned if the condition evaluates to True - <= false Returned if the condition evaluates to False - - ''' - - - def __init__(self, predicate, input_keys): - '''Constructor''' - super(FlexibleCheckConditionState, self).__init__(outcomes=['true', 'false'], - input_keys=input_keys) - - self._input_keys = input_keys - self._predicate = predicate - self._outcome = 'false' - - - def execute(self, userdata): - '''Execute this state''' - - return self._outcome - - - def on_enter(self, userdata): - - if self._predicate is not None: - try: - self._outcome = "true" if self._predicate([userdata[key] for key in self._input_keys]) else 'false' - except Exception as e: - Logger.logwarn('Failed to execute condition function!\n%s' % str(e)) - else: - Logger.logwarn('Passed no predicate!') + ''' + Implements a state that checks if the given condition is true based on multiple userdata inputs + provided as a list to the calculation function and returns the corresponding outcome. + This state can be used if the further control flow of the behavior depends on a simple condition. + + -- predicate function The condition whose truth value will be evaluated. + 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[0]^2 + x[1]^2). + -- input_keys string[] List of available input keys. + + ># input_keys object[] Input(s) to the calculation function as a list of userdata. + The individual inputs can be accessed as list elements (see lambda expression example). + + <= true Returned if the condition evaluates to True + <= false Returned if the condition evaluates to False + ''' + + def __init__(self, predicate, input_keys): + '''Constructor''' + super(FlexibleCheckConditionState, self).__init__(outcomes=['true', 'false'], + input_keys=input_keys) + self._predicate = predicate + self._outcome = 'false' + + def execute(self, userdata): + return self._outcome + + def on_enter(self, userdata): + if self._predicate is not None: + try: + self._outcome = "true" if self._predicate([userdata[key] for key in self._input_keys]) else 'false' + except Exception as e: + Logger.logwarn('Failed to execute condition function!\n%s' % str(e)) + else: + Logger.logwarn('Passed no predicate!') diff --git a/flexbe_states/src/flexbe_states/input_state.py b/flexbe_states/src/flexbe_states/input_state.py index 75d8806d..b92f72a3 100644 --- a/flexbe_states/src/flexbe_states/input_state.py +++ b/flexbe_states/src/flexbe_states/input_state.py @@ -1,18 +1,10 @@ #!/usr/bin/env python - -import rospy -import actionlib import pickle from flexbe_core import EventState, Logger from flexbe_msgs.msg import BehaviorInputAction, BehaviorInputGoal, BehaviorInputResult from flexbe_core.proxy import ProxyActionClient -''' -Created on 02/13/2015 - -@author: Philipp Schillinger -''' class InputState(EventState): ''' @@ -36,7 +28,6 @@ def __init__(self, request, message): ''' super(InputState, self).__init__(outcomes=['received', 'aborted', 'no_connection', 'data_error'], output_keys=['data']) - self._action_topic = 'flexbe/behavior_input' self._client = ProxyActionClient({self._action_topic: BehaviorInputAction}) @@ -46,9 +37,6 @@ def __init__(self, request, message): self._received = False def execute(self, userdata): - ''' - Execute this state - ''' if not self._connected: return 'no_connection' if self._received: diff --git a/flexbe_states/src/flexbe_states/log_key_state.py b/flexbe_states/src/flexbe_states/log_key_state.py index 0b924ad0..d9ad672d 100644 --- a/flexbe_states/src/flexbe_states/log_key_state.py +++ b/flexbe_states/src/flexbe_states/log_key_state.py @@ -1,44 +1,30 @@ #!/usr/bin/env python - from flexbe_core import EventState, Logger -''' -Created on 09.12.2013 - -@author: Philipp Schillinger -''' class LogKeyState(EventState): - ''' - A state that can log a predefined message including an input key 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) - - #> 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. - - ''' - - def __init__(self, text, severity = Logger.REPORT_HINT): - '''Constructor''' - super(LogKeyState, self).__init__( - outcomes=['done'], - input_keys=['data']) - - self._text = text - self._severity = severity - - - def execute(self, userdata): - '''Execute this state''' - - # Already logged. No need to wait for anything. - return 'done' - - - def on_enter(self, userdata): - '''Log upon entering the state.''' - - Logger.log(self._text.format(userdata.data), self._severity) + ''' + A state that can log a predefined message including an input key + 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) + + #> 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. + ''' + + def __init__(self, text, severity=Logger.REPORT_HINT): + super(LogKeyState, self).__init__(outcomes=['done'], + input_keys=['data']) + self._text = text + self._severity = severity + + def execute(self, userdata): + # Already logged. No need to wait for anything. + return 'done' + + def on_enter(self, userdata): + '''Log upon entering the state.''' + Logger.log(self._text.format(userdata.data), self._severity) diff --git a/flexbe_states/src/flexbe_states/log_state.py b/flexbe_states/src/flexbe_states/log_state.py index ced30c84..ec457807 100644 --- a/flexbe_states/src/flexbe_states/log_state.py +++ b/flexbe_states/src/flexbe_states/log_state.py @@ -1,40 +1,27 @@ #!/usr/bin/env python - from flexbe_core import EventState, Logger -''' -Created on 09.12.2013 - -@author: Philipp Schillinger -''' class LogState(EventState): - ''' - A state that can log a predefined message 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) + ''' + A state that can log a predefined message 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) - <= done Indicates that the message has been logged. - - ''' - - def __init__(self, text, severity = Logger.REPORT_HINT): - '''Constructor''' - super(LogState, self).__init__(outcomes=['done']) - - self._text = text - self._severity = severity + <= done Indicates that the message has been logged. + ''' - - def execute(self, userdata): - '''Execute this state''' - - # Already logged. No need to wait for anything. - return 'done' + def __init__(self, text, severity=Logger.REPORT_HINT): + super(LogState, self).__init__(outcomes=['done']) + self._text = text + self._severity = severity - - def on_enter(self, userdata): - '''Log upon entering the state.''' + def execute(self, userdata): + # Already logged. No need to wait for anything. + return 'done' - Logger.log(self._text, self._severity) + def on_enter(self, userdata): + '''Log upon entering the state.''' + Logger.log(self._text, self._severity) diff --git a/flexbe_states/src/flexbe_states/operator_decision_state.py b/flexbe_states/src/flexbe_states/operator_decision_state.py index 6b3fc438..b94de6bc 100644 --- a/flexbe_states/src/flexbe_states/operator_decision_state.py +++ b/flexbe_states/src/flexbe_states/operator_decision_state.py @@ -1,46 +1,31 @@ #!/usr/bin/env python - from flexbe_core import EventState, Logger -''' -Created on 12.12.2013 - -@author: Philipp Schillinger -''' class OperatorDecisionState(EventState): - ''' - Implements a state where the operator has to manually choose an outcome. - Autonomy Level of all outcomes should be set to Full, because this state is not able to choose an outcome on its own. - Only exception is the suggested outcome, which will be returned immediately by default. - This state can be used to create alternative execution paths by setting the suggestion to High autonomy instead of Full. - - -- outcomes string[] A list of all possible outcomes of this state. - -- hint string Text displayed to the operator to give instructions how to decide. - -- suggestion string The outcome which is suggested. Will be returned if the level of autonomy is high enough. - - ''' - - - def __init__(self, outcomes, hint=None, suggestion=None): - ''' - Constructor - ''' - super(OperatorDecisionState, self).__init__(outcomes=outcomes) - - self._hint = hint - self._suggestion = suggestion - self._my_outcomes = outcomes - - - def execute(self, userdata): - ''' - Execute this state - ''' - if self._suggestion is not None and self._my_outcomes.count(self._suggestion) > 0: - return self._suggestion - - - def on_enter(self, userdata): - if self._hint is not None: - Logger.loghint(self._hint) + ''' + Implements a state where the operator has to manually choose an outcome. + Autonomy Level of all outcomes should be set to Full, + because this state is not able to choose an outcome on its own. + Only exception is the suggested outcome, which will be returned immediately by default. + This state can be used to create alternative execution paths + by setting the suggestion to High autonomy instead of Full. + + -- outcomes string[] A list of all possible outcomes of this state. + -- hint string Text displayed to the operator to give instructions how to decide. + -- suggestion string The outcome which is suggested. + Will be returned if the level of autonomy is high enough. + ''' + + def __init__(self, outcomes, hint=None, suggestion=None): + super(OperatorDecisionState, self).__init__(outcomes=outcomes) + self._hint = hint + self._suggestion = suggestion + + def execute(self, userdata): + if self._suggestion is not None and self._suggestion in self._outcomes: + return self._suggestion + + def on_enter(self, userdata): + if self._hint is not None: + Logger.loghint(self._hint) diff --git a/flexbe_states/src/flexbe_states/publisher_bool_state.py b/flexbe_states/src/flexbe_states/publisher_bool_state.py index d9853f72..0f8998c8 100644 --- a/flexbe_states/src/flexbe_states/publisher_bool_state.py +++ b/flexbe_states/src/flexbe_states/publisher_bool_state.py @@ -1,35 +1,22 @@ #!/usr/bin/env python - - -from flexbe_core import EventState, Logger +from flexbe_core import EventState from flexbe_core.proxy import ProxyPublisher from std_msgs.msg import Bool -''' -Created on 31.01.2017 - -@author: Alberto Romay -''' - class PublisherBoolState(EventState): ''' - Publishes an empty (std_msgs/Bool) message on a given topic name. + Publishes an empty (std_msgs/Bool) 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 bool. + >= value Value of bool. - <= done Done publishing. - - ''' + <= done Done publishing. + ''' def __init__(self, topic): - ''' - Constructor - ''' super(PublisherBoolState, self).__init__(outcomes=['done'], input_keys=['value']) - self._topic = topic self._pub = ProxyPublisher({self._topic: Bool}) diff --git a/flexbe_states/src/flexbe_states/publisher_empty_state.py b/flexbe_states/src/flexbe_states/publisher_empty_state.py index 707ae913..9eae2b20 100644 --- a/flexbe_states/src/flexbe_states/publisher_empty_state.py +++ b/flexbe_states/src/flexbe_states/publisher_empty_state.py @@ -1,33 +1,20 @@ #!/usr/bin/env python - - -from flexbe_core import EventState, Logger +from flexbe_core import EventState from flexbe_core.proxy import ProxyPublisher from std_msgs.msg import Empty -''' -Created on 31.01.2017 - -@author: Alberto Romay -''' - class PublisherEmptyState(EventState): ''' - Publishes an empty (std_msgs/Empty) message on a given topic name. + Publishes an empty (std_msgs/Empty) message on a given topic name. - -- topic string The topic on which should be published. + -- topic string The topic on which should be published. - <= done Done publishing. - - ''' + <= done Done publishing. + ''' def __init__(self, topic): - ''' - Constructor - ''' super(PublisherEmptyState, self).__init__(outcomes=['done']) - self._topic = topic self._pub = ProxyPublisher({self._topic: Empty}) @@ -35,4 +22,4 @@ def execute(self, userdata): return 'done' def on_enter(self, userdata): - self._pub.publish(self._topic, Empty()) \ No newline at end of file + self._pub.publish(self._topic, Empty()) diff --git a/flexbe_states/src/flexbe_states/publisher_string_state.py b/flexbe_states/src/flexbe_states/publisher_string_state.py index 9d55143f..0cfbc96f 100644 --- a/flexbe_states/src/flexbe_states/publisher_string_state.py +++ b/flexbe_states/src/flexbe_states/publisher_string_state.py @@ -1,35 +1,22 @@ #!/usr/bin/env python - - -from flexbe_core import EventState, Logger +from flexbe_core import EventState from flexbe_core.proxy import ProxyPublisher from std_msgs.msg import String -''' -Created on 9.11.2017 - -@author: Alireza Hosseini -''' - class PublisherStringState(EventState): ''' - Publishes a string (std_msgs/String) message on a given topic name. + 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): - ''' - Constructor - ''' super(PublisherStringState, self).__init__(outcomes=['done'], input_keys=['value']) - self._topic = topic self._pub = ProxyPublisher({self._topic: String}) diff --git a/flexbe_states/src/flexbe_states/subscriber_state.py b/flexbe_states/src/flexbe_states/subscriber_state.py index 8d99dc20..64718f91 100644 --- a/flexbe_states/src/flexbe_states/subscriber_state.py +++ b/flexbe_states/src/flexbe_states/subscriber_state.py @@ -1,88 +1,61 @@ #!/usr/bin/env python -import rospy import rostopic -import inspect from flexbe_core import EventState, Logger from flexbe_core.proxy import ProxySubscriberCached -''' -Created on 11.06.2013 - -@author: Philipp Schillinger -''' class SubscriberState(EventState): - ''' - Gets the latest message on the given topic and stores it to userdata. - - -- topic string The topic on which should be listened. - -- blocking bool Blocks until a message is received. - -- clear bool Drops last message on this topic on enter - in order to only handle message received since this state is active. - - #> message object Latest message on the given topic of the respective type. - - <= received Message has been received and stored in userdata or state is not blocking. - <= unavailable The topic is not available when this state becomes actives. - - ''' - - - def __init__(self, topic, blocking = True, clear = False): - ''' - Constructor - ''' - super(SubscriberState, self).__init__(outcomes=['received', 'unavailable'], - output_keys=['message']) - - self._topic = topic - self._blocking = blocking - self._clear = clear - self._connected = False - - (msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic) - - if msg_topic == self._topic: - msg_type = self._get_msg_from_path(msg_path) - self._sub = ProxySubscriberCached({self._topic: msg_type}) - self._connected = True - else: - Logger.logwarn('Topic %s for state %s not yet available.\nFound: %s\nWill try again when entering the state...' % (self._topic, self.name, str(msg_topic))) - - - def execute(self, userdata): - ''' - Execute this state - ''' - if not self._connected: - userdata.message = None - return 'unavailable' - - if self._sub.has_msg(self._topic) or not self._blocking: - userdata.message = self._sub.get_last_msg(self._topic) - self._sub.remove_last_msg(self._topic) - return 'received' - - - def on_enter(self, userdata): - if not self._connected: - (msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic) - if msg_topic == self._topic: - msg_type = self._get_msg_from_path(msg_path) - self._sub = ProxySubscriberCached({self._topic: msg_type}) - self._connected = True - Logger.loginfo('Successfully subscribed to previously unavailable topic %s' % self._topic) - else: - Logger.logwarn('Topic %s still not available, giving up.' % self._topic) - - if self._connected and self._clear and self._sub.has_msg(self._topic): - self._sub.remove_last_msg(self._topic) - - - def _get_msg_from_path(self, msg_path): - msg_import = msg_path.split('/') - msg_module = '%s.msg' % (msg_import[0]) - package = __import__(msg_module, fromlist=[msg_module]) - clsmembers = inspect.getmembers(package, lambda member: inspect.isclass(member) and member.__module__.endswith(msg_import[1])) - return clsmembers[0][1] + ''' + Gets the latest message on the given topic and stores it to userdata. + + -- topic string The topic on which should be listened. + -- blocking bool Blocks until a message is received. + -- clear bool Drops last message on this topic on enter + in order to only handle message received since this state is active. + + #> message object Latest message on the given topic of the respective type. + + <= received Message has been received and stored in userdata or state is not blocking. + <= unavailable The topic is not available when this state becomes actives. + ''' + + def __init__(self, topic, blocking=True, clear=False): + super(SubscriberState, self).__init__(outcomes=['received', 'unavailable'], + output_keys=['message']) + self._topic = topic + self._blocking = blocking + self._clear = clear + self._connected = False + + if not self._connect(): + Logger.logwarn('Topic %s for state %s not yet available.\n' + 'Will try again when entering the state...' % (self._topic, self.name)) + + def execute(self, userdata): + if not self._connected: + userdata.message = None + return 'unavailable' + + if self._sub.has_msg(self._topic) or not self._blocking: + userdata.message = self._sub.get_last_msg(self._topic) + self._sub.remove_last_msg(self._topic) + return 'received' + + def on_enter(self, userdata): + if not self._connected: + if self._connect(): + Logger.loginfo('Successfully subscribed to previously unavailable topic %s' % self._topic) + else: + Logger.logwarn('Topic %s still not available, giving up.' % self._topic) + + if self._connected and self._clear and self._sub.has_msg(self._topic): + self._sub.remove_last_msg(self._topic) + + def _connect(self): + msg_type, msg_topic, _ = rostopic.get_topic_class(self._topic) + if msg_topic == self._topic: + self._sub = ProxySubscriberCached({self._topic: msg_type}) + self._connected = True + return True + return False diff --git a/flexbe_states/src/flexbe_states/wait_state.py b/flexbe_states/src/flexbe_states/wait_state.py index d5565937..c527d934 100644 --- a/flexbe_states/src/flexbe_states/wait_state.py +++ b/flexbe_states/src/flexbe_states/wait_state.py @@ -1,44 +1,26 @@ #!/usr/bin/env python - import rospy from flexbe_core import EventState -from rospy.exceptions import ROSInterruptException - -''' -Created on 15.06.2013 -@author: David Conner -''' 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. - - <= done Indicates that the wait time has elapsed. + ''' + Implements a state that can be used to wait on timed process. - ''' - def __init__(self, wait_time): - '''Constructor''' - super(WaitState, self).__init__(outcomes=['done']) - self._wait = wait_time + -- wait_time float Amount of time to wait in seconds. - - def execute(self, userdata): - '''Execute this state''' + <= done Indicates that the wait time has elapsed. + ''' - elapsed = rospy.get_rostime() - self._start_time; - if (elapsed.to_sec() > self._wait): - return 'done' + def __init__(self, wait_time): + super(WaitState, self).__init__(outcomes=['done']) + self._wait = wait_time - - def on_enter(self, userdata): - '''Upon entering the state, save the current time and start waiting.''' + def execute(self, userdata): + elapsed = rospy.get_rostime() - self._start_time + if elapsed.to_sec() > self._wait: + return 'done' - self._start_time = rospy.get_rostime() - - try: - self._rate.sleep() - except ROSInterruptException: - rospy.logwarn('Skipped sleep.') + def on_enter(self, userdata): + '''Upon entering the state, save the current time and start waiting.''' + self._start_time = rospy.get_rostime() From 0d6d2422c631ba761ee05afc9f1a199702defbbb Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Sun, 14 Jun 2020 13:07:54 +0200 Subject: [PATCH 33/57] [flexbe_states] Fix ambiguous import of subscriber state --- .../src/flexbe_states/subscriber_state.py | 133 +++++++----------- 1 file changed, 53 insertions(+), 80 deletions(-) diff --git a/flexbe_states/src/flexbe_states/subscriber_state.py b/flexbe_states/src/flexbe_states/subscriber_state.py index 8d99dc20..64718f91 100644 --- a/flexbe_states/src/flexbe_states/subscriber_state.py +++ b/flexbe_states/src/flexbe_states/subscriber_state.py @@ -1,88 +1,61 @@ #!/usr/bin/env python -import rospy import rostopic -import inspect from flexbe_core import EventState, Logger from flexbe_core.proxy import ProxySubscriberCached -''' -Created on 11.06.2013 - -@author: Philipp Schillinger -''' class SubscriberState(EventState): - ''' - Gets the latest message on the given topic and stores it to userdata. - - -- topic string The topic on which should be listened. - -- blocking bool Blocks until a message is received. - -- clear bool Drops last message on this topic on enter - in order to only handle message received since this state is active. - - #> message object Latest message on the given topic of the respective type. - - <= received Message has been received and stored in userdata or state is not blocking. - <= unavailable The topic is not available when this state becomes actives. - - ''' - - - def __init__(self, topic, blocking = True, clear = False): - ''' - Constructor - ''' - super(SubscriberState, self).__init__(outcomes=['received', 'unavailable'], - output_keys=['message']) - - self._topic = topic - self._blocking = blocking - self._clear = clear - self._connected = False - - (msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic) - - if msg_topic == self._topic: - msg_type = self._get_msg_from_path(msg_path) - self._sub = ProxySubscriberCached({self._topic: msg_type}) - self._connected = True - else: - Logger.logwarn('Topic %s for state %s not yet available.\nFound: %s\nWill try again when entering the state...' % (self._topic, self.name, str(msg_topic))) - - - def execute(self, userdata): - ''' - Execute this state - ''' - if not self._connected: - userdata.message = None - return 'unavailable' - - if self._sub.has_msg(self._topic) or not self._blocking: - userdata.message = self._sub.get_last_msg(self._topic) - self._sub.remove_last_msg(self._topic) - return 'received' - - - def on_enter(self, userdata): - if not self._connected: - (msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic) - if msg_topic == self._topic: - msg_type = self._get_msg_from_path(msg_path) - self._sub = ProxySubscriberCached({self._topic: msg_type}) - self._connected = True - Logger.loginfo('Successfully subscribed to previously unavailable topic %s' % self._topic) - else: - Logger.logwarn('Topic %s still not available, giving up.' % self._topic) - - if self._connected and self._clear and self._sub.has_msg(self._topic): - self._sub.remove_last_msg(self._topic) - - - def _get_msg_from_path(self, msg_path): - msg_import = msg_path.split('/') - msg_module = '%s.msg' % (msg_import[0]) - package = __import__(msg_module, fromlist=[msg_module]) - clsmembers = inspect.getmembers(package, lambda member: inspect.isclass(member) and member.__module__.endswith(msg_import[1])) - return clsmembers[0][1] + ''' + Gets the latest message on the given topic and stores it to userdata. + + -- topic string The topic on which should be listened. + -- blocking bool Blocks until a message is received. + -- clear bool Drops last message on this topic on enter + in order to only handle message received since this state is active. + + #> message object Latest message on the given topic of the respective type. + + <= received Message has been received and stored in userdata or state is not blocking. + <= unavailable The topic is not available when this state becomes actives. + ''' + + def __init__(self, topic, blocking=True, clear=False): + super(SubscriberState, self).__init__(outcomes=['received', 'unavailable'], + output_keys=['message']) + self._topic = topic + self._blocking = blocking + self._clear = clear + self._connected = False + + if not self._connect(): + Logger.logwarn('Topic %s for state %s not yet available.\n' + 'Will try again when entering the state...' % (self._topic, self.name)) + + def execute(self, userdata): + if not self._connected: + userdata.message = None + return 'unavailable' + + if self._sub.has_msg(self._topic) or not self._blocking: + userdata.message = self._sub.get_last_msg(self._topic) + self._sub.remove_last_msg(self._topic) + return 'received' + + def on_enter(self, userdata): + if not self._connected: + if self._connect(): + Logger.loginfo('Successfully subscribed to previously unavailable topic %s' % self._topic) + else: + Logger.logwarn('Topic %s still not available, giving up.' % self._topic) + + if self._connected and self._clear and self._sub.has_msg(self._topic): + self._sub.remove_last_msg(self._topic) + + def _connect(self): + msg_type, msg_topic, _ = rostopic.get_topic_class(self._topic) + if msg_topic == self._topic: + self._sub = ProxySubscriberCached({self._topic: msg_type}) + self._connected = True + return True + return False From 4f1b854dedfb5831de2707e556c4ee81957e5f59 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Sun, 14 Jun 2020 13:09:06 +0200 Subject: [PATCH 34/57] [flexbe_core] Fix typo in userdata handling of concurrency container --- flexbe_core/src/flexbe_core/core/concurrency_container.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flexbe_core/src/flexbe_core/core/concurrency_container.py b/flexbe_core/src/flexbe_core/core/concurrency_container.py index 4f7ee87a..303203a6 100644 --- a/flexbe_core/src/flexbe_core/core/concurrency_container.py +++ b/flexbe_core/src/flexbe_core/core/concurrency_container.py @@ -82,7 +82,7 @@ def _execute_current_state(self): def _execute_single_state(self, state, force_exit=False): result = None try: - with UserData(reference=self.userdata, remap=self._remappings[state.name], + with UserData(reference=self._userdata, remap=self._remappings[state.name], input_keys=state.input_keys, output_keys=state.output_keys) as userdata: if force_exit: state._entering = True From aa5d8769370afc3232d878f8f32f5bec16865dfb Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Sun, 14 Jun 2020 13:55:14 +0200 Subject: [PATCH 35/57] Update changelog --- flexbe_behavior_engine/CHANGELOG.rst | 5 +++++ flexbe_core/CHANGELOG.rst | 12 ++++++++++++ flexbe_input/CHANGELOG.rst | 5 +++++ flexbe_mirror/CHANGELOG.rst | 5 +++++ flexbe_msgs/CHANGELOG.rst | 6 ++++++ flexbe_onboard/CHANGELOG.rst | 12 ++++++++++++ flexbe_states/CHANGELOG.rst | 7 +++++++ flexbe_testing/CHANGELOG.rst | 5 +++++ flexbe_widget/CHANGELOG.rst | 20 ++++++++++++++++++++ 9 files changed, 77 insertions(+) diff --git a/flexbe_behavior_engine/CHANGELOG.rst b/flexbe_behavior_engine/CHANGELOG.rst index 7e38ae12..9a285452 100644 --- a/flexbe_behavior_engine/CHANGELOG.rst +++ b/flexbe_behavior_engine/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package flexbe_behavior_engine ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge branch 'develop' into feature/state_logger_rework +* Contributors: Philipp Schillinger + 1.2.4 (2020-03-25) ------------------ diff --git a/flexbe_core/CHANGELOG.rst b/flexbe_core/CHANGELOG.rst index 5daf91f6..4f59c2e9 100644 --- a/flexbe_core/CHANGELOG.rst +++ b/flexbe_core/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package flexbe_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [flexbe_core] Consider a running mirror as being controlled + (see `#123 `_) +* Merge pull request `#113 `_ from team-vigir/feature/state_logger_rework + State Logger Rework +* Merge branch 'develop' into feature/state_logger_rework +* [flexbe_core] Allow to specify a subset of logged userdata keys +* [flexbe_core] Extend configuration options +* [flexbe_core] Rework state logger implementation +* Contributors: Philipp Schillinger + 1.2.4 (2020-03-25) ------------------ * [flexbe_core] Add tests for proxies diff --git a/flexbe_input/CHANGELOG.rst b/flexbe_input/CHANGELOG.rst index 847fff9e..8b2e712f 100644 --- a/flexbe_input/CHANGELOG.rst +++ b/flexbe_input/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package flexbe_input ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge branch 'develop' into feature/state_logger_rework +* Contributors: Philipp Schillinger + 1.2.4 (2020-03-25) ------------------ * Merge branch 'fmessmer-feature/python3_compatibility' into develop diff --git a/flexbe_mirror/CHANGELOG.rst b/flexbe_mirror/CHANGELOG.rst index 33fd7ca1..e15f0e88 100644 --- a/flexbe_mirror/CHANGELOG.rst +++ b/flexbe_mirror/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package flexbe_mirror ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge branch 'develop' into feature/state_logger_rework +* Contributors: Philipp Schillinger + 1.2.4 (2020-03-25) ------------------ * Merge branch 'fmessmer-feature/python3_compatibility' into develop diff --git a/flexbe_msgs/CHANGELOG.rst b/flexbe_msgs/CHANGELOG.rst index 9a83622c..73c7ee62 100644 --- a/flexbe_msgs/CHANGELOG.rst +++ b/flexbe_msgs/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package flexbe_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge branch 'develop' into feature/state_logger_rework +* [flexbe_msgs] Remove deprecated constants from input action +* Contributors: Philipp Schillinger + 1.2.4 (2020-03-25) ------------------ * [flexbe_msgs] [flexbe_core] Add debug level to logger diff --git a/flexbe_onboard/CHANGELOG.rst b/flexbe_onboard/CHANGELOG.rst index f7888a68..dbb0276b 100644 --- a/flexbe_onboard/CHANGELOG.rst +++ b/flexbe_onboard/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package flexbe_onboard ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [flexbe_onboard] Un-import behavior-specific imports after execution +* [flexbe_onboard] Add test cases for onboard engine +* Merge pull request `#113 `_ from team-vigir/feature/state_logger_rework + State Logger Rework +* Merge branch 'develop' into feature/state_logger_rework +* [flexbe_onboard] Cleanup onboard and add thread locks + (see `#117 `_) +* [flexbe_onboard] Expose new state logger args in onboard launch file +* Contributors: Philipp Schillinger + 1.2.4 (2020-03-25) ------------------ * Merge pull request `#110 `_ from team-vigir/fix/catkin_install diff --git a/flexbe_states/CHANGELOG.rst b/flexbe_states/CHANGELOG.rst index 9cf6b39c..7b0fc9c4 100644 --- a/flexbe_states/CHANGELOG.rst +++ b/flexbe_states/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package flexbe_states ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [flexbe_states] Fix ambiguous import of subscriber state +* Merge branch 'develop' into feature/state_logger_rework +* [flexbe_states] Clean up input state +* Contributors: Philipp Schillinger + 1.2.4 (2020-03-25) ------------------ * Merge branch 'fmessmer-feature/python3_compatibility' into develop diff --git a/flexbe_testing/CHANGELOG.rst b/flexbe_testing/CHANGELOG.rst index 1c2c1d3d..7241601a 100644 --- a/flexbe_testing/CHANGELOG.rst +++ b/flexbe_testing/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package flexbe_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge branch 'develop' into feature/state_logger_rework +* Contributors: Philipp Schillinger + 1.2.4 (2020-03-25) ------------------ * Merge pull request `#109 `_ from Achllle/feature/testing/timeout_parameter diff --git a/flexbe_widget/CHANGELOG.rst b/flexbe_widget/CHANGELOG.rst index a59aca15..02bd6e2e 100644 --- a/flexbe_widget/CHANGELOG.rst +++ b/flexbe_widget/CHANGELOG.rst @@ -2,6 +2,26 @@ Changelog for package flexbe_widget ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge pull request `#120 `_ from cheffe112/startup_race_condition + wait for READY status from Behavior Engine before launching behavior to avoid race conditions on startup +* avoid callback trigger before ready event has been created +* Merge pull request `#113 `_ from team-vigir/feature/state_logger_rework + State Logger Rework +* Merge branch 'develop' into feature/state_logger_rework +* wait for READY status from Behavior Engine before launching behavior + Whenever behavior_onboard and be_launcher are launched together, there used to be a race condition of publishing the behavior in behavior_launcher, but the subscriber in behavior_onboard not being ready yet. Hence, behavior_launcher now waits for the READY status to appear on the flexbe/status topic before it actually attempts to launch the behavior. +* [flexbe_widget] Update evaluate_logs script to new format +* Merge pull request `#118 `_ from StefanFabian/action_server_callback_based + Using event based action server instead of control loop. +* Improved preempt logic. +* Only accept goal if ActionServer is not active. +* Handle errors before behavior start. +* Using event based action server instead of control loop. + Waiting for terminal state of flexbe before setting goal to a terminal state and accepting a new one. +* Contributors: Philipp Schillinger, Stefan Fabian, Tobias Doernbach + 1.2.4 (2020-03-25) ------------------ * Merge pull request `#110 `_ from team-vigir/fix/catkin_install From d097f45ce2cc769ac68ce99eb6ca9c03b372c2f9 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Sun, 14 Jun 2020 13:55:26 +0200 Subject: [PATCH 36/57] 1.2.5 --- flexbe_behavior_engine/CHANGELOG.rst | 4 ++-- flexbe_behavior_engine/package.xml | 2 +- flexbe_core/CHANGELOG.rst | 4 ++-- flexbe_core/package.xml | 2 +- flexbe_input/CHANGELOG.rst | 4 ++-- flexbe_input/package.xml | 2 +- flexbe_mirror/CHANGELOG.rst | 4 ++-- flexbe_mirror/package.xml | 2 +- flexbe_msgs/CHANGELOG.rst | 4 ++-- flexbe_msgs/package.xml | 2 +- flexbe_onboard/CHANGELOG.rst | 4 ++-- flexbe_onboard/package.xml | 2 +- flexbe_states/CHANGELOG.rst | 4 ++-- flexbe_states/package.xml | 2 +- flexbe_testing/CHANGELOG.rst | 4 ++-- flexbe_testing/package.xml | 2 +- flexbe_widget/CHANGELOG.rst | 4 ++-- flexbe_widget/package.xml | 2 +- 18 files changed, 27 insertions(+), 27 deletions(-) diff --git a/flexbe_behavior_engine/CHANGELOG.rst b/flexbe_behavior_engine/CHANGELOG.rst index 9a285452..940650dc 100644 --- a/flexbe_behavior_engine/CHANGELOG.rst +++ b/flexbe_behavior_engine/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_behavior_engine ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.5 (2020-06-14) +------------------ * Merge branch 'develop' into feature/state_logger_rework * Contributors: Philipp Schillinger diff --git a/flexbe_behavior_engine/package.xml b/flexbe_behavior_engine/package.xml index 8bc1f761..98224f3f 100644 --- a/flexbe_behavior_engine/package.xml +++ b/flexbe_behavior_engine/package.xml @@ -1,7 +1,7 @@ flexbe_behavior_engine - 1.2.4 + 1.2.5 A meta-package to aggregate all the FlexBE packages diff --git a/flexbe_core/CHANGELOG.rst b/flexbe_core/CHANGELOG.rst index 4f59c2e9..169c28d5 100644 --- a/flexbe_core/CHANGELOG.rst +++ b/flexbe_core/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.5 (2020-06-14) +------------------ * [flexbe_core] Consider a running mirror as being controlled (see `#123 `_) * Merge pull request `#113 `_ from team-vigir/feature/state_logger_rework diff --git a/flexbe_core/package.xml b/flexbe_core/package.xml index 3ba85ff0..487cfbfd 100644 --- a/flexbe_core/package.xml +++ b/flexbe_core/package.xml @@ -1,6 +1,6 @@ flexbe_core - 1.2.4 + 1.2.5 flexbe_core provides the core smach extension for the FlexBE behavior engine. diff --git a/flexbe_input/CHANGELOG.rst b/flexbe_input/CHANGELOG.rst index 8b2e712f..e4928405 100644 --- a/flexbe_input/CHANGELOG.rst +++ b/flexbe_input/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_input ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.5 (2020-06-14) +------------------ * Merge branch 'develop' into feature/state_logger_rework * Contributors: Philipp Schillinger diff --git a/flexbe_input/package.xml b/flexbe_input/package.xml index ec6a5f14..3a239023 100644 --- a/flexbe_input/package.xml +++ b/flexbe_input/package.xml @@ -1,6 +1,6 @@ flexbe_input - 1.2.4 + 1.2.5 flexbe_input enables to send data to onboard behavior when required. diff --git a/flexbe_mirror/CHANGELOG.rst b/flexbe_mirror/CHANGELOG.rst index e15f0e88..36bc9007 100644 --- a/flexbe_mirror/CHANGELOG.rst +++ b/flexbe_mirror/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_mirror ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.5 (2020-06-14) +------------------ * Merge branch 'develop' into feature/state_logger_rework * Contributors: Philipp Schillinger diff --git a/flexbe_mirror/package.xml b/flexbe_mirror/package.xml index 6acd3406..eb154b70 100644 --- a/flexbe_mirror/package.xml +++ b/flexbe_mirror/package.xml @@ -1,6 +1,6 @@ flexbe_mirror - 1.2.4 + 1.2.5 flexbe_mirror implements functionality to remotely mirror an executed behavior. diff --git a/flexbe_msgs/CHANGELOG.rst b/flexbe_msgs/CHANGELOG.rst index 73c7ee62..8f72f214 100644 --- a/flexbe_msgs/CHANGELOG.rst +++ b/flexbe_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.5 (2020-06-14) +------------------ * Merge branch 'develop' into feature/state_logger_rework * [flexbe_msgs] Remove deprecated constants from input action * Contributors: Philipp Schillinger diff --git a/flexbe_msgs/package.xml b/flexbe_msgs/package.xml index f8e67abc..74e476f1 100644 --- a/flexbe_msgs/package.xml +++ b/flexbe_msgs/package.xml @@ -1,6 +1,6 @@ flexbe_msgs - 1.2.4 + 1.2.5 flexbe_msgs provides the messages used by FlexBE. diff --git a/flexbe_onboard/CHANGELOG.rst b/flexbe_onboard/CHANGELOG.rst index dbb0276b..ed17d37a 100644 --- a/flexbe_onboard/CHANGELOG.rst +++ b/flexbe_onboard/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_onboard ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.5 (2020-06-14) +------------------ * [flexbe_onboard] Un-import behavior-specific imports after execution * [flexbe_onboard] Add test cases for onboard engine * Merge pull request `#113 `_ from team-vigir/feature/state_logger_rework diff --git a/flexbe_onboard/package.xml b/flexbe_onboard/package.xml index 2f9804d6..491f04da 100644 --- a/flexbe_onboard/package.xml +++ b/flexbe_onboard/package.xml @@ -1,6 +1,6 @@ flexbe_onboard - 1.2.4 + 1.2.5 flexbe_onboard implements the robot-side of the behavior engine from where all behaviors are started. diff --git a/flexbe_states/CHANGELOG.rst b/flexbe_states/CHANGELOG.rst index 7b0fc9c4..a15f3628 100644 --- a/flexbe_states/CHANGELOG.rst +++ b/flexbe_states/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_states ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.5 (2020-06-14) +------------------ * [flexbe_states] Fix ambiguous import of subscriber state * Merge branch 'develop' into feature/state_logger_rework * [flexbe_states] Clean up input state diff --git a/flexbe_states/package.xml b/flexbe_states/package.xml index 55abe343..f3049daf 100644 --- a/flexbe_states/package.xml +++ b/flexbe_states/package.xml @@ -1,6 +1,6 @@ flexbe_states - 1.2.4 + 1.2.5 flexbe_states provides a collection of predefined states. Feel free to add new states. diff --git a/flexbe_testing/CHANGELOG.rst b/flexbe_testing/CHANGELOG.rst index 7241601a..06437006 100644 --- a/flexbe_testing/CHANGELOG.rst +++ b/flexbe_testing/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.5 (2020-06-14) +------------------ * Merge branch 'develop' into feature/state_logger_rework * Contributors: Philipp Schillinger diff --git a/flexbe_testing/package.xml b/flexbe_testing/package.xml index 43213437..bc77022c 100644 --- a/flexbe_testing/package.xml +++ b/flexbe_testing/package.xml @@ -1,6 +1,6 @@ flexbe_testing - 1.2.4 + 1.2.5 flexbe_testing provides a framework for unit testing states. diff --git a/flexbe_widget/CHANGELOG.rst b/flexbe_widget/CHANGELOG.rst index 02bd6e2e..7f1de10c 100644 --- a/flexbe_widget/CHANGELOG.rst +++ b/flexbe_widget/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_widget ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.5 (2020-06-14) +------------------ * Merge pull request `#120 `_ from cheffe112/startup_race_condition wait for READY status from Behavior Engine before launching behavior to avoid race conditions on startup * avoid callback trigger before ready event has been created diff --git a/flexbe_widget/package.xml b/flexbe_widget/package.xml index b2cf4099..94126c9f 100644 --- a/flexbe_widget/package.xml +++ b/flexbe_widget/package.xml @@ -1,6 +1,6 @@ flexbe_widget - 1.2.4 + 1.2.5 flexbe_widget implements some smaller scripts for the behavior engine. From f064a3cb55351107f06f1647343ae66db3cba87e Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Fri, 24 Jul 2020 12:21:38 +0200 Subject: [PATCH 37/57] [flexbe_widget] Accept more valid status codes by launcher --- flexbe_widget/src/flexbe_widget/behavior_launcher.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flexbe_widget/src/flexbe_widget/behavior_launcher.py b/flexbe_widget/src/flexbe_widget/behavior_launcher.py index 6f6c4242..4a90e9b3 100644 --- a/flexbe_widget/src/flexbe_widget/behavior_launcher.py +++ b/flexbe_widget/src/flexbe_widget/behavior_launcher.py @@ -38,7 +38,7 @@ def __init__(self): rospy.loginfo("%d behaviors available, ready for start request." % self._behavior_lib.count_behaviors()) def _status_callback(self, msg): - if msg.code == BEStatus.READY: + if msg.code in [BEStatus.READY, BEStatus.FINISHED, BEStatus.FAILED, BEStatus.ERROR]: self._ready_event.set() def _callback(self, msg): From bbe82cf28392ad82d09cce07cc6210579c91b6e0 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Fri, 24 Jul 2020 12:22:41 +0200 Subject: [PATCH 38/57] [flexbe_onboard] Remove clearing imports, not working robustly (see e.g. flexbe/flexbe_app#66) --- flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py b/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py index 74290e70..28fe9620 100644 --- a/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py +++ b/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py @@ -148,8 +148,9 @@ def _behavior_execution(self, msg): # done, remove left-overs like the temporary behavior file try: - if not self._switching: - self._clear_imports() + # hotfix: do not clear imports for now, not working correctly (e.g., flexbe/flexbe_app#66) + # if not self._switching: + # self._clear_imports() self._cleanup_behavior(msg.behavior_checksum) except Exception as e: rospy.logerr('Failed to clean up behavior:\n%s' % str(e)) From 56c878160277c8beae983991652dd67b4ed32e8e Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Fri, 24 Jul 2020 15:47:34 +0200 Subject: [PATCH 39/57] Add support for python3 --- flexbe_core/src/flexbe_core/behavior.py | 2 +- .../src/flexbe_core/behavior_library.py | 2 +- .../core/operatable_state_machine.py | 4 +- .../src/flexbe_core/core/state_machine.py | 2 +- flexbe_input/bin/behavior_input | 11 +- flexbe_mirror/bin/behavior_mirror_sm | 9 +- .../src/flexbe_mirror/flexbe_mirror.py | 2 +- flexbe_onboard/bin/start_behavior | 9 +- .../src/flexbe_onboard/flexbe_onboard.py | 2 +- flexbe_onboard/test/test_onboard.py | 4 +- flexbe_testing/bin/testing_node | 13 ++- flexbe_widget/bin/be_action_server | 19 ++-- flexbe_widget/bin/be_compress | 100 ++++++++++-------- flexbe_widget/bin/be_launcher | 93 ++++++++-------- flexbe_widget/bin/breakpoint | 9 +- flexbe_widget/bin/evaluate_logs | 15 ++- .../flexbe_widget/behavior_action_server.py | 6 +- .../src/flexbe_widget/behavior_launcher.py | 6 +- 18 files changed, 184 insertions(+), 124 deletions(-) diff --git a/flexbe_core/src/flexbe_core/behavior.py b/flexbe_core/src/flexbe_core/behavior.py index 4536f8a4..10ecc17f 100644 --- a/flexbe_core/src/flexbe_core/behavior.py +++ b/flexbe_core/src/flexbe_core/behavior.py @@ -229,7 +229,7 @@ def _set_typed_attribute(self, name, value): value = (value != "0") elif type(attr) is dict: import yaml - value = yaml.load(value) + value = yaml.full_load(value) setattr(self, name, value) def set_up(self, id, autonomy_level, debug): diff --git a/flexbe_core/src/flexbe_core/behavior_library.py b/flexbe_core/src/flexbe_core/behavior_library.py index 6e9b1a58..0cd13d84 100644 --- a/flexbe_core/src/flexbe_core/behavior_library.py +++ b/flexbe_core/src/flexbe_core/behavior_library.py @@ -53,7 +53,7 @@ def _add_behavior_manifests(self, path, pkg=None): e = m.find("executable") if pkg is not None and e.get("package_path").split(".")[0] != pkg: continue # ignore if manifest not in specified package - be_id = zlib.adler32(e.get("package_path")) + be_id = zlib.adler32(e.get("package_path").encode()) & 0x7fffffff self._behavior_lib[be_id] = { "name": m.get("name"), "package": ".".join(e.get("package_path").split(".")[:-1]), diff --git a/flexbe_core/src/flexbe_core/core/operatable_state_machine.py b/flexbe_core/src/flexbe_core/core/operatable_state_machine.py index ed3f0c28..52312ab3 100644 --- a/flexbe_core/src/flexbe_core/core/operatable_state_machine.py +++ b/flexbe_core/src/flexbe_core/core/operatable_state_machine.py @@ -108,7 +108,7 @@ def _execute_current_state(self): else: msg = BehaviorSync() msg.behavior_id = self.id - msg.current_state_checksum = zlib.adler32(self.get_deep_state().path) + msg.current_state_checksum = zlib.adler32(self.get_deep_state().path.encode()) & 0x7fffffff self._pub.publish('flexbe/mirror/sync', msg) return outcome @@ -177,7 +177,7 @@ def _sync_callback(self, msg): msg.behavior_id = self.id # make sure we are already executing self.wait(condition=lambda: self.get_deep_state() is not None) - msg.current_state_checksum = zlib.adler32(self.get_deep_state().path) + msg.current_state_checksum = zlib.adler32(self.get_deep_state().path.encode()) & 0x7fffffff self._pub.publish('flexbe/mirror/sync', msg) self._pub.publish('flexbe/command_feedback', CommandFeedback(command="sync", args=[])) Logger.localinfo("<-- Sent synchronization message for mirror.") diff --git a/flexbe_core/src/flexbe_core/core/state_machine.py b/flexbe_core/src/flexbe_core/core/state_machine.py index 23b2ee39..7e60b16e 100644 --- a/flexbe_core/src/flexbe_core/core/state_machine.py +++ b/flexbe_core/src/flexbe_core/core/state_machine.py @@ -138,7 +138,7 @@ def sleep_duration(self): @property def _valid_targets(self): - return self._labels.keys() + self.outcomes + return list(self._labels.keys()) + self.outcomes def assert_consistent_transitions(self): for transitions in self._transitions.values(): diff --git a/flexbe_input/bin/behavior_input b/flexbe_input/bin/behavior_input index 68833e31..67f7957a 100755 --- a/flexbe_input/bin/behavior_input +++ b/flexbe_input/bin/behavior_input @@ -1,6 +1,11 @@ -#!/usr/bin/env python - -import roslib; roslib.load_manifest('flexbe_input') +#!/bin/bash +if "true" : '''\' +then +python${ROS_PYTHON_VERSION:-} "${BASH_SOURCE[0]}" $* +exit +fi +''' +# flake8: noqa import rospy from flexbe_input.flexbe_input import BehaviorInput diff --git a/flexbe_mirror/bin/behavior_mirror_sm b/flexbe_mirror/bin/behavior_mirror_sm index b5ec3230..9846d9ea 100755 --- a/flexbe_mirror/bin/behavior_mirror_sm +++ b/flexbe_mirror/bin/behavior_mirror_sm @@ -1,4 +1,11 @@ -#!/usr/bin/env python +#!/bin/bash +if "true" : '''\' +then +python${ROS_PYTHON_VERSION:-} "${BASH_SOURCE[0]}" $* +exit +fi +''' +# flake8: noqa import rospy from flexbe_core.proxy import ProxySubscriberCached diff --git a/flexbe_mirror/src/flexbe_mirror/flexbe_mirror.py b/flexbe_mirror/src/flexbe_mirror/flexbe_mirror.py index a03fcf32..2ff9f238 100644 --- a/flexbe_mirror/src/flexbe_mirror/flexbe_mirror.py +++ b/flexbe_mirror/src/flexbe_mirror/flexbe_mirror.py @@ -207,7 +207,7 @@ def _mirror_state_machine(self, msg): # calculate checksums of all states for con_msg in msg.containers: if con_msg.path.find('/') != -1: - self._state_checksums[zlib.adler32(con_msg.path)] = con_msg.path + self._state_checksums[zlib.adler32(con_msg.path.encode()) & 0x7fffffff] = con_msg.path def _add_node(self, msg, path): container = None diff --git a/flexbe_onboard/bin/start_behavior b/flexbe_onboard/bin/start_behavior index e487d31c..21d3cabf 100755 --- a/flexbe_onboard/bin/start_behavior +++ b/flexbe_onboard/bin/start_behavior @@ -1,4 +1,11 @@ -#!/usr/bin/env python +#!/bin/bash +if "true" : '''\' +then +python${ROS_PYTHON_VERSION:-} "${BASH_SOURCE[0]}" $* +exit +fi +''' +# flake8: noqa import rospy from flexbe_core.proxy import ProxySubscriberCached diff --git a/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py b/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py index 191704aa..93987321 100644 --- a/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py +++ b/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py @@ -187,7 +187,7 @@ def _prepare_behavior(self, msg): 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) != msg.behavior_checksum: + if zlib.adler32(file_content.encode()) & 0x7fffffff != msg.behavior_checksum: 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" diff --git a/flexbe_onboard/test/test_onboard.py b/flexbe_onboard/test/test_onboard.py index 30e8d348..db4b8b56 100755 --- a/flexbe_onboard/test/test_onboard.py +++ b/flexbe_onboard/test/test_onboard.py @@ -58,7 +58,7 @@ def test_onboard_behaviors(self): # send valid simple behavior request with open(self.lib.get_sourcecode_filepath(be_id)) as f: - request.behavior_checksum = zlib.adler32(f.read()) + request.behavior_checksum = zlib.adler32(f.read().encode()) & 0x7fffffff behavior_pub.publish(request) self.assertStatus(BEStatus.STARTED, 1) self.assertStatus(BEStatus.FINISHED, 3) @@ -81,7 +81,7 @@ def test_onboard_behaviors(self): request.modifications.append(BehaviorModification(index, index + len(replace), by)) for replace, by in modifications: content = content.replace(replace, by) - request.behavior_checksum = zlib.adler32(content) + request.behavior_checksum = zlib.adler32(content.encode()) & 0x7fffffff behavior_pub.publish(request) self.assertStatus(BEStatus.STARTED, 1) result = self.assertStatus(BEStatus.FINISHED, 3) diff --git a/flexbe_testing/bin/testing_node b/flexbe_testing/bin/testing_node index 7a4dd3c5..40733d44 100755 --- a/flexbe_testing/bin/testing_node +++ b/flexbe_testing/bin/testing_node @@ -1,4 +1,11 @@ -#!/usr/bin/env python +#!/bin/bash +if "true" : '''\' +then +python${ROS_PYTHON_VERSION:-} "${BASH_SOURCE[0]}" $* +exit +fi +''' +# flake8: noqa import rospy import sys import os @@ -36,7 +43,7 @@ if __name__ == '__main__': test_name, _ = os.path.splitext(name) try: with open(filename, 'r') as f: - config = yaml.load(f) + config = yaml.full_load(f) test_cases[test_name] = config except IOError as io_error: rospy.logerr(io_error) @@ -46,7 +53,7 @@ if __name__ == '__main__': tester = Tester() success_cases = 0 - for test_name, test_config in test_cases.iteritems(): + for test_name, test_config in test_cases.items(): if rospy.is_shutdown(): break success_cases += tester.run_test(test_name, test_config) diff --git a/flexbe_widget/bin/be_action_server b/flexbe_widget/bin/be_action_server index 4d99ba3d..a55c54fa 100755 --- a/flexbe_widget/bin/be_action_server +++ b/flexbe_widget/bin/be_action_server @@ -1,15 +1,20 @@ -#!/usr/bin/env python - -import roslib; roslib.load_manifest('flexbe_widget') +#!/bin/bash +if "true" : '''\' +then +python${ROS_PYTHON_VERSION:-} "${BASH_SOURCE[0]}" $* +exit +fi +''' +# flake8: noqa import rospy from flexbe_widget.behavior_action_server import BehaviorActionServer if __name__ == '__main__': - rospy.init_node('flexbe_action_server') + rospy.init_node('flexbe_action_server') - server = BehaviorActionServer() + server = BehaviorActionServer() - # Wait for ctrl-c to stop the application - rospy.spin() + # Wait for ctrl-c to stop the application + rospy.spin() diff --git a/flexbe_widget/bin/be_compress b/flexbe_widget/bin/be_compress index 31a23d88..72351883 100755 --- a/flexbe_widget/bin/be_compress +++ b/flexbe_widget/bin/be_compress @@ -1,6 +1,11 @@ -#!/usr/bin/env python - -import roslib; roslib.load_manifest('flexbe_widget') +#!/bin/bash +if "true" : '''\' +then +python${ROS_PYTHON_VERSION:-} "${BASH_SOURCE[0]}" $* +exit +fi +''' +# flake8: noqa import rospy from flexbe_msgs.msg import BehaviorSelection from rospkg import RosPack @@ -11,59 +16,60 @@ import difflib def _callback(msg): - new_code = msg.behavior_code - rp = p = RosPack() - path = rp.get_path('vigir_behavior_flexbe_test_behavior') - path += "/src/vigir_behavior_flexbe_test_behavior/flexbe_test_behavior_sm_original.py" - with open(path) as f: - lines = [line.rstrip('\n') for line in f] - old_code = "\n".join(lines) - - sqm = difflib.SequenceMatcher(a=old_code, b=new_code) - diffs = [x[1] for x in sqm.get_grouped_opcodes(0)] - patch = [] - for opcode, a0, a1, b0, b1 in diffs: - content = new_code[b0:b1] - patch.append(tuple([a0, a1, content])) - print "Modifications:" - print str(patch) + new_code = msg.behavior_code + rp = RosPack() + path = rp.get_path('vigir_behavior_flexbe_test_behavior') + path += "/src/vigir_behavior_flexbe_test_behavior/flexbe_test_behavior_sm_original.py" + with open(path) as f: + lines = [line.rstrip('\n') for line in f] + old_code = "\n".join(lines) - packed = zlib.compress(pickle.dumps(patch), 9) - print "Size of compressed patch: " + str(packed.__sizeof__()) + sqm = difflib.SequenceMatcher(a=old_code, b=new_code) + diffs = [x[1] for x in sqm.get_grouped_opcodes(0)] + patch = [] + for opcode, a0, a1, b0, b1 in diffs: + content = new_code[b0:b1] + patch.append(tuple([a0, a1, content])) + print("Modifications:") + print(str(patch)) - received = pickle.loads(zlib.decompress(packed)) - gen_code = "" - last_index = 0 - for a0, a1, content in received: - gen_code += old_code[last_index:a0] + content - last_index = a1 - gen_code += old_code[last_index:] + packed = zlib.compress(pickle.dumps(patch), 9) + print("Size of compressed patch: " + str(packed.__sizeof__())) -# print gen_code - print "Differences between desired and reconstructed modifications:" - print str([x for x in difflib.unified_diff(gen_code, new_code)]) + received = pickle.loads(zlib.decompress(packed)) + gen_code = "" + last_index = 0 + for a0, a1, content in received: + gen_code += old_code[last_index:a0] + content + last_index = a1 + gen_code += old_code[last_index:] - sqm2 = difflib.SequenceMatcher(a=old_code, b=gen_code) - diffs2 = [x[1] for x in sqm2.get_grouped_opcodes(0)] - print "The following changes have been made:" - for opcode, a0, a1, b0, b1 in diffs2: - if opcode == "equal": continue - change_string = "\033[1m" + opcode + "\033[0m" + (" " if opcode == "replace" else " ") - if a0 != a1: - change_string += old_code[max(0, a0 - 10):a0] + "\033[41m" + old_code[a0:a1] + "\033[0m" + old_code[a1:min(len(old_code), a1 + 10)] - if a0 != a1 and b0 != b1: - change_string += " \033[1mby\033[0m " - if b0 != b1: - change_string += gen_code[max(0, b0 - 10):b0] + "\033[42m" + gen_code[b0:b1] + "\033[0m" + gen_code[b1:min(len(gen_code), b1 + 10)] - change_string = change_string.replace("\n", "\\n") - change_string = change_string.replace("\t", "\\t") - print change_string + print("Differences between desired and reconstructed modifications:") + print(str([x for x in difflib.unified_diff(gen_code, new_code)])) + sqm2 = difflib.SequenceMatcher(a=old_code, b=gen_code) + diffs2 = [x[1] for x in sqm2.get_grouped_opcodes(0)] + print("The following changes have been made:") + for opcode, a0, a1, b0, b1 in diffs2: + if opcode == "equal": + continue + change_string = "\033[1m" + opcode + "\033[0m" + (" " if opcode == "replace" else " ") + if a0 != a1: + change_string += (old_code[max(0, a0 - 10):a0] + "\033[41m" + old_code[a0:a1] + + "\033[0m" + old_code[a1:min(len(old_code), a1 + 10)]) + if a0 != a1 and b0 != b1: + change_string += " \033[1mby\033[0m " + if b0 != b1: + change_string += (gen_code[max(0, b0 - 10):b0] + "\033[42m" + gen_code[b0:b1] + + "\033[0m" + gen_code[b1:min(len(gen_code), b1 + 10)]) + change_string = change_string.replace("\n", "\\n") + change_string = change_string.replace("\t", "\\t") + print(change_string) if __name__ == '__main__': rospy.init_node('flexbe_widget') - + sub = rospy.Subscriber("/flexbe/start_behavior", BehaviorSelection, _callback) # Wait for ctrl-c to stop the application diff --git a/flexbe_widget/bin/be_launcher b/flexbe_widget/bin/be_launcher index 616ce5a3..97fa46f5 100755 --- a/flexbe_widget/bin/be_launcher +++ b/flexbe_widget/bin/be_launcher @@ -1,6 +1,11 @@ -#!/usr/bin/env python - -import roslib; roslib.load_manifest('flexbe_widget') +#!/bin/bash +if "true" : '''\' +then +python${ROS_PYTHON_VERSION:-} "${BASH_SOURCE[0]}" $* +exit +fi +''' +# flake8: noqa import rospy import sys import getopt @@ -8,45 +13,49 @@ import getopt from flexbe_widget.behavior_launcher import BehaviorLauncher from flexbe_msgs.msg import BehaviorRequest + def usage(): - print "Soon..." + print("Soon...") + if __name__ == '__main__': - rospy.init_node('flexbe_widget') - - try: - opts, args = getopt.getopt(sys.argv[1:], "hb:a:", ["help", "behavior=", "autonomy="]) - except getopt.GetoptError: - usage() - sys.exit(2) - - behavior = "" - autonomy = 255 - - for opt, arg in opts: - if opt in ("-h", "--help"): - usage() - sys.exit() - elif opt in ("-b", "--behavior"): - behavior = arg - elif opt in ("-a", "--autonomy"): - autonomy = int(arg) - ignore_args = ['__name', '__log'] # auto-set by roslaunch - - launcher = BehaviorLauncher() - - if behavior != "": - request = BehaviorRequest() - request.behavior_name = behavior - request.autonomy_level = autonomy - for arg in args: - if not ':=' in arg: continue - k, v = arg.split(':=', 1) - if k in ignore_args: continue - request.arg_keys.append('/'+k) - request.arg_values.append(v) - rospy.sleep(0.2) # wait for publishers... - launcher._callback(request) - - # Wait for ctrl-c to stop the application - rospy.spin() + rospy.init_node('flexbe_widget') + + try: + opts, args = getopt.getopt(sys.argv[1:], "hb:a:", ["help", "behavior=", "autonomy="]) + except getopt.GetoptError: + usage() + sys.exit(2) + + behavior = "" + autonomy = 255 + + for opt, arg in opts: + if opt in ("-h", "--help"): + usage() + sys.exit() + elif opt in ("-b", "--behavior"): + behavior = arg + elif opt in ("-a", "--autonomy"): + autonomy = int(arg) + ignore_args = ['__name', '__log'] # auto-set by roslaunch + + launcher = BehaviorLauncher() + + if behavior != "": + request = BehaviorRequest() + request.behavior_name = behavior + request.autonomy_level = autonomy + for arg in args: + if ':=' not in arg: + continue + k, v = arg.split(':=', 1) + if k in ignore_args: + continue + request.arg_keys.append('/'+k) + request.arg_values.append(v) + rospy.sleep(0.2) # wait for publishers... + launcher._callback(request) + + # Wait for ctrl-c to stop the application + rospy.spin() diff --git a/flexbe_widget/bin/breakpoint b/flexbe_widget/bin/breakpoint index 42052ba2..2cf321df 100755 --- a/flexbe_widget/bin/breakpoint +++ b/flexbe_widget/bin/breakpoint @@ -1,4 +1,11 @@ -#!/usr/bin/env python +#!/bin/bash +if "true" : '''\' +then +python${ROS_PYTHON_VERSION:-} "${BASH_SOURCE[0]}" $* +exit +fi +''' +# flake8: noqa import rospy import sys diff --git a/flexbe_widget/bin/evaluate_logs b/flexbe_widget/bin/evaluate_logs index 23b66aec..9fe0e382 100755 --- a/flexbe_widget/bin/evaluate_logs +++ b/flexbe_widget/bin/evaluate_logs @@ -1,4 +1,11 @@ -#!/usr/bin/env python +#!/bin/bash +if "true" : '''\' +then +python${ROS_PYTHON_VERSION:-} "${BASH_SOURCE[0]}" $* +exit +fi +''' +# flake8: noqa import sys import yaml import argparse @@ -33,7 +40,7 @@ if __name__ == '__main__': try: with open(args.logfile) as f: - logs = yaml.load(f) + logs = yaml.safe_load(f) except Exception as e: print('Failed to load logfile "%s":\n%s' % (str(args.logfile), str(e))) sys.exit(1) @@ -104,7 +111,7 @@ if __name__ == '__main__': 'transition': lambda s: "%s > %s" % (s['state_path'], s['outcome']) } key_function = key_definitions[args.key] - except KeyError as e: + except KeyError: print('Invalid key selected: %s\nAvailable: %s' % (str(args.key), ', '.join(key_definitions.keys()))) sys.exit(1) @@ -117,7 +124,7 @@ if __name__ == '__main__': accumulated[key] = defaultdict(int) for value in values: accumulated[key][value] += state[value] - except KeyError as e: + except KeyError: keys = set(state.keys()) - {'state_name', 'state_path', 'state_class'} print('Invalid value selected: %s\nAvailable: %s' % (str(args.key), ', '.join(keys))) sys.exit(1) diff --git a/flexbe_widget/src/flexbe_widget/behavior_action_server.py b/flexbe_widget/src/flexbe_widget/behavior_action_server.py index 3ad27bee..7010812f 100644 --- a/flexbe_widget/src/flexbe_widget/behavior_action_server.py +++ b/flexbe_widget/src/flexbe_widget/behavior_action_server.py @@ -71,7 +71,7 @@ def _goal_cb(self): with open(filepath, 'r') as f: content = f.read() if ns != '': - content = yaml.load(content) + content = yaml.full_load(content) if ns in content: content = content[ns] content = yaml.dump(content) @@ -94,7 +94,7 @@ def _goal_cb(self): be_filepath_old = self._behavior_lib.get_sourcecode_filepath(be_id, add_tmp=True) if not os.path.isfile(be_filepath_old): - be_selection.behavior_checksum = zlib.adler32(be_content_new) + be_selection.behavior_checksum = zlib.adler32(be_content_new.encode()) & 0x7fffffff else: with open(be_filepath_old, "r") as f: be_content_old = f.read() @@ -105,7 +105,7 @@ def _goal_cb(self): content = be_content_new[b0:b1] be_selection.modifications.append(BehaviorModification(a0, a1, content)) - be_selection.behavior_checksum = zlib.adler32(be_content_new) + be_selection.behavior_checksum = zlib.adler32(be_content_new.encode()) & 0x7fffffff # reset state before starting new behavior self._current_state = None diff --git a/flexbe_widget/src/flexbe_widget/behavior_launcher.py b/flexbe_widget/src/flexbe_widget/behavior_launcher.py index 2d37bb7a..f4df608f 100644 --- a/flexbe_widget/src/flexbe_widget/behavior_launcher.py +++ b/flexbe_widget/src/flexbe_widget/behavior_launcher.py @@ -57,7 +57,7 @@ def _callback(self, msg): else: yamlpath = os.path.join(self._rp.get_path(path.split('/')[0]), '/'.join(path.split('/')[1:])) with open(yamlpath, 'r') as f: - content = yaml.load(f) + content = yaml.full_load(f) if ns != '' and ns in content: content = content[ns] be_selection.arg_keys.append(key) @@ -85,7 +85,7 @@ def _callback(self, msg): be_filepath_old = self._behavior_lib.get_sourcecode_filepath(be_id, add_tmp=True) if not os.path.isfile(be_filepath_old): - be_selection.behavior_checksum = zlib.adler32(be_content_new) + be_selection.behavior_checksum = zlib.adler32(be_content_new.encode()) & 0x7fffffff if msg.autonomy_level != 255: be_structure.behavior_id = be_selection.behavior_checksum self._mirror_pub.publish(be_structure) @@ -102,7 +102,7 @@ def _callback(self, msg): content = be_content_new[b0:b1] be_selection.modifications.append(BehaviorModification(a0, a1, content)) - be_selection.behavior_checksum = zlib.adler32(be_content_new) + be_selection.behavior_checksum = zlib.adler32(be_content_new.encode()) & 0x7fffffff if msg.autonomy_level != 255: be_structure.behavior_id = be_selection.behavior_checksum self._mirror_pub.publish(be_structure) From db8bb91b92bbd144001c1d61e8653be04c57c217 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Fri, 24 Jul 2020 16:23:59 +0200 Subject: [PATCH 40/57] [flexbe_core] [flexbe_testing] [flexbe_widget] Use yaml backwards compatible --- flexbe_core/src/flexbe_core/behavior.py | 2 +- flexbe_testing/bin/testing_node | 2 +- flexbe_widget/src/flexbe_widget/behavior_action_server.py | 2 +- flexbe_widget/src/flexbe_widget/behavior_launcher.py | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/flexbe_core/src/flexbe_core/behavior.py b/flexbe_core/src/flexbe_core/behavior.py index 10ecc17f..b2839118 100644 --- a/flexbe_core/src/flexbe_core/behavior.py +++ b/flexbe_core/src/flexbe_core/behavior.py @@ -229,7 +229,7 @@ def _set_typed_attribute(self, name, value): value = (value != "0") elif type(attr) is dict: import yaml - value = yaml.full_load(value) + value = getattr(yaml, 'full_load', yaml.load)(value) setattr(self, name, value) def set_up(self, id, autonomy_level, debug): diff --git a/flexbe_testing/bin/testing_node b/flexbe_testing/bin/testing_node index 40733d44..e5953897 100755 --- a/flexbe_testing/bin/testing_node +++ b/flexbe_testing/bin/testing_node @@ -43,7 +43,7 @@ if __name__ == '__main__': test_name, _ = os.path.splitext(name) try: with open(filename, 'r') as f: - config = yaml.full_load(f) + config = getattr(yaml, 'full_load', yaml.load)(f) test_cases[test_name] = config except IOError as io_error: rospy.logerr(io_error) diff --git a/flexbe_widget/src/flexbe_widget/behavior_action_server.py b/flexbe_widget/src/flexbe_widget/behavior_action_server.py index 7010812f..050ed514 100644 --- a/flexbe_widget/src/flexbe_widget/behavior_action_server.py +++ b/flexbe_widget/src/flexbe_widget/behavior_action_server.py @@ -71,7 +71,7 @@ def _goal_cb(self): with open(filepath, 'r') as f: content = f.read() if ns != '': - content = yaml.full_load(content) + content = getattr(yaml, 'full_load', yaml.load)(content) if ns in content: content = content[ns] content = yaml.dump(content) diff --git a/flexbe_widget/src/flexbe_widget/behavior_launcher.py b/flexbe_widget/src/flexbe_widget/behavior_launcher.py index fec7387e..dec26b2a 100644 --- a/flexbe_widget/src/flexbe_widget/behavior_launcher.py +++ b/flexbe_widget/src/flexbe_widget/behavior_launcher.py @@ -64,7 +64,7 @@ def _callback(self, msg): else: yamlpath = os.path.join(self._rp.get_path(path.split('/')[0]), '/'.join(path.split('/')[1:])) with open(yamlpath, 'r') as f: - content = yaml.full_load(f) + content = getattr(yaml, 'full_load', yaml.load)(f) if ns != '' and ns in content: content = content[ns] be_selection.arg_keys.append(key) From 027b5b1b6383d5c9f3694fb9e5410a6e31a26751 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Wed, 4 Nov 2020 18:07:13 +0100 Subject: [PATCH 41/57] [flexbe_core] Several fixes to userdata (see #129) --- flexbe_core/src/flexbe_core/core/user_data.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/flexbe_core/src/flexbe_core/core/user_data.py b/flexbe_core/src/flexbe_core/core/user_data.py index 5df87c10..d4706fea 100644 --- a/flexbe_core/src/flexbe_core/core/user_data.py +++ b/flexbe_core/src/flexbe_core/core/user_data.py @@ -1,4 +1,5 @@ #!/user/bin/env python +from copy import deepcopy from flexbe_core.core.exceptions import UserDataError @@ -6,7 +7,7 @@ class UserData(object): def __init__(self, reference=None, input_keys=None, output_keys=None, remap=None): self._data = dict() - self._reference = reference or dict() + self._reference = reference if reference is not None else dict() self._input_keys = input_keys self._output_keys = output_keys self._remap = remap or dict() @@ -29,7 +30,7 @@ def __contains__(self, key): return self._remap.get(key, key) in self._reference def __getitem__(self, key): - if key in self._data: + if key in self._data and self._remap.get(key, key) not in self._reference: return self._data[key] if key not in self: raise UserDataError("Key '%s' cannot be accessed, declare it as input key for read access." % key @@ -39,6 +40,11 @@ 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): + # 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. + value = deepcopy(value) return value def __setitem__(self, key, value): From dcb4f7bbaac8e27776663fb7eb3753cea537fb80 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Fri, 23 Oct 2020 15:00:33 +0200 Subject: [PATCH 42/57] Optionally fail test if launch-file fails If the launch-file in a FlexBE test fails, signal this to the TestContext that can then optionally fail due to this This allows to run scripts etc to verify State/Behavior side effects --- .../src/flexbe_testing/test_context.py | 20 +++++++++++++++++++ flexbe_testing/src/flexbe_testing/tester.py | 5 +++++ 2 files changed, 25 insertions(+) diff --git a/flexbe_testing/src/flexbe_testing/test_context.py b/flexbe_testing/src/flexbe_testing/test_context.py index 135364df..32ab2791 100644 --- a/flexbe_testing/src/flexbe_testing/test_context.py +++ b/flexbe_testing/src/flexbe_testing/test_context.py @@ -8,6 +8,15 @@ from .logger import Logger +class Callback(roslaunch.pmon.ProcessListener): + def __init__(self, callback): + self._callback = callback + + def process_died(self, process_name, exit_code): + rospy.loginfo("{}, {}".format(process_name, exit_code)) + self._callback(process_name, exit_code) + + class TestContext(object): """ Default context for a test case. @@ -29,6 +38,10 @@ def spin_once(self): def __exit__(self, exception_type, exception_value, traceback): pass + @property + def success(self): + return True + class LaunchContext(TestContext): """ Test context that runs a specified launch file configuration. """ @@ -38,6 +51,8 @@ def __init__(self, launch_config, wait_cond="True"): launchpath = None launchcontent = None + self._exit_codes = [] + # load from system path if launch_config.startswith('~') or launch_config.startswith('/'): launchpath = os.path.expanduser(launch_config) @@ -57,6 +72,7 @@ def __init__(self, launch_config, wait_cond="True"): else: loader.load_string(launchcontent, launchconfig, verbose=False) self._launchrunner = roslaunch.launch.ROSLaunchRunner(self._run_id, launchconfig) + self._launchrunner.add_process_listener(Callback(lambda process_name, exit_code: self._exit_codes.append(exit_code))) self._wait_cond = wait_cond self._valid = True @@ -86,3 +102,7 @@ def spin_once(self): def __exit__(self, exception_type, exception_value, traceback): self._launchrunner.stop() Logger.print_positive('launchfile stopped') + + @property + def success(self): + return not any(code > 0 for code in self._exit_codes) diff --git a/flexbe_testing/src/flexbe_testing/tester.py b/flexbe_testing/src/flexbe_testing/tester.py index ae15d192..5ad9b649 100644 --- a/flexbe_testing/src/flexbe_testing/tester.py +++ b/flexbe_testing/src/flexbe_testing/tester.py @@ -114,6 +114,11 @@ def run_test(self, name, config): else: Logger.print_negative('no result for %s' % expected_key) output_ok = False + + if not context.success and config.get('require_launch_success', False): + Logger.print_negative('Launch file did not exit cleanly') + output_ok = False + if len(expected) > 0 and output_ok: Logger.print_positive('all result outputs match expected') From 0b2dc264493f07b784aa3e2827d427449dc969e3 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Mon, 26 Oct 2020 13:40:55 +0100 Subject: [PATCH 43/57] Check all launched nodes have exited And check they exit code to decide success/fail of launch file --- .../src/flexbe_testing/test_context.py | 21 ++++++++++++++++--- flexbe_testing/src/flexbe_testing/tester.py | 3 +++ 2 files changed, 21 insertions(+), 3 deletions(-) diff --git a/flexbe_testing/src/flexbe_testing/test_context.py b/flexbe_testing/src/flexbe_testing/test_context.py index 32ab2791..c49eb015 100644 --- a/flexbe_testing/src/flexbe_testing/test_context.py +++ b/flexbe_testing/src/flexbe_testing/test_context.py @@ -38,6 +38,9 @@ def spin_once(self): def __exit__(self, exception_type, exception_value, traceback): pass + def wait_for_finishing(self): + pass + @property def success(self): return True @@ -51,7 +54,8 @@ def __init__(self, launch_config, wait_cond="True"): launchpath = None launchcontent = None - self._exit_codes = [] + self._launched_proc_names = [] + self._exit_codes = {} # load from system path if launch_config.startswith('~') or launch_config.startswith('/'): @@ -72,7 +76,10 @@ def __init__(self, launch_config, wait_cond="True"): else: loader.load_string(launchcontent, launchconfig, verbose=False) self._launchrunner = roslaunch.launch.ROSLaunchRunner(self._run_id, launchconfig) - self._launchrunner.add_process_listener(Callback(lambda process_name, exit_code: self._exit_codes.append(exit_code))) + + def store(process_name, exit_code): + self._exit_codes[process_name] = exit_code + self._launchrunner.add_process_listener(Callback(store)) self._wait_cond = wait_cond self._valid = True @@ -82,6 +89,8 @@ def __enter__(self): Logger.print_positive('launchfile running') self._valid = True + self._launched_proc_names = [p.name for p in self._launchrunner.pm.procs] + try: check_running_rate = rospy.Rate(10) is_running = False @@ -99,10 +108,16 @@ def verify(self): def spin_once(self): self._launchrunner.spin_once() + def wait_for_finishing(self): + check_exited_rate = rospy.Rate(10) + rospy.loginfo("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() + def __exit__(self, exception_type, exception_value, traceback): self._launchrunner.stop() Logger.print_positive('launchfile stopped') @property def success(self): - return not any(code > 0 for code in self._exit_codes) + return not any(code > 0 for code in self._exit_codes.values()) diff --git a/flexbe_testing/src/flexbe_testing/tester.py b/flexbe_testing/src/flexbe_testing/tester.py index 5ad9b649..8a386328 100644 --- a/flexbe_testing/src/flexbe_testing/tester.py +++ b/flexbe_testing/src/flexbe_testing/tester.py @@ -92,6 +92,9 @@ def run_test(self, name, config): self._tests['test_%s_pass' % name] = self._test_pass(False) return 0 + if config.get('require_launch_success', False): + context.wait_for_finishing() + # evaluate outcome self._tests['test_%s_outcome' % name] = self._test_outcome(outcome, config['outcome']) outcome_ok = outcome == config['outcome'] From d2cd790fc9e63465983b3abfa36a7a8922bef837 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Thu, 5 Nov 2020 15:00:02 +0100 Subject: [PATCH 44/57] Clear up logging for exiting processes --- flexbe_testing/src/flexbe_testing/test_context.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flexbe_testing/src/flexbe_testing/test_context.py b/flexbe_testing/src/flexbe_testing/test_context.py index c49eb015..d2cea5be 100644 --- a/flexbe_testing/src/flexbe_testing/test_context.py +++ b/flexbe_testing/src/flexbe_testing/test_context.py @@ -13,7 +13,7 @@ def __init__(self, callback): self._callback = callback def process_died(self, process_name, exit_code): - rospy.loginfo("{}, {}".format(process_name, exit_code)) + rospy.loginfo("Process {} exited with {}".format(process_name, exit_code)) self._callback(process_name, exit_code) From ab1e5089d2ad296ab5900c1ddb15c5cb3dd09898 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Mon, 9 Nov 2020 18:04:44 +0100 Subject: [PATCH 45/57] [flexbe_core] [flexbe_widget] Correctly handle non-existing behaviors in action goals (fix #133) --- flexbe_core/src/flexbe_core/behavior_library.py | 6 +++++- flexbe_widget/src/flexbe_widget/behavior_action_server.py | 2 +- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/flexbe_core/src/flexbe_core/behavior_library.py b/flexbe_core/src/flexbe_core/behavior_library.py index 0cd13d84..12227fa1 100644 --- a/flexbe_core/src/flexbe_core/behavior_library.py +++ b/flexbe_core/src/flexbe_core/behavior_library.py @@ -94,7 +94,11 @@ def find_behavior(self, be_name): except StopIteration: Logger.logwarn("Did not find behavior '%s' in libary, updating..." % be_name) self.parse_packages() - return find() + try: + return find() + except StopIteration: + Logger.logerr("Still cannot find behavior '%s' in libary after update, giving up!" % be_name) + return None, None def count_behaviors(self): """ diff --git a/flexbe_widget/src/flexbe_widget/behavior_action_server.py b/flexbe_widget/src/flexbe_widget/behavior_action_server.py index 050ed514..9f7edb59 100644 --- a/flexbe_widget/src/flexbe_widget/behavior_action_server.py +++ b/flexbe_widget/src/flexbe_widget/behavior_action_server.py @@ -48,7 +48,7 @@ def _goal_cb(self): rospy.loginfo('Received a new request to start behavior: %s' % goal.behavior_name) be_id, behavior = self._behavior_lib.find_behavior(goal.behavior_name) if be_id is None: - Logger.logerr("Did not find behavior with requested name: %s" % goal.behavior_name) + rospy.logerr("Deny goal: Did not find behavior with requested name %s" % goal.behavior_name) self._as.set_preempted() return From ad766aa51d1b48069efbd9efd1dcdecca44b1ee4 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Thu, 12 Nov 2020 17:34:11 +0100 Subject: [PATCH 46/57] [flexbe_core] Further fixes to userdata --- flexbe_core/src/flexbe_core/behavior.py | 6 ++++-- flexbe_core/src/flexbe_core/core/user_data.py | 6 ++++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/flexbe_core/src/flexbe_core/behavior.py b/flexbe_core/src/flexbe_core/behavior.py index b2839118..9149c322 100644 --- a/flexbe_core/src/flexbe_core/behavior.py +++ b/flexbe_core/src/flexbe_core/behavior.py @@ -99,6 +99,8 @@ def use_behavior(self, behavior_class, behavior_id, default_keys=None, parameter if default_keys is not None: state_machine._input_keys = list(set(state_machine._input_keys) - set(default_keys)) + for key in state_machine._input_keys: + state_machine._own_userdata(remove_key=key) return state_machine @@ -115,8 +117,8 @@ def prepare_for_execution(self, input_data=None): if input_data is None: input_data = dict() for k, v in input_data.items(): - if k in self._state_machine.userdata: - self._state_machine.userdata[k] = v + if k in self._state_machine._own_userdata: + self._state_machine._own_userdata[k] = v def set_parameter(self, name, value): """ diff --git a/flexbe_core/src/flexbe_core/core/user_data.py b/flexbe_core/src/flexbe_core/core/user_data.py index d4706fea..3bc99c44 100644 --- a/flexbe_core/src/flexbe_core/core/user_data.py +++ b/flexbe_core/src/flexbe_core/core/user_data.py @@ -30,7 +30,7 @@ def __contains__(self, key): return self._remap.get(key, key) in self._reference def __getitem__(self, key): - if key in self._data and self._remap.get(key, key) not in self._reference: + if key in self._data: return self._data[key] if key not in self: raise UserDataError("Key '%s' cannot be accessed, declare it as input key for read access." % key @@ -64,7 +64,7 @@ def __setattr__(self, key, value): raise UserDataError("Key '%s' cannot be set, declare it as output key for write access." % key) self[key] = value - def __call__(self, reference=None, add_from=None, update_from=None): + def __call__(self, reference=None, add_from=None, update_from=None, remove_key=None): self._reference = reference or self._reference if isinstance(add_from, UserData): for key, value in add_from._data.items(): @@ -73,6 +73,8 @@ def __call__(self, reference=None, add_from=None, update_from=None): if isinstance(update_from, UserData): for key, value in update_from._data.items(): self._data[key] = value + if remove_key is not None and remove_key in self._data: + del self._data[remove_key] def __len__(self): return len(self._data) + len(self._reference) From 081911598c21051686acbfb1b63a38b2635f8fdb Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Thu, 19 Nov 2020 11:33:55 +0100 Subject: [PATCH 47/57] Update changelog --- flexbe_behavior_engine/CHANGELOG.rst | 3 +++ flexbe_core/CHANGELOG.rst | 23 +++++++++++++++++++++++ flexbe_input/CHANGELOG.rst | 10 ++++++++++ flexbe_mirror/CHANGELOG.rst | 12 ++++++++++++ flexbe_msgs/CHANGELOG.rst | 9 +++++++++ flexbe_onboard/CHANGELOG.rst | 13 +++++++++++++ flexbe_states/CHANGELOG.rst | 11 +++++++++++ flexbe_testing/CHANGELOG.rst | 22 ++++++++++++++++++++++ flexbe_widget/CHANGELOG.rst | 14 ++++++++++++++ 9 files changed, 117 insertions(+) diff --git a/flexbe_behavior_engine/CHANGELOG.rst b/flexbe_behavior_engine/CHANGELOG.rst index 940650dc..b7cdf2da 100644 --- a/flexbe_behavior_engine/CHANGELOG.rst +++ b/flexbe_behavior_engine/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package flexbe_behavior_engine ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.2.5 (2020-06-14) ------------------ * Merge branch 'develop' into feature/state_logger_rework diff --git a/flexbe_core/CHANGELOG.rst b/flexbe_core/CHANGELOG.rst index 169c28d5..a7a61d3e 100644 --- a/flexbe_core/CHANGELOG.rst +++ b/flexbe_core/CHANGELOG.rst @@ -2,6 +2,29 @@ Changelog for package flexbe_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [flexbe_core] Further fixes to userdata +* [flexbe_core] [flexbe_widget] Correctly handle non-existing behaviors in action goals + (fix `#133 `_) +* [flexbe_core] Several fixes to userdata + (see `#129 `_) +* [flexbe_core] [flexbe_testing] [flexbe_widget] Use yaml backwards compatible +* Merge remote-tracking branch 'origin/feature/core_rework' into develop + # Conflicts: + # flexbe_core/src/flexbe_core/core/operatable_state_machine.py + # flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py +* Merge branch 'feature/core_rework' of https://github.com/team-vigir/flexbe_behavior_engine into feature/core_rework +* Add support for python3 +* [flexbe_core] Fix typo in userdata handling of concurrency container +* [flexbe_core] Use more explicit userdata update operations +* [flexbe_core] Add userdata tests and improvements +* [flexbe_core] Add checks and specific exception types +* [flexbe_core] Add test cases for state capabilities +* Major clean-up of most core components +* Remove smach dependency +* Contributors: Philipp Schillinger + 1.2.5 (2020-06-14) ------------------ * [flexbe_core] Consider a running mirror as being controlled diff --git a/flexbe_input/CHANGELOG.rst b/flexbe_input/CHANGELOG.rst index e4928405..ed0ee883 100644 --- a/flexbe_input/CHANGELOG.rst +++ b/flexbe_input/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package flexbe_input ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge remote-tracking branch 'origin/feature/core_rework' into develop + # Conflicts: + # flexbe_core/src/flexbe_core/core/operatable_state_machine.py + # flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py +* Add support for python3 +* Remove smach dependency +* Contributors: Philipp Schillinger + 1.2.5 (2020-06-14) ------------------ * Merge branch 'develop' into feature/state_logger_rework diff --git a/flexbe_mirror/CHANGELOG.rst b/flexbe_mirror/CHANGELOG.rst index 36bc9007..4c242b0e 100644 --- a/flexbe_mirror/CHANGELOG.rst +++ b/flexbe_mirror/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package flexbe_mirror ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge remote-tracking branch 'origin/feature/core_rework' into develop + # Conflicts: + # flexbe_core/src/flexbe_core/core/operatable_state_machine.py + # flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py +* Add support for python3 +* [flexbe_mirror] Minor cleanup of mirror +* Major clean-up of most core components +* Remove smach dependency +* Contributors: Philipp Schillinger + 1.2.5 (2020-06-14) ------------------ * Merge branch 'develop' into feature/state_logger_rework diff --git a/flexbe_msgs/CHANGELOG.rst b/flexbe_msgs/CHANGELOG.rst index 8f72f214..e3d19d03 100644 --- a/flexbe_msgs/CHANGELOG.rst +++ b/flexbe_msgs/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package flexbe_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge remote-tracking branch 'origin/feature/core_rework' into develop + # Conflicts: + # flexbe_core/src/flexbe_core/core/operatable_state_machine.py + # flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py +* Remove smach dependency +* Contributors: Philipp Schillinger + 1.2.5 (2020-06-14) ------------------ * Merge branch 'develop' into feature/state_logger_rework diff --git a/flexbe_onboard/CHANGELOG.rst b/flexbe_onboard/CHANGELOG.rst index ed17d37a..37df53f8 100644 --- a/flexbe_onboard/CHANGELOG.rst +++ b/flexbe_onboard/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package flexbe_onboard ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge remote-tracking branch 'origin/feature/core_rework' into develop + # Conflicts: + # flexbe_core/src/flexbe_core/core/operatable_state_machine.py + # flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py +* Add support for python3 +* [flexbe_onboard] Remove clearing imports, not working robustly + (see e.g. `flexbe/flexbe_app#66 `_) +* Major clean-up of most core components +* Remove smach dependency +* Contributors: Philipp Schillinger + 1.2.5 (2020-06-14) ------------------ * [flexbe_onboard] Un-import behavior-specific imports after execution diff --git a/flexbe_states/CHANGELOG.rst b/flexbe_states/CHANGELOG.rst index a15f3628..4f61de20 100644 --- a/flexbe_states/CHANGELOG.rst +++ b/flexbe_states/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package flexbe_states ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge remote-tracking branch 'origin/feature/core_rework' into develop + # Conflicts: + # flexbe_core/src/flexbe_core/core/operatable_state_machine.py + # flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py +* [flexbe_states] Cleanup of states +* Major clean-up of most core components +* Remove smach dependency +* Contributors: Philipp Schillinger + 1.2.5 (2020-06-14) ------------------ * [flexbe_states] Fix ambiguous import of subscriber state diff --git a/flexbe_testing/CHANGELOG.rst b/flexbe_testing/CHANGELOG.rst index 06437006..7bc61bd5 100644 --- a/flexbe_testing/CHANGELOG.rst +++ b/flexbe_testing/CHANGELOG.rst @@ -2,6 +2,28 @@ Changelog for package flexbe_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge pull request `#132 `_ from LoyVanBeek/feature/test_require_launch_file_success + Optionally fail test if launch-file fails +* Clear up logging for exiting processes +* Check all launched nodes have exited + And check they exit code to decide success/fail of launch file +* Optionally fail test if launch-file fails + If the launch-file in a FlexBE test fails, signal this to the TestContext that can then optionally fail due to this + This allows to run scripts etc to verify State/Behavior side effects +* [flexbe_core] [flexbe_testing] [flexbe_widget] Use yaml backwards compatible +* Merge remote-tracking branch 'origin/feature/core_rework' into develop + # Conflicts: + # flexbe_core/src/flexbe_core/core/operatable_state_machine.py + # flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py +* Add support for python3 +* [flexbe_testing] Add a behavior to the self-test +* [flexbe_testing] Fix check of userdata output +* Major clean-up of most core components +* Remove smach dependency +* Contributors: Loy van Beek, Philipp Schillinger + 1.2.5 (2020-06-14) ------------------ * Merge branch 'develop' into feature/state_logger_rework diff --git a/flexbe_widget/CHANGELOG.rst b/flexbe_widget/CHANGELOG.rst index 7f1de10c..53f0e803 100644 --- a/flexbe_widget/CHANGELOG.rst +++ b/flexbe_widget/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package flexbe_widget ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [flexbe_core] [flexbe_widget] Correctly handle non-existing behaviors in action goals + (fix `#133 `_) +* [flexbe_core] [flexbe_testing] [flexbe_widget] Use yaml backwards compatible +* Merge remote-tracking branch 'origin/feature/core_rework' into develop + # Conflicts: + # flexbe_core/src/flexbe_core/core/operatable_state_machine.py + # flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py +* Add support for python3 +* [flexbe_widget] Accept more valid status codes by launcher +* Remove smach dependency +* Contributors: Philipp Schillinger + 1.2.5 (2020-06-14) ------------------ * Merge pull request `#120 `_ from cheffe112/startup_race_condition From 96801db5ef93940632564e496c04791c6ca207e4 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Thu, 19 Nov 2020 11:35:53 +0100 Subject: [PATCH 48/57] 1.3.0 --- flexbe_behavior_engine/CHANGELOG.rst | 4 ++-- flexbe_behavior_engine/package.xml | 2 +- flexbe_core/CHANGELOG.rst | 4 ++-- flexbe_core/package.xml | 2 +- flexbe_input/CHANGELOG.rst | 4 ++-- flexbe_input/package.xml | 2 +- flexbe_mirror/CHANGELOG.rst | 4 ++-- flexbe_mirror/package.xml | 2 +- flexbe_msgs/CHANGELOG.rst | 4 ++-- flexbe_msgs/package.xml | 2 +- flexbe_onboard/CHANGELOG.rst | 4 ++-- flexbe_onboard/package.xml | 2 +- flexbe_states/CHANGELOG.rst | 4 ++-- flexbe_states/package.xml | 2 +- flexbe_testing/CHANGELOG.rst | 4 ++-- flexbe_testing/package.xml | 2 +- flexbe_widget/CHANGELOG.rst | 4 ++-- flexbe_widget/package.xml | 2 +- 18 files changed, 27 insertions(+), 27 deletions(-) diff --git a/flexbe_behavior_engine/CHANGELOG.rst b/flexbe_behavior_engine/CHANGELOG.rst index b7cdf2da..7beace0a 100644 --- a/flexbe_behavior_engine/CHANGELOG.rst +++ b/flexbe_behavior_engine/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_behavior_engine ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.3.0 (2020-11-19) +------------------ 1.2.5 (2020-06-14) ------------------ diff --git a/flexbe_behavior_engine/package.xml b/flexbe_behavior_engine/package.xml index 98224f3f..a59f28aa 100644 --- a/flexbe_behavior_engine/package.xml +++ b/flexbe_behavior_engine/package.xml @@ -1,7 +1,7 @@ flexbe_behavior_engine - 1.2.5 + 1.3.0 A meta-package to aggregate all the FlexBE packages diff --git a/flexbe_core/CHANGELOG.rst b/flexbe_core/CHANGELOG.rst index a7a61d3e..de06d6ac 100644 --- a/flexbe_core/CHANGELOG.rst +++ b/flexbe_core/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.3.0 (2020-11-19) +------------------ * [flexbe_core] Further fixes to userdata * [flexbe_core] [flexbe_widget] Correctly handle non-existing behaviors in action goals (fix `#133 `_) diff --git a/flexbe_core/package.xml b/flexbe_core/package.xml index c1772d32..5f50c5ba 100644 --- a/flexbe_core/package.xml +++ b/flexbe_core/package.xml @@ -1,6 +1,6 @@ flexbe_core - 1.2.5 + 1.3.0 flexbe_core provides the core components for the FlexBE behavior engine. diff --git a/flexbe_input/CHANGELOG.rst b/flexbe_input/CHANGELOG.rst index ed0ee883..fe340999 100644 --- a/flexbe_input/CHANGELOG.rst +++ b/flexbe_input/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_input ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.3.0 (2020-11-19) +------------------ * Merge remote-tracking branch 'origin/feature/core_rework' into develop # Conflicts: # flexbe_core/src/flexbe_core/core/operatable_state_machine.py diff --git a/flexbe_input/package.xml b/flexbe_input/package.xml index 89c8d282..464e7176 100644 --- a/flexbe_input/package.xml +++ b/flexbe_input/package.xml @@ -1,6 +1,6 @@ flexbe_input - 1.2.5 + 1.3.0 flexbe_input enables to send data to onboard behavior when required. diff --git a/flexbe_mirror/CHANGELOG.rst b/flexbe_mirror/CHANGELOG.rst index 4c242b0e..4a73efa3 100644 --- a/flexbe_mirror/CHANGELOG.rst +++ b/flexbe_mirror/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_mirror ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.3.0 (2020-11-19) +------------------ * Merge remote-tracking branch 'origin/feature/core_rework' into develop # Conflicts: # flexbe_core/src/flexbe_core/core/operatable_state_machine.py diff --git a/flexbe_mirror/package.xml b/flexbe_mirror/package.xml index 97ea7a9f..880149ef 100644 --- a/flexbe_mirror/package.xml +++ b/flexbe_mirror/package.xml @@ -1,6 +1,6 @@ flexbe_mirror - 1.2.5 + 1.3.0 flexbe_mirror implements functionality to remotely mirror an executed behavior. diff --git a/flexbe_msgs/CHANGELOG.rst b/flexbe_msgs/CHANGELOG.rst index e3d19d03..8a057efa 100644 --- a/flexbe_msgs/CHANGELOG.rst +++ b/flexbe_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.3.0 (2020-11-19) +------------------ * Merge remote-tracking branch 'origin/feature/core_rework' into develop # Conflicts: # flexbe_core/src/flexbe_core/core/operatable_state_machine.py diff --git a/flexbe_msgs/package.xml b/flexbe_msgs/package.xml index 6c820430..df327906 100644 --- a/flexbe_msgs/package.xml +++ b/flexbe_msgs/package.xml @@ -1,6 +1,6 @@ flexbe_msgs - 1.2.5 + 1.3.0 flexbe_msgs provides the messages used by FlexBE. diff --git a/flexbe_onboard/CHANGELOG.rst b/flexbe_onboard/CHANGELOG.rst index 37df53f8..5815e38a 100644 --- a/flexbe_onboard/CHANGELOG.rst +++ b/flexbe_onboard/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_onboard ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.3.0 (2020-11-19) +------------------ * Merge remote-tracking branch 'origin/feature/core_rework' into develop # Conflicts: # flexbe_core/src/flexbe_core/core/operatable_state_machine.py diff --git a/flexbe_onboard/package.xml b/flexbe_onboard/package.xml index 346647c5..5a3ba2d1 100644 --- a/flexbe_onboard/package.xml +++ b/flexbe_onboard/package.xml @@ -1,6 +1,6 @@ flexbe_onboard - 1.2.5 + 1.3.0 flexbe_onboard implements the robot-side of the behavior engine from where all behaviors are started. diff --git a/flexbe_states/CHANGELOG.rst b/flexbe_states/CHANGELOG.rst index 4f61de20..4e72fd1e 100644 --- a/flexbe_states/CHANGELOG.rst +++ b/flexbe_states/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_states ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.3.0 (2020-11-19) +------------------ * Merge remote-tracking branch 'origin/feature/core_rework' into develop # Conflicts: # flexbe_core/src/flexbe_core/core/operatable_state_machine.py diff --git a/flexbe_states/package.xml b/flexbe_states/package.xml index 8dac183b..1b889133 100644 --- a/flexbe_states/package.xml +++ b/flexbe_states/package.xml @@ -1,6 +1,6 @@ flexbe_states - 1.2.5 + 1.3.0 flexbe_states provides a collection of predefined states. Feel free to add new states. diff --git a/flexbe_testing/CHANGELOG.rst b/flexbe_testing/CHANGELOG.rst index 7bc61bd5..5ecc36f2 100644 --- a/flexbe_testing/CHANGELOG.rst +++ b/flexbe_testing/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.3.0 (2020-11-19) +------------------ * Merge pull request `#132 `_ from LoyVanBeek/feature/test_require_launch_file_success Optionally fail test if launch-file fails * Clear up logging for exiting processes diff --git a/flexbe_testing/package.xml b/flexbe_testing/package.xml index b5f587ac..b7980475 100644 --- a/flexbe_testing/package.xml +++ b/flexbe_testing/package.xml @@ -1,6 +1,6 @@ flexbe_testing - 1.2.5 + 1.3.0 flexbe_testing provides a framework for unit testing states. diff --git a/flexbe_widget/CHANGELOG.rst b/flexbe_widget/CHANGELOG.rst index 53f0e803..5cd4e0c8 100644 --- a/flexbe_widget/CHANGELOG.rst +++ b/flexbe_widget/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_widget ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.3.0 (2020-11-19) +------------------ * [flexbe_core] [flexbe_widget] Correctly handle non-existing behaviors in action goals (fix `#133 `_) * [flexbe_core] [flexbe_testing] [flexbe_widget] Use yaml backwards compatible diff --git a/flexbe_widget/package.xml b/flexbe_widget/package.xml index d9191556..5d0451cd 100644 --- a/flexbe_widget/package.xml +++ b/flexbe_widget/package.xml @@ -1,6 +1,6 @@ flexbe_widget - 1.2.5 + 1.3.0 flexbe_widget implements some smaller scripts for the behavior engine. From 2ededb384c68507a964cb0d1777c6034d43282a8 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Wed, 9 Dec 2020 16:44:03 +0100 Subject: [PATCH 49/57] [flexbe_core] Replace set conversion for python3 compatibility (see #136) --- flexbe_core/src/flexbe_core/core/state.py | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/flexbe_core/src/flexbe_core/core/state.py b/flexbe_core/src/flexbe_core/core/state.py index 425fc249..77b4882d 100644 --- a/flexbe_core/src/flexbe_core/core/state.py +++ b/flexbe_core/src/flexbe_core/core/state.py @@ -2,13 +2,21 @@ from flexbe_core.core.exceptions import StateError +def _remove_duplicates(input_list): + output_list = list() + for entry in input_list: + if entry not in output_list: + output_list.append(entry) + return output_list + + class State(object): def __init__(self, *args, **kwargs): - self._outcomes = list(set(kwargs.get('outcomes', []))) + self._outcomes = _remove_duplicates(kwargs.get('outcomes', [])) io_keys = kwargs.get('io_keys', []) - self._input_keys = list(set(kwargs.get('input_keys', []) + io_keys)) - self._output_keys = list(set(kwargs.get('output_keys', []) + io_keys)) + self._input_keys = _remove_duplicates(kwargs.get('input_keys', []) + io_keys) + self._output_keys = _remove_duplicates(kwargs.get('output_keys', []) + io_keys) # properties of instances of a state machine self._name = None self._parent = None From 3b10b4b61d70492db86ea4e8f921c26c2f0b0070 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Wed, 9 Dec 2020 16:44:38 +0100 Subject: [PATCH 50/57] [flexbe_onboard] Print stack trace on behavior import errors --- flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py b/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py index cbd9f71c..d069dbd6 100644 --- a/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py +++ b/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py @@ -152,7 +152,7 @@ def _behavior_execution(self, msg): rospy.logerr('Failed to clean up behavior:\n%s' % str(e)) if not self._switching: - rospy.loginfo('Behavior execution finished with result %s.', str(result)) + Logger.loginfo('Behavior execution finished with result %s.', str(result)) rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m') self._running = False self.be = None @@ -224,7 +224,10 @@ def _prepare_behavior(self, msg): be = beclass() rospy.loginfo('Behavior ' + be.name + ' created.') except Exception as e: - Logger.logerr('Exception caught in behavior definition:\n%s' % str(e)) + Logger.logerr('Exception caught in behavior definition:\n%s\n' + 'See onboard terminal for more information.' % str(e)) + import traceback + traceback.print_exc() self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) return From 299c2e4af5e2b3213f21d161272627ffa97a52b4 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Wed, 9 Dec 2020 16:45:25 +0100 Subject: [PATCH 51/57] [flexbe_states] Pass flexible calculation state input as kwargs for python3 compatibility --- flexbe_states/src/flexbe_states/flexible_calculation_state.py | 2 +- flexbe_states/tests/flexible_calculation_state_simple.test | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/flexbe_states/src/flexbe_states/flexible_calculation_state.py b/flexbe_states/src/flexbe_states/flexible_calculation_state.py index aa33d1a1..69049291 100644 --- a/flexbe_states/src/flexbe_states/flexible_calculation_state.py +++ b/flexbe_states/src/flexbe_states/flexible_calculation_state.py @@ -35,7 +35,7 @@ def execute(self, userdata): def on_enter(self, userdata): if self._calculation is not None: try: - self._calculation_result = self._calculation([userdata[key] for key in self._input_keys]) + self._calculation_result = self._calculation(**{key: userdata[key] for key in self._input_keys}) except Exception as e: Logger.logwarn('Failed to execute calculation function!\n%s' % str(e)) else: diff --git a/flexbe_states/tests/flexible_calculation_state_simple.test b/flexbe_states/tests/flexible_calculation_state_simple.test index 3635db26..40b787c8 100644 --- a/flexbe_states/tests/flexible_calculation_state_simple.test +++ b/flexbe_states/tests/flexible_calculation_state_simple.test @@ -2,7 +2,7 @@ path: flexbe_states.flexible_calculation_state class: FlexibleCalculationState params: - calculation: "lambda x: x[0] * x[1]" + calculation: "lambda b, a: a - b" input_keys: ['a', 'b'] input: @@ -10,6 +10,6 @@ input: b: 4 output: - output_value: 12 + output_value: -1 outcome: done From 04f78c920ecfd98fb3fafedbe89b34163aff2e17 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Wed, 9 Dec 2020 17:21:54 +0100 Subject: [PATCH 52/57] [flexbe_onboard] Offer option to enable clearing of imports (see #135) --- flexbe_onboard/launch/behavior_onboard.launch | 5 +++++ .../src/flexbe_onboard/flexbe_onboard.py | 17 +++++++++++++---- 2 files changed, 18 insertions(+), 4 deletions(-) diff --git a/flexbe_onboard/launch/behavior_onboard.launch b/flexbe_onboard/launch/behavior_onboard.launch index b81fd803..3a3b8765 100644 --- a/flexbe_onboard/launch/behavior_onboard.launch +++ b/flexbe_onboard/launch/behavior_onboard.launch @@ -6,11 +6,16 @@ + + + + diff --git a/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py b/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py index d069dbd6..44625fc3 100644 --- a/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py +++ b/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py @@ -45,6 +45,7 @@ def __init__(self): self._execute_heartbeat() # listen for new behavior to start + self._enable_clear_imports = rospy.get_param('~enable_clear_imports', False) self._running = False self._run_lock = threading.Lock() self._switching = False @@ -144,9 +145,10 @@ def _behavior_execution(self, msg): # done, remove left-overs like the temporary behavior file try: - # hotfix: do not clear imports for now, not working correctly (e.g., flexbe/flexbe_app#66) - # if not self._switching: - # self._clear_imports() + # do not clear imports for now, not working correctly (e.g., flexbe/flexbe_app#66) + # only if specifically enabled + if not self._switching and self._enable_clear_imports: + self._clear_imports() self._cleanup_behavior(msg.behavior_checksum) except Exception as e: rospy.logerr('Failed to clean up behavior:\n%s' % str(e)) @@ -229,6 +231,8 @@ def _prepare_behavior(self, msg): import traceback traceback.print_exc() self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) + if self._enable_clear_imports: + self._clear_imports() return # initialize behavior parameters @@ -259,8 +263,13 @@ def _prepare_behavior(self, msg): be.prepare_for_execution(self._convert_input_data(msg.input_keys, msg.input_values)) rospy.loginfo('State machine built.') except Exception as e: - Logger.logerr('Behavior construction failed!\n%s' % str(e)) + Logger.logerr('Behavior construction failed!\n%s\n' + 'See onboard terminal for more information.' % str(e)) + import traceback + traceback.print_exc() self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) + if self._enable_clear_imports: + self._clear_imports() return return be From 0d1bd532c756edd96bb3078eb4161b42f40cd3b9 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Wed, 9 Dec 2020 22:12:18 +0100 Subject: [PATCH 53/57] Add github actions file from flexbe_ci --- .github/workflows/flexbe_ci.yml | 50 +++++++++++++++++++++++++++++++++ 1 file changed, 50 insertions(+) create mode 100644 .github/workflows/flexbe_ci.yml diff --git a/.github/workflows/flexbe_ci.yml b/.github/workflows/flexbe_ci.yml new file mode 100644 index 00000000..9b516edf --- /dev/null +++ b/.github/workflows/flexbe_ci.yml @@ -0,0 +1,50 @@ +# This is a basic workflow to help you get started with Actions +name: FlexBE CI +# Controls when the action will run. +on: + # Triggers the workflow on push or pull request events but only for the master branch + push: + # Allows you to run this workflow manually from the Actions tab + workflow_dispatch: + +# A workflow run is made up of one or more jobs that can run sequentially or in parallel +jobs: + test: + strategy: + matrix: + ros: [kinetic, melodic, noetic] + include: + - os: ubuntu-16.04 + ros: kinetic + - os: ubuntu-18.04 + ros: melodic + - os: ubuntu-20.04 + ros: noetic + python: python3 + + runs-on: ${{ matrix.os }} + env: + ROS_DISTRO: ${{ matrix.ros }} + PYTHON: ${{ matrix.python }} + + # Steps represent a sequence of tasks that will be executed as part of the job + steps: + # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it + - uses: actions/checkout@v2 + - name: Prepare CI + run: | + git clone https://github.com/FlexBE/flexbe_ci.git ~/flexbe_ci + source ~/flexbe_ci/setup.bash $ROS_DISTRO + + - name: Install ROS + run: ~/flexbe_ci/ci_scripts/install_ros.bash + - name: Setup Workspace + run: ~/flexbe_ci/ci_scripts/setup_workspace.bash + - name: Clone FlexBE Repos + run: ~/flexbe_ci/ci_scripts/run_rosinstall.bash + - name: Test Create Repo + run: ~/flexbe_ci/ci_scripts/test_create_repo.bash + - name: Test Devel Workspace + run: ~/flexbe_ci/ci_scripts/run_devel_tests.bash + - name: Test Install Workspace + run: ~/flexbe_ci/ci_scripts/run_install_tests.bash From 4c12c62a0cc6178e7882805627eed5fc3f4bc51a Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Fri, 11 Dec 2020 14:12:47 +0100 Subject: [PATCH 54/57] [flexbe_onboard] Fix assertion in onboard test --- flexbe_onboard/test/test_onboard.py | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/flexbe_onboard/test/test_onboard.py b/flexbe_onboard/test/test_onboard.py index db4b8b56..18d0aa81 100755 --- a/flexbe_onboard/test/test_onboard.py +++ b/flexbe_onboard/test/test_onboard.py @@ -59,10 +59,14 @@ def test_onboard_behaviors(self): # send valid simple behavior request with open(self.lib.get_sourcecode_filepath(be_id)) as f: request.behavior_checksum = zlib.adler32(f.read().encode()) & 0x7fffffff + self.sub.enable_buffer('flexbe/log') behavior_pub.publish(request) self.assertStatus(BEStatus.STARTED, 1) self.assertStatus(BEStatus.FINISHED, 3) - self.assertIn('data', self.sub.get_last_msg('flexbe/log').text) + behavior_logs = [] + while self.sub.has_buffered('flexbe/log'): + behavior_logs.append(self.sub.get_from_buffer('flexbe/log').text) + self.assertIn('Test data', behavior_logs) # send valid complex behavior request be_id, _ = self.lib.find_behavior("Test Behavior Complex") @@ -86,7 +90,10 @@ def test_onboard_behaviors(self): self.assertStatus(BEStatus.STARTED, 1) result = self.assertStatus(BEStatus.FINISHED, 3) self.assertEqual(result.args[0], 'finished') - self.assertIn('value_2', self.sub.get_last_msg('flexbe/log').text) + behavior_logs = [] + while self.sub.has_buffered('flexbe/log'): + behavior_logs.append(self.sub.get_from_buffer('flexbe/log').text) + self.assertIn('value_2', behavior_logs) # send the same behavior with different parameters request.arg_keys = ['param', 'invalid'] @@ -97,7 +104,10 @@ def test_onboard_behaviors(self): self.assertStatus(BEStatus.STARTED, 1) result = self.assertStatus(BEStatus.FINISHED, 3) self.assertEqual(result.args[0], 'failed') - self.assertIn('value_1', self.sub.get_last_msg('flexbe/log').text) + behavior_logs = [] + while self.sub.has_buffered('flexbe/log'): + behavior_logs.append(self.sub.get_from_buffer('flexbe/log').text) + self.assertIn('value_1', behavior_logs) if __name__ == '__main__': From f88d359739e1beba08b75bc5c08bd1ddd05c8ba1 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Fri, 11 Dec 2020 14:18:46 +0100 Subject: [PATCH 55/57] Remove travis and add status badge --- .travis.yml | 17 ----------------- README.md | 2 ++ 2 files changed, 2 insertions(+), 17 deletions(-) delete mode 100644 .travis.yml diff --git a/.travis.yml b/.travis.yml deleted file mode 100644 index 52fd8cfd..00000000 --- a/.travis.yml +++ /dev/null @@ -1,17 +0,0 @@ -sudo: required -dist: xenial -language: generic - -before_install: - - git clone https://github.com/FlexBE/flexbe_ci.git ~/flexbe_ci - - source ~/flexbe_ci/setup.bash - - ~/flexbe_ci/ci_scripts/before_install.bash - -install: - - ~/flexbe_ci/ci_scripts/install.bash - -before_script: - - ~/flexbe_ci/ci_scripts/before_script.bash - -script: - - ~/flexbe_ci/ci_scripts/script.bash diff --git a/README.md b/README.md index d091a4b6..c90b8128 100644 --- a/README.md +++ b/README.md @@ -8,6 +8,8 @@ The user interface features a runtime control interface as well as a graphical e Please refer to the FlexBE Homepage ([flexbe.github.io](http://flexbe.github.io)) for further information, tutorials, application examples, and much more. +![FlexBE CI](https://github.com/team-vigir/flexbe_behavior_engine/workflows/FlexBE%20CI/badge.svg) + ## Installation Execute the following commands to install FlexBE: From a06842a5ba9a46ed00c169f9bb17786c75c09da8 Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Fri, 11 Dec 2020 14:46:14 +0100 Subject: [PATCH 56/57] Update changelog --- flexbe_behavior_engine/CHANGELOG.rst | 3 +++ flexbe_core/CHANGELOG.rst | 6 ++++++ flexbe_input/CHANGELOG.rst | 3 +++ flexbe_mirror/CHANGELOG.rst | 3 +++ flexbe_msgs/CHANGELOG.rst | 3 +++ flexbe_onboard/CHANGELOG.rst | 8 ++++++++ flexbe_states/CHANGELOG.rst | 5 +++++ flexbe_testing/CHANGELOG.rst | 3 +++ flexbe_widget/CHANGELOG.rst | 3 +++ 9 files changed, 37 insertions(+) diff --git a/flexbe_behavior_engine/CHANGELOG.rst b/flexbe_behavior_engine/CHANGELOG.rst index 7beace0a..3e539345 100644 --- a/flexbe_behavior_engine/CHANGELOG.rst +++ b/flexbe_behavior_engine/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package flexbe_behavior_engine ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.3.0 (2020-11-19) ------------------ diff --git a/flexbe_core/CHANGELOG.rst b/flexbe_core/CHANGELOG.rst index de06d6ac..c94caee2 100644 --- a/flexbe_core/CHANGELOG.rst +++ b/flexbe_core/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package flexbe_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [flexbe_core] Replace set conversion for python3 compatibility + (see `#136 `_) +* Contributors: Philipp Schillinger + 1.3.0 (2020-11-19) ------------------ * [flexbe_core] Further fixes to userdata diff --git a/flexbe_input/CHANGELOG.rst b/flexbe_input/CHANGELOG.rst index fe340999..6cc22cb7 100644 --- a/flexbe_input/CHANGELOG.rst +++ b/flexbe_input/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package flexbe_input ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.3.0 (2020-11-19) ------------------ * Merge remote-tracking branch 'origin/feature/core_rework' into develop diff --git a/flexbe_mirror/CHANGELOG.rst b/flexbe_mirror/CHANGELOG.rst index 4a73efa3..cb21956f 100644 --- a/flexbe_mirror/CHANGELOG.rst +++ b/flexbe_mirror/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package flexbe_mirror ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.3.0 (2020-11-19) ------------------ * Merge remote-tracking branch 'origin/feature/core_rework' into develop diff --git a/flexbe_msgs/CHANGELOG.rst b/flexbe_msgs/CHANGELOG.rst index 8a057efa..23339110 100644 --- a/flexbe_msgs/CHANGELOG.rst +++ b/flexbe_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package flexbe_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.3.0 (2020-11-19) ------------------ * Merge remote-tracking branch 'origin/feature/core_rework' into develop diff --git a/flexbe_onboard/CHANGELOG.rst b/flexbe_onboard/CHANGELOG.rst index 5815e38a..5c74ce89 100644 --- a/flexbe_onboard/CHANGELOG.rst +++ b/flexbe_onboard/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package flexbe_onboard ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [flexbe_onboard] Fix assertion in onboard test +* [flexbe_onboard] Offer option to enable clearing of imports + (see `#135 `_) +* [flexbe_onboard] Print stack trace on behavior import errors +* Contributors: Philipp Schillinger + 1.3.0 (2020-11-19) ------------------ * Merge remote-tracking branch 'origin/feature/core_rework' into develop diff --git a/flexbe_states/CHANGELOG.rst b/flexbe_states/CHANGELOG.rst index 4e72fd1e..4555f21d 100644 --- a/flexbe_states/CHANGELOG.rst +++ b/flexbe_states/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package flexbe_states ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [flexbe_states] Pass flexible calculation state input as kwargs for python3 compatibility +* Contributors: Philipp Schillinger + 1.3.0 (2020-11-19) ------------------ * Merge remote-tracking branch 'origin/feature/core_rework' into develop diff --git a/flexbe_testing/CHANGELOG.rst b/flexbe_testing/CHANGELOG.rst index 5ecc36f2..6d2ce280 100644 --- a/flexbe_testing/CHANGELOG.rst +++ b/flexbe_testing/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package flexbe_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.3.0 (2020-11-19) ------------------ * Merge pull request `#132 `_ from LoyVanBeek/feature/test_require_launch_file_success diff --git a/flexbe_widget/CHANGELOG.rst b/flexbe_widget/CHANGELOG.rst index 5cd4e0c8..8d0b7448 100644 --- a/flexbe_widget/CHANGELOG.rst +++ b/flexbe_widget/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package flexbe_widget ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.3.0 (2020-11-19) ------------------ * [flexbe_core] [flexbe_widget] Correctly handle non-existing behaviors in action goals From 8e2ca45fbdb68818599944d2d155f9367b607c5c Mon Sep 17 00:00:00 2001 From: Philipp Schillinger Date: Fri, 11 Dec 2020 14:46:30 +0100 Subject: [PATCH 57/57] 1.3.1 --- flexbe_behavior_engine/CHANGELOG.rst | 4 ++-- flexbe_behavior_engine/package.xml | 2 +- flexbe_core/CHANGELOG.rst | 4 ++-- flexbe_core/package.xml | 2 +- flexbe_input/CHANGELOG.rst | 4 ++-- flexbe_input/package.xml | 2 +- flexbe_mirror/CHANGELOG.rst | 4 ++-- flexbe_mirror/package.xml | 2 +- flexbe_msgs/CHANGELOG.rst | 4 ++-- flexbe_msgs/package.xml | 2 +- flexbe_onboard/CHANGELOG.rst | 4 ++-- flexbe_onboard/package.xml | 2 +- flexbe_states/CHANGELOG.rst | 4 ++-- flexbe_states/package.xml | 2 +- flexbe_testing/CHANGELOG.rst | 4 ++-- flexbe_testing/package.xml | 2 +- flexbe_widget/CHANGELOG.rst | 4 ++-- flexbe_widget/package.xml | 2 +- 18 files changed, 27 insertions(+), 27 deletions(-) diff --git a/flexbe_behavior_engine/CHANGELOG.rst b/flexbe_behavior_engine/CHANGELOG.rst index 3e539345..a41ddd5a 100644 --- a/flexbe_behavior_engine/CHANGELOG.rst +++ b/flexbe_behavior_engine/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_behavior_engine ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.3.1 (2020-12-11) +------------------ 1.3.0 (2020-11-19) ------------------ diff --git a/flexbe_behavior_engine/package.xml b/flexbe_behavior_engine/package.xml index a59f28aa..c51480dd 100644 --- a/flexbe_behavior_engine/package.xml +++ b/flexbe_behavior_engine/package.xml @@ -1,7 +1,7 @@ flexbe_behavior_engine - 1.3.0 + 1.3.1 A meta-package to aggregate all the FlexBE packages diff --git a/flexbe_core/CHANGELOG.rst b/flexbe_core/CHANGELOG.rst index c94caee2..df191a21 100644 --- a/flexbe_core/CHANGELOG.rst +++ b/flexbe_core/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.3.1 (2020-12-11) +------------------ * [flexbe_core] Replace set conversion for python3 compatibility (see `#136 `_) * Contributors: Philipp Schillinger diff --git a/flexbe_core/package.xml b/flexbe_core/package.xml index 5f50c5ba..b301b864 100644 --- a/flexbe_core/package.xml +++ b/flexbe_core/package.xml @@ -1,6 +1,6 @@ flexbe_core - 1.3.0 + 1.3.1 flexbe_core provides the core components for the FlexBE behavior engine. diff --git a/flexbe_input/CHANGELOG.rst b/flexbe_input/CHANGELOG.rst index 6cc22cb7..fadad6d3 100644 --- a/flexbe_input/CHANGELOG.rst +++ b/flexbe_input/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_input ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.3.1 (2020-12-11) +------------------ 1.3.0 (2020-11-19) ------------------ diff --git a/flexbe_input/package.xml b/flexbe_input/package.xml index 464e7176..6808e107 100644 --- a/flexbe_input/package.xml +++ b/flexbe_input/package.xml @@ -1,6 +1,6 @@ flexbe_input - 1.3.0 + 1.3.1 flexbe_input enables to send data to onboard behavior when required. diff --git a/flexbe_mirror/CHANGELOG.rst b/flexbe_mirror/CHANGELOG.rst index cb21956f..15bca17a 100644 --- a/flexbe_mirror/CHANGELOG.rst +++ b/flexbe_mirror/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_mirror ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.3.1 (2020-12-11) +------------------ 1.3.0 (2020-11-19) ------------------ diff --git a/flexbe_mirror/package.xml b/flexbe_mirror/package.xml index 880149ef..76944d47 100644 --- a/flexbe_mirror/package.xml +++ b/flexbe_mirror/package.xml @@ -1,6 +1,6 @@ flexbe_mirror - 1.3.0 + 1.3.1 flexbe_mirror implements functionality to remotely mirror an executed behavior. diff --git a/flexbe_msgs/CHANGELOG.rst b/flexbe_msgs/CHANGELOG.rst index 23339110..bdb0eaf7 100644 --- a/flexbe_msgs/CHANGELOG.rst +++ b/flexbe_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.3.1 (2020-12-11) +------------------ 1.3.0 (2020-11-19) ------------------ diff --git a/flexbe_msgs/package.xml b/flexbe_msgs/package.xml index df327906..099bf125 100644 --- a/flexbe_msgs/package.xml +++ b/flexbe_msgs/package.xml @@ -1,6 +1,6 @@ flexbe_msgs - 1.3.0 + 1.3.1 flexbe_msgs provides the messages used by FlexBE. diff --git a/flexbe_onboard/CHANGELOG.rst b/flexbe_onboard/CHANGELOG.rst index 5c74ce89..615819d3 100644 --- a/flexbe_onboard/CHANGELOG.rst +++ b/flexbe_onboard/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_onboard ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.3.1 (2020-12-11) +------------------ * [flexbe_onboard] Fix assertion in onboard test * [flexbe_onboard] Offer option to enable clearing of imports (see `#135 `_) diff --git a/flexbe_onboard/package.xml b/flexbe_onboard/package.xml index 5a3ba2d1..ae3a3b11 100644 --- a/flexbe_onboard/package.xml +++ b/flexbe_onboard/package.xml @@ -1,6 +1,6 @@ flexbe_onboard - 1.3.0 + 1.3.1 flexbe_onboard implements the robot-side of the behavior engine from where all behaviors are started. diff --git a/flexbe_states/CHANGELOG.rst b/flexbe_states/CHANGELOG.rst index 4555f21d..d7737353 100644 --- a/flexbe_states/CHANGELOG.rst +++ b/flexbe_states/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_states ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.3.1 (2020-12-11) +------------------ * [flexbe_states] Pass flexible calculation state input as kwargs for python3 compatibility * Contributors: Philipp Schillinger diff --git a/flexbe_states/package.xml b/flexbe_states/package.xml index 1b889133..40c949f0 100644 --- a/flexbe_states/package.xml +++ b/flexbe_states/package.xml @@ -1,6 +1,6 @@ flexbe_states - 1.3.0 + 1.3.1 flexbe_states provides a collection of predefined states. Feel free to add new states. diff --git a/flexbe_testing/CHANGELOG.rst b/flexbe_testing/CHANGELOG.rst index 6d2ce280..f1b43e80 100644 --- a/flexbe_testing/CHANGELOG.rst +++ b/flexbe_testing/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.3.1 (2020-12-11) +------------------ 1.3.0 (2020-11-19) ------------------ diff --git a/flexbe_testing/package.xml b/flexbe_testing/package.xml index b7980475..efa644ca 100644 --- a/flexbe_testing/package.xml +++ b/flexbe_testing/package.xml @@ -1,6 +1,6 @@ flexbe_testing - 1.3.0 + 1.3.1 flexbe_testing provides a framework for unit testing states. diff --git a/flexbe_widget/CHANGELOG.rst b/flexbe_widget/CHANGELOG.rst index 8d0b7448..dbe7ac85 100644 --- a/flexbe_widget/CHANGELOG.rst +++ b/flexbe_widget/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package flexbe_widget ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.3.1 (2020-12-11) +------------------ 1.3.0 (2020-11-19) ------------------ diff --git a/flexbe_widget/package.xml b/flexbe_widget/package.xml index 5d0451cd..4f7032cb 100644 --- a/flexbe_widget/package.xml +++ b/flexbe_widget/package.xml @@ -1,6 +1,6 @@ flexbe_widget - 1.3.0 + 1.3.1 flexbe_widget implements some smaller scripts for the behavior engine.