diff --git a/flexbe_core/src/flexbe_core/core/event_state.py b/flexbe_core/src/flexbe_core/core/event_state.py index 816feab3..08e4cd42 100644 --- a/flexbe_core/src/flexbe_core/core/event_state.py +++ b/flexbe_core/src/flexbe_core/core/event_state.py @@ -5,7 +5,7 @@ from flexbe_core.core.priority_container import PriorityContainer from flexbe_msgs.msg import CommandFeedback -from std_msgs.msg import Bool, Empty +from std_msgs.msg import Bool, Empty, String from flexbe_core.core.operatable_state import OperatableState @@ -24,6 +24,8 @@ def __init__(self, *args, **kwargs): super(EventState, self).__init__(*args, **kwargs) self.__execute = self.execute self.execute = self._event_execute + self.__on_enter = self.on_enter + self.on_enter = self._event_on_enter self._entering = True self._skipped = False @@ -33,6 +35,14 @@ def __init__(self, *args, **kwargs): self._feedback_topic = 'flexbe/command_feedback' self._repeat_topic = 'flexbe/command/repeat' self._pause_topic = 'flexbe/command/pause' + self._update_topic = 'flexbe/behavior_update' + + self._pub.createPublisher(self._update_topic, String) + + def _event_on_enter(self, *args, **kwargs): + Logger.loginfo("Enter state " + self.path) + self._pub.publish(self._update_topic, String(self.path)) + self.__on_enter(*args, **kwargs) def _event_execute(self, *args, **kwargs): if self._is_controlled and self._sub.has_msg(self._pause_topic): @@ -124,12 +134,6 @@ def on_resume(self, userdata): """ 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).