class PreemptableState(LoopbackState):
"""
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()
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
rospy.loginfo("--> Behavior will be preempted")
if PreemptableState.preempt:
PreemptableState.preempt = False
preempting = True
rospy.loginfo("Behavior will be preempted")
if preempting:
self.service_preempt()
self._force_transition = True
return self._preempted_name
return self.__execute(*args, **kwargs)
def _enable_ros_control(self):
super(PreemptableState, self)._enable_ros_control()
self._pub.createPublisher(self._feedback_topic, CommandFeedback)
self._sub.subscribe(self._preempt_topic, Empty)
PreemptableState.preempt = False
def _disable_ros_control(self):
super(PreemptableState, self)._disable_ros_control()
self._sub.unsubscribe_topic(self._preempt_topic)
class WaitForNaoQiSpeechState(EventState):
"""
Implements a state that waits for a NaoQI-Speech-Command.
-- strings_to_rec string[] List of speech commands to listen for
-- outcomes string[] List of outcomes corresponding to the speech commands
<= done Indicates completion.
"""
recognized = None
_outcomes = None
def __init__(self,
strings_to_rec,
outcomes,
topic='/pepper_robot/speechrec/context'):
"""
Constructor
"""
super(WaitForNaoQiSpeechState, self).__init__(outcomes=outcomes)
self._topic = topic
self.outcomes = outcomes
self._target_strings = strings_to_rec
self._sub = ProxySubscriberCached()
self.recognized = None
def _speech_callback(self, msg):
Logger.loginfo('speechrec callback:%s' % msg.data.lower())
for result in self._target_strings:
Logger.loginfo('\tspeechrec check:%s' % result.lower())
if msg.data.lower() == result.lower():
Logger.loginfo('\t\tspeechrec MATCH!')
self.recognized = result
def execute(self, userdata):
"""Execute this state"""
if self.recognized is not None:
try:
index = self._target_strings.index(self.recognized)
return self.outcomes[index]
except Exception as e:
Logger.loginfo(str(e))
def on_enter(self, userdata):
self.recognized = None
self._sub.subscribe(self._topic, String, self._speech_callback)
def on_stop(self):
self._sub.unsubscribe_topic(self._topic)
def on_exit(self, userdata):
self._sub.unsubscribe_topic(self._topic)
class ManuallyTransitionableState(MonitoringState):
"""
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._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]))
if command_msg.target != self.name:
rospy.logwarn("--> Requested outcome for state " +
command_msg.target + " but active state is " +
self.name)
else:
self._force_transition = True
outcome = self._outcome_list[command_msg.outcome]
rospy.loginfo("--> Manually triggered outcome " + outcome +
" of state " + self.name)
return outcome
# 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)
self._sub.subscribe(self._transition_topic, OutcomeRequest)
self._sub.enable_buffer(self._transition_topic)
def _disable_ros_control(self):
super(ManuallyTransitionableState, self)._disable_ros_control()
self._sub.unsubscribe_topic(self._transition_topic)
class WaitForRosNavgoalState(EventState):
"""
Implements a state that waits for a Navgoal on a RosTopic.
<# topic string The ROS topic to listen on
#> navgoal PoseStamped The navgoal.
<= done Indicates completion.
"""
def __init__(self, topic='/hololens/navgoal'):
"""
Constructor
"""
super(WaitForRosNavgoalState, self).__init__(outcomes=['done'],
output_keys=['navgoal'])
self._sub = ProxySubscriberCached()
self._topic = topic
self._navgoal = None
def execute(self, userdata):
"""Execute this state"""
if self._navgoal is not None:
userdata.navgoal = self._navgoal
return 'done'
def _navgoal_callback(self, msg):
# prevent re-using latched message
# (this is probably only happening when using rostopic pub from the console, but whatever)
try:
if self.last_id == msg.header.stamp:
return
except AttributeError:
pass
self.last_id = msg.header.stamp
Logger.loginfo(
'navgoal callback on topic %s:%s' %
(self._topic, str(msg.pose.position).replace('\n', ' ')))
self._navgoal = msg
def on_enter(self, userdata):
self._navgoal = None
self._sub.subscribe(self._topic, PoseStamped, self._navgoal_callback)
def on_stop(self):
self._sub.unsubscribe_topic(self._topic)
def on_exit(self, userdata):
self._sub.unsubscribe_topic(self._topic)
class ManuallyTransitionableState(MonitoringState):
"""
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._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]))
if command_msg.target != self.name:
rospy.logwarn("--> Requested outcome for state " + command_msg.target + " but active state is " + self.name)
else:
self._force_transition = True
outcome = self._outcome_list[command_msg.outcome]
rospy.loginfo("--> Manually triggered outcome " + outcome + " of state " + self.name)
return outcome
# 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)
self._sub.subscribe(self._transition_topic, OutcomeRequest)
self._sub.enable_buffer(self._transition_topic)
def _disable_ros_control(self):
super(ManuallyTransitionableState, self)._disable_ros_control()
self._sub.unsubscribe_topic(self._transition_topic)
class VigirBeOnboard(object):
'''
Implements an idle state where the robot waits for a behavior to be started.
'''
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
# prepare temp folder
rp = rospkg.RosPack()
self._tmp_folder = "/tmp/flexbe_onboard"
if not os.path.exists(self._tmp_folder):
os.makedirs(self._tmp_folder)
sys.path.append(self._tmp_folder)
# prepare manifest folder access
self._behavior_lib = BehaviorLibrary()
self._pub = ProxyPublisher()
self._sub = ProxySubscriberCached()
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)
# listen for new behavior to start
self._running = False
self._switching = False
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
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()
def _behavior_execution(self, msg):
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...')
be = self._prepare_behavior(msg)
if be is None:
Logger.logerr('Dropped behavior start request because preparation failed.')
if self._running:
self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['failed']))
else:
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
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()
self._switching = False
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"
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
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)
be_filepath = os.path.join(rp.get_path(behavior["package"]), 'src/' + behavior["package"] + '/' + behavior["file"] + '_tmp.py')
if os.path.isfile(be_filepath):
be_file = open(be_filepath, "r")
rospy.logwarn("Found a tmp version of the referred behavior! Assuming local test run.")
else:
be_filepath = os.path.join(rp.get_path(behavior["package"]), 'src/' + behavior["package"] + '/' + behavior["file"] + '.py')
be_file = open(be_filepath, "r")
be_content = be_file.read()
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))
return
# apply modifications if any
try:
file_content = ""
last_index = 0
for mod in msg.modifications:
file_content += be_content[last_index:mod.index_begin] + mod.new_content
last_index = mod.index_end
file_content += be_content[last_index:]
if zlib.adler32(file_content) != 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"
"Also try: rosrun flexbe_widget clear_cache" % str(be_filepath))
raise Exception(mismatch_msg)
else:
rospy.loginfo("Successfully applied %d modifications." % len(msg.modifications))
except Exception as e:
Logger.logerr('Failed to apply behavior modifications:\n%s' % str(e))
self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR))
return
# 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()
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))
return
# 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()
rospy.loginfo('Behavior ' + be.name + ' created.')
except Exception as e:
Logger.logerr('Exception caught in behavior definition:\n%s' % str(e))
self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR))
return
# import contained behaviors
contain_list = {}
try:
contain_list = self._build_contains(be, "")
except Exception as e:
Logger.logerr('Failed to load contained behaviors:\n%s' % str(e))
return
# initialize behavior parameters
if len(msg.arg_keys) > 0:
rospy.loginfo('The following parameters will be used:')
try:
for i in range(len(msg.arg_keys)):
if msg.arg_keys[i] == '':
# action call has empty string as default, not a valid param key
continue
key_splitted = msg.arg_keys[i].rsplit('/', 1)
if len(key_splitted) == 1:
behavior = ''
key = key_splitted[0]
rospy.logwarn('Parameter key %s has no path specification, assuming: /%s' % (key, key))
else:
behavior = key_splitted[0]
key = key_splitted[1]
found = False
if behavior == '' and hasattr(be, key):
self._set_typed_attribute(be, key, msg.arg_values[i])
# propagate to contained behaviors
for b in contain_list:
if hasattr(contain_list[b], key):
self._set_typed_attribute(contain_list[b], key, msg.arg_values[i], b)
found = True
for b in contain_list:
if b == behavior and hasattr(contain_list[b], key):
self._set_typed_attribute(contain_list[b], key, msg.arg_values[i], b)
found = True
if not found:
rospy.logwarn('Parameter ' + msg.arg_keys[i] + ' (set to ' + msg.arg_values[i] + ') not implemented')
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))
return
# build state machine
try:
be.set_up(id=msg.behavior_checksum, autonomy_level=msg.autonomy_level, debug=False)
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
return be
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))
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)
try:
os.remove(file_path)
except OSError:
pass
try:
os.remove(file_path + 'c')
except OSError:
pass
def _set_typed_attribute(self, obj, name, value, behavior=''):
attr = getattr(obj, name)
if type(attr) is int:
value = int(value)
elif type(attr) is long:
value = long(value)
elif type(attr) is float:
value = float(value)
elif type(attr) is bool:
value = (value != "0")
elif type(attr) is dict:
value = yaml.load(value)
setattr(obj, name, value)
suffix = ' (' + behavior + ')' if behavior != '' else ''
rospy.loginfo(name + ' = ' + str(value) + suffix)
def _convert_input_data(self, keys, values):
result = dict()
for k, v in zip(keys, values):
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.logdebug('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 _build_contains(self, obj, path):
contain_list = dict((path+"/"+key,value) for (key,value) in getattr(obj, 'contains', {}).items())
add_to_list = {}
for b_id, b_inst in contain_list.items():
add_to_list.update(self._build_contains(b_inst, b_id))
contain_list.update(add_to_list)
return contain_list
def _execute_heartbeat(self):
thread = threading.Thread(target=self._heartbeat_worker)
thread.daemon = True
thread.start()
def _heartbeat_worker(self):
while True:
self._pub.publish('flexbe/heartbeat', Empty())
time.sleep(1) # sec
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]
elif isinstance(o, dict):
return self._attr_dict((k, self._convert_dict(v)) for k, v in o.items())
else:
return o
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._paused = False
self._last_active_container = None
self._feedback_topic = 'flexbe/command_feedback'
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)
if msg.data:
self._sub.remove_last_msg(self._pause_topic)
rospy.loginfo("--> 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()
self._paused = True
if self._paused and not PreemptableState.preempt:
self._notify_skipped()
return self._loopback_name
if self._entering:
self._entering = False
self.on_enter(*args, **kwargs)
if self._skipped and not PreemptableState.preempt:
self._skipped = False
self.on_resume(*args, **kwargs)
execute_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)
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:
self._entering = True
self.on_exit(*args, **kwargs)
return execute_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)
self._sub.subscribe(self._repeat_topic, Empty)
self._sub.subscribe(self._pause_topic, Bool)
def _disable_ros_control(self):
super(EventState, self)._disable_ros_control()
self._sub.unsubscribe_topic(self._repeat_topic)
self._sub.unsubscribe_topic(self._pause_topic)
self._last_active_container = None
if self._paused:
PriorityContainer.active_container = None
# Events
# (just implement the ones you need)
def on_start(self):
"""
Will be executed once when the behavior starts.
"""
pass
def on_stop(self):
"""
Will be executed once when the behavior stops or is preempted.
"""
pass
def on_pause(self):
"""
Will be executed each time this state is paused.
"""
pass
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
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._paused = False
self._last_active_container = None
self._feedback_topic = "flexbe/command_feedback"
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)
if msg.data:
self._sub.remove_last_msg(self._pause_topic)
rospy.loginfo("--> Pausing in state %s", self.name)
self._pub.publish(self._feedback_topic, CommandFeedback(command="pause"))
self._last_active_container = PriorityContainer.active_container
PriorityContainer.active_container = ""
self._paused = True
if self._paused and not PreemptableState.preempt:
self._notify_skipped()
return self._loopback_name
if self._entering:
self._entering = False
self.on_enter(*args, **kwargs)
if self._skipped:
self._skipped = False
self.on_resume(*args, **kwargs)
execute_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)
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 or repeat:
self._entering = True
self.on_exit(*args, **kwargs)
return execute_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)
self._sub.subscribe(self._repeat_topic, Empty)
self._sub.subscribe(self._pause_topic, Bool)
def _disable_ros_control(self):
super(EventState, self)._disable_ros_control()
self._sub.unsubscribe_topic(self._repeat_topic)
self._sub.unsubscribe_topic(self._pause_topic)
self._last_active_container = None
if self._paused:
PriorityContainer.active_container = None
# Events
# (just implement the ones you need)
def on_start(self):
"""
Will be executed once when the behavior starts.
"""
pass
def on_stop(self):
"""
Will be executed once when the behavior stops or is preempted.
"""
pass
def on_pause(self):
"""
Will be executed each time this state is paused.
"""
pass
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
class VigirBeOnboard(object):
'''
Implements an idle state where the robot waits for a behavior to be started.
'''
def __init__(self):
'''
Constructor
'''
self.be = None
Logger.initialize()
#ProxyPublisher._simulate_delay = True
#ProxySubscriberCached._simulate_delay = True
# prepare temp folder
rp = rospkg.RosPack()
self._tmp_folder = os.path.join(rp.get_path('flexbe_onboard'), 'tmp/')
if not os.path.exists(self._tmp_folder):
os.makedirs(self._tmp_folder)
sys.path.append(self._tmp_folder)
# prepare manifest folder access
manifest_folder = os.path.join(rp.get_path('flexbe_behaviors'), 'behaviors/')
rospy.loginfo("Parsing available behaviors...")
file_entries = [os.path.join(manifest_folder, filename) for filename in os.listdir(manifest_folder) if not filename.startswith('#')]
manifests = sorted([xmlpath for xmlpath in file_entries if not os.path.isdir(xmlpath)])
self._behavior_lib = dict()
for i in range(len(manifests)):
m = ET.parse(manifests[i]).getroot()
e = m.find("executable")
self._behavior_lib[i] = {"name": m.get("name"), "package": e.get("package_path").split(".")[0], "file": e.get("package_path").split(".")[1], "class": e.get("class")}
# rospy.loginfo("+++ " + self._behavior_lib[i]["name"])
self._pub = ProxyPublisher()
self._sub = ProxySubscriberCached()
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)
# listen for new behavior to start
self._running = False
self._switching = False
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
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()
def _behavior_execution(self, msg):
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...')
be = self._prepare_behavior(msg)
if be is None:
Logger.logerr('Dropped behavior start request because preparation failed.')
if self._running:
self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['failed']))
else:
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
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()
self._switching = False
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))
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))
result = "failed"
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
def _prepare_behavior(self, msg):
# get sourcecode from ros package
try:
rp = rospkg.RosPack()
behavior = self._behavior_lib[msg.behavior_id]
be_filepath = os.path.join(rp.get_path(behavior["package"]), 'src/' + behavior["package"] + '/' + behavior["file"] + '_tmp.py')
if os.path.isfile(be_filepath):
be_file = open(be_filepath, "r")
rospy.logwarn("Found a tmp version of the referred behavior! Assuming local test run.")
else:
be_filepath = os.path.join(rp.get_path(behavior["package"]), 'src/' + behavior["package"] + '/' + behavior["file"] + '.py')
be_file = open(be_filepath, "r")
be_content = be_file.read()
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))
return
# apply modifications if any
try:
file_content = ""
last_index = 0
for mod in msg.modifications:
file_content += be_content[last_index:mod.index_begin] + mod.new_content
last_index = mod.index_end
file_content += be_content[last_index:]
if zlib.adler32(file_content) != msg.behavior_checksum:
mismatch_msg = ("checksum mismatch of behavior versions! \n"
"Attempted to load behavior: %s" % str(be_filepath))
raise Exception(mismatch_msg)
else:
rospy.loginfo("Successfully applied %d modifications." % len(msg.modifications))
except Exception as e:
Logger.logerr('Failed to apply behavior modifications:\n%s' % str(e))
self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR))
return
# 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()
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))
return
# 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()
rospy.loginfo('Behavior ' + be.name + ' created.')
except Exception as e:
Logger.logerr('Unable to import temporary behavior file:\n%s' % str(e))
self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR))
return
# import contained behaviors
contain_list = {}
try:
contain_list = self._build_contains(be, "")
except Exception as e:
Logger.logerr('Failed to load contained behaviors:\n%s' % str(e))
return
# initialize behavior parameters
if len(msg.arg_keys) > 0:
rospy.loginfo('The following parameters will be used:')
try:
for i in range(len(msg.arg_keys)):
key_splitted = msg.arg_keys[i].rsplit('/', 1)
behavior = key_splitted[0]
key = key_splitted[1]
found = False
if behavior == '' and hasattr(be, key):
self._set_typed_attribute(be, key, msg.arg_values[i])
found = True
for b in contain_list:
if hasattr(contain_list[b], key):
self._set_typed_attribute(contain_list[b], key, msg.arg_values[i], b)
found = True
if not found:
rospy.logwarn('Parameter ' + msg.arg_keys[i] + ' (set to ' + msg.arg_values[i] + ') not implemented')
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))
return
# build state machine
try:
be.set_up(id=msg.behavior_checksum, autonomy_level=msg.autonomy_level, debug=False)
be.prepare_for_execution()
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
return be
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))
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)
os.remove(file_path)
def _set_typed_attribute(self, obj, name, value, behavior=''):
attr = getattr(obj, name)
if type(attr) is int:
value = int(value)
elif type(attr) is long:
value = long(value)
elif type(attr) is float:
value = float(value)
elif type(attr) is bool:
value = (value != "0")
setattr(obj, name, value)
suffix = ' (' + behavior + ')' if behavior != '' else ''
rospy.loginfo(name + ' = ' + str(value) + suffix)
def _build_contains(self, obj, path):
contain_list = dict((path+"/"+key,value) for (key,value) in getattr(obj, 'contains', {}).items())
add_to_list = {}
for b_id, b_inst in contain_list.items():
add_to_list.update(self._build_contains(b_inst, b_id))
contain_list.update(add_to_list)
return contain_list
def _execute_heartbeat(self):
thread = threading.Thread(target=self._heartbeat_worker)
thread.daemon = True
thread.start()
def _heartbeat_worker(self):
while True:
self._pub.publish('/flexbe/heartbeat', Empty())
time.sleep(1) # sec
class MonitoringState(smach.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.name = None
self._parent = None
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 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
class PreemptableState(LoopbackState):
"""
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
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")
if PreemptableState.preempt:
preempting = True
rospy.loginfo("Behavior will be preempted")
if preempting:
self.service_preempt()
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):
self._sub.remove_last_msg(self._preempt_topic)
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)
self._sub.subscribe(self._preempt_topic, Empty)
PreemptableState.preempt = False
def _disable_ros_control(self):
super(PreemptableState, self)._disable_ros_control()
self._sub.unsubscribe_topic(self._preempt_topic)
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.id = None
self.autonomy = None
self._autonomy = {}
self._ordered_states = []
self._pub = ProxyPublisher()
self._sub = ProxySubscriberCached()
@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
@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
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
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
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()
self._sub.unsubscribe_topic('/flexbe/command/autonomy')
self._sub.unsubscribe_topic('/flexbe/command/sync')
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.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
self._sub.subscribe('/flexbe/command/autonomy', UInt8, self._set_autonomy_level)
self._sub.subscribe('/flexbe/command/sync', Empty, self._sync_callback)
self._sub.subscribe('/flexbe/request_mirror_structure', Int32, self._mirror_structure_callback)
StateLogger.initialize(name)
if OperatableStateMachine.autonomy_level != 255:
self._enable_ros_control()
rospy.sleep(0.5) # no clean way to wait for publisher to be ready...
self._notify_start()
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)
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...")
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)
self._pub.publish('/flexbe/command_feedback', CommandFeedback(command="sync", args=[]))
rospy.loginfo("<-- Sent synchronization message for mirror.")
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 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
def _notify_start(self):
for state in self._ordered_states:
if isinstance(state, LoopbackState):
state.on_start()
if isinstance(state, OperatableStateMachine):
state._notify_start()
def _enable_ros_control(self):
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):
state.on_stop()
state._disable_ros_control()
if isinstance(state, OperatableStateMachine):
state._notify_stop()
def _disable_ros_control(self):
for state in self._ordered_states:
if isinstance(state, LoopbackState):
state._disable_ros_control()
if isinstance(state, OperatableStateMachine):
state._disable_ros_control()
class VigirBehaviorMirror(object):
'''
classdocs
'''
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
self._pub = ProxyPublisher({'flexbe/behavior_update': String,
'flexbe/request_mirror_structure': Int32})
self._running = False
self._stopping = False
self._active_id = 0
self._starting_path = None
self._current_struct = None
self._outcome_topic = 'flexbe/mirror/outcome'
self._struct_buffer = list()
# listen for mirror message
self._sub = ProxySubscriberCached()
self._sub.subscribe(self._outcome_topic, UInt8)
self._sub.enable_buffer(self._outcome_topic)
self._sub.subscribe('flexbe/mirror/structure', ContainerStructure, self._mirror_callback)
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 (ID: %s).' % str(msg.behavior_id))
elif self._active_id != 0 and msg.behavior_id != self._active_id:
rospy.logwarn('ID of received mirror structure (%s) does not match expected ID (%s), will ignore.' % (str(msg.behavior_id), str(self._active_id)))
return
else:
rospy.loginfo('Received a new mirror structure for ID %s' % str(msg.behavior_id))
self._struct_buffer.append(msg)
if self._active_id == msg.behavior_id:
self._struct_buffer = list()
self._mirror_state_machine(msg)
rospy.loginfo('Mirror built.')
self._execute_mirror()
def _status_callback(self, msg):
if msg.code == BEStatus.STARTED:
thread = threading.Thread(target=self._start_mirror, args=[msg])
thread.daemon = True
thread.start()
else:
thread = threading.Thread(target=self._stop_mirror, args=[msg])
thread.daemon = True
thread.start()
def _start_mirror(self, msg):
rate = rospy.Rate(10)
while self._stopping:
rate.sleep()
if self._running:
rospy.logwarn('Tried to start mirror while it is already running, will ignore.')
return
if len(msg.args) > 0:
self._starting_path = "/" + msg.args[0][1:].replace("/", "_mirror/") + "_mirror"
self._active_id = msg.behavior_id
while self._sm is None and len(self._struct_buffer) > 0:
struct = self._struct_buffer[0]
self._struct_buffer = self._struct_buffer[1:]
if struct.behavior_id == self._active_id:
self._mirror_state_machine(struct)
rospy.loginfo('Mirror built.')
else:
rospy.logwarn('Discarded mismatching buffered structure for ID %s' % str(struct.behavior_id))
if self._sm is None:
rospy.logwarn('Missing correct mirror structure, requesting...')
rospy.sleep(0.2) # no clean way 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):
self._stopping = True
if self._sm is not None and self._running:
if msg is not None and msg.code == BEStatus.FINISHED:
rospy.loginfo('Onboard behavior finished successfully.')
self._pub.publish('flexbe/behavior_update', String())
elif msg is not None and msg.code == BEStatus.SWITCHING:
self._starting_path = None
rospy.loginfo('Onboard performing behavior switch.')
elif msg is not None and msg.code == BEStatus.READY:
rospy.loginfo('Onboard engine just started, stopping currently running mirror.')
self._pub.publish('flexbe/behavior_update', String())
elif msg is not None:
rospy.logwarn('Onboard behavior failed!')
self._pub.publish('flexbe/behavior_update', String())
PreemptableState.preempt = True
rate = rospy.Rate(10)
while self._running:
rate.sleep()
else:
rospy.loginfo('No onboard behavior is active.')
self._active_id = 0
self._sm = None
self._current_struct = None
self._sub.remove_last_msg(self._outcome_topic, clear_buffer=True)
if msg is not None and msg.code != BEStatus.SWITCHING:
rospy.loginfo('\033[92m--- Behavior Mirror ready! ---\033[0m')
self._stopping = False
def _sync_callback(self, msg):
if msg.behavior_id == self._active_id:
thread = threading.Thread(target=self._restart_mirror, args=[msg])
thread.daemon = True
thread.start()
else:
rospy.logerr('Cannot synchronize! Different behavior is running onboard, please stop execution!')
thread = threading.Thread(target=self._stop_mirror, args=[None])
thread.daemon = True
thread.start()
def _restart_mirror(self, msg):
rospy.loginfo('Restarting mirror for synchronization...')
self._sub.remove_last_msg(self._outcome_topic, clear_buffer=True)
if self._sm is not None and self._running:
PreemptableState.preempt = True
rate = rospy.Rate(10)
while self._running:
rate.sleep()
self._sm = None
if msg.current_state_checksum in self._state_checksums:
current_state_path = self._state_checksums[msg.current_state_checksum]
self._starting_path = "/" + current_state_path[1:].replace("/", "_mirror/") + "_mirror"
rospy.loginfo('Current state: %s' % current_state_path)
try:
self._mirror_state_machine(self._current_struct)
rospy.loginfo('Mirror built.')
self._execute_mirror()
except (AttributeError, smach.InvalidStateError):
rospy.loginfo('Stopping synchronization because behavior has stopped.')
def _execute_mirror(self):
self._running = True
rospy.loginfo("Executing mirror...")
if self._starting_path is not None:
LockableStateMachine.path_for_switch = self._starting_path
rospy.loginfo("Starting mirror in state " + self._starting_path)
self._starting_path = None
result = 'preempted'
try:
result = self._sm.execute()
except Exception as e:
rospy.loginfo('Catched exception on preempt:\n%s' % str(e))
if self._sm is not None:
self._sm.destroy()
self._running = False
rospy.loginfo('Mirror finished with result ' + result)
def _mirror_state_machine(self, msg):
self._current_struct = msg
self._state_checksums = dict()
root = None
for con_msg in msg.containers:
if con_msg.path.find('/') == -1:
root = con_msg.path
break
self._add_node(msg, root)
# 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
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')
sm = JumpableStateMachine(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)
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!')
def _preempt_callback(self, msg):
if self._sm is not None and self._sm.is_running():
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!')
class FlexbeOnboard(object):
"""
Controls the execution of robot behaviors.
"""
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)
# prepare temp folder
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()
# prepare communication
self.status_topic = 'flexbe/status'
self.feedback_topic = 'flexbe/command_feedback'
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)
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.')
if self._running:
self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['failed']))
else:
rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m')
return
# perform the behavior switch if required
with self._switch_lock:
self._switching = True
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
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
# 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))
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:
behavior = self._behavior_lib.get_behavior(msg.behavior_id)
if behavior is None:
raise ValueError(msg.behavior_id)
be_filepath = self._behavior_lib.get_sourcecode_filepath(msg.behavior_id, add_tmp=True)
if os.path.isfile(be_filepath):
be_file = open(be_filepath, "r")
rospy.logwarn("Found a tmp version of the referred behavior! Assuming local test run.")
else:
be_filepath = self._behavior_lib.get_sourcecode_filepath(msg.behavior_id)
be_file = open(be_filepath, "r")
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))
return
# apply modifications if any
try:
file_content = ""
last_index = 0
for mod in msg.modifications:
file_content += be_content[last_index:mod.index_begin] + mod.new_content
last_index = mod.index_end
file_content += be_content[last_index:]
if zlib.adler32(file_content) != 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"
"Also try: rosrun flexbe_widget clear_cache" % str(be_filepath))
raise Exception(mismatch_msg)
else:
rospy.loginfo("Successfully applied %d modifications." % len(msg.modifications))
except Exception as e:
Logger.logerr('Failed to apply behavior modifications:\n%s' % str(e))
self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR))
return
# create temp file for behavior class
try:
file_path = os.path.join(self._tmp_folder, 'tmp_%d.py' % msg.behavior_checksum)
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))
return
# import temp class file and initialize behavior
try:
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))
self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR))
return
# initialize behavior parameters
if len(msg.arg_keys) > 0:
rospy.loginfo('The following parameters will be used:')
try:
for i in range(len(msg.arg_keys)):
# action call has empty string as default, not a valid param key
if msg.arg_keys[i] == '':
continue
found = be.set_parameter(msg.arg_keys[i], msg.arg_values[i])
if found:
name_split = msg.arg_keys[i].rsplit('/', 1)
behavior = name_split[0] if len(name_split) == 2 else ''
key = name_split[-1]
suffix = ' (' + behavior + ')' if behavior != '' else ''
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))
return
# build state machine
try:
be.set_up(id=msg.behavior_checksum, autonomy_level=msg.autonomy_level, debug=False)
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
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))
return False
# locked inside
# locked state exists in new behavior
# ok, can switch
return True
def _cleanup_behavior(self, behavior_checksum):
file_path = os.path.join(self._tmp_folder, 'tmp_%d.pyc' % behavior_checksum)
try:
os.remove(file_path)
except OSError:
pass
try:
os.remove(file_path + 'c')
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)
except OSError:
pass
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)))
result[k] = str(v)
return result
def _execute_heartbeat(self):
thread = threading.Thread(target=self._heartbeat_worker)
thread.daemon = True
thread.start()
def _heartbeat_worker(self):
while True:
self._pub.publish('flexbe/heartbeat', Empty())
time.sleep(1)
def _convert_dict(self, o):
if isinstance(o, list):
return [self._convert_dict(e) for e in o]
elif isinstance(o, dict):
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__
@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)
class MonitoringState(smach.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.name = None
self._parent = None
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 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
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.id = None
self.autonomy = None
self._autonomy = {}
self._ordered_states = []
self._inner_sync_request = False
self._pub = ProxyPublisher()
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._current_state is not None:
try:
self._current_state._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):
"""
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.name = label
state.transitions = transitions
state.autonomy = autonomy
state._parent = self
StateMachine.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
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()
self._sub.unsubscribe_topic('flexbe/command/autonomy')
self._sub.unsubscribe_topic('flexbe/command/sync')
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.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
self._sub.subscribe('flexbe/command/autonomy', UInt8,
self._set_autonomy_level)
self._sub.subscribe('flexbe/command/sync', Empty, self._sync_callback)
self._sub.subscribe('flexbe/command/attach', UInt8,
self._attach_callback)
self._sub.subscribe('flexbe/request_mirror_structure', Int32,
self._mirror_structure_callback)
StateLogger.initialize(name)
if OperatableStateMachine.autonomy_level != 255:
self._enable_ros_control()
rospy.sleep(0.5) # no clean way to wait for publisher to be ready...
self._notify_start()
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)
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...")
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())
self._pub.publish('flexbe/mirror/sync', msg)
self._pub.publish('flexbe/command_feedback',
CommandFeedback(command="sync", args=[]))
rospy.loginfo("<-- Sent synchronization message for mirror.")
def _attach_callback(self, msg):
rospy.loginfo("--> Enabling control...")
# set autonomy level
OperatableStateMachine.autonomy_level = msg.data
# enable control of states
self._enable_ros_control()
self._inner_sync_request = True
# send command feedback
cfb = CommandFeedback(command="attach")
cfb.args.append(self.name)
self._pub.publish('flexbe/command_feedback', cfb)
rospy.loginfo("<-- 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
def _notify_start(self):
for state in self._ordered_states:
if isinstance(state, LoopbackState):
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):
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):
state._disable_ros_control()
def on_exit(self, userdata):
if self._current_state is not None:
self._current_state.on_exit(userdata)
self._current_state = None
class VigirBehaviorMirror(object):
'''
classdocs
'''
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
self._pub = ProxyPublisher({
'flexbe/behavior_update': String,
'flexbe/request_mirror_structure': Int32
})
self._running = False
self._stopping = False
self._active_id = 0
self._starting_path = None
self._current_struct = None
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)
self._sub.enable_buffer(self._outcome_topic)
self._sub.subscribe('flexbe/mirror/structure', ContainerStructure,
self._mirror_callback)
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))
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)))
return
else:
rospy.loginfo('Received a new mirror structure for checksum %s' %
str(msg.behavior_id))
self._struct_buffer.append(msg)
if self._active_id == msg.behavior_id:
self._struct_buffer = list()
self._mirror_state_machine(msg)
rospy.loginfo('Mirror built.')
self._execute_mirror()
def _status_callback(self, msg):
if msg.code == BEStatus.STARTED:
thread = threading.Thread(target=self._start_mirror, args=[msg])
thread.daemon = True
thread.start()
else:
thread = threading.Thread(target=self._stop_mirror, args=[msg])
thread.daemon = True
thread.start()
def _start_mirror(self, msg):
with self._sync_lock:
rate = rospy.Rate(10)
while self._stopping:
rate.sleep()
if self._running:
rospy.logwarn(
'Tried to start mirror while it is already running, will ignore.'
)
return
if len(msg.args) > 0:
self._starting_path = "/" + msg.args[0][1:].replace(
"/", "_mirror/") + "_mirror"
self._active_id = msg.behavior_id
while self._sm is None and len(self._struct_buffer) > 0:
struct = self._struct_buffer[0]
self._struct_buffer = self._struct_buffer[1:]
if struct.behavior_id == self._active_id:
self._mirror_state_machine(struct)
rospy.loginfo('Mirror built.')
else:
rospy.logwarn(
'Discarded mismatching buffered structure for checksum %s'
% str(struct.behavior_id))
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...
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
if self._sm is not None and self._running:
if msg is not None and msg.code == BEStatus.FINISHED:
rospy.loginfo('Onboard behavior finished successfully.')
self._pub.publish('flexbe/behavior_update', String())
elif msg is not None and msg.code == BEStatus.SWITCHING:
self._starting_path = None
rospy.loginfo('Onboard performing behavior switch.')
elif msg is not None and msg.code == BEStatus.READY:
rospy.loginfo(
'Onboard engine just started, stopping currently running mirror.'
)
self._pub.publish('flexbe/behavior_update', String())
elif msg is not None:
rospy.logwarn('Onboard behavior failed!')
self._pub.publish('flexbe/behavior_update', String())
PreemptableState.preempt = True
rate = rospy.Rate(10)
while self._running:
rate.sleep()
else:
rospy.loginfo('No onboard behavior is active.')
self._active_id = 0
self._sm = None
self._current_struct = None
self._sub.remove_last_msg(self._outcome_topic, clear_buffer=True)
if msg is not None and msg.code != BEStatus.SWITCHING:
rospy.loginfo('\033[92m--- Behavior Mirror ready! ---\033[0m')
self._stopping = False
def _sync_callback(self, msg):
if msg.behavior_id == self._active_id:
thread = threading.Thread(target=self._restart_mirror, args=[msg])
thread.daemon = True
thread.start()
else:
rospy.logerr(
'Cannot synchronize! Different behavior is running onboard, please stop execution!'
)
thread = threading.Thread(target=self._stop_mirror, args=[None])
thread.daemon = True
thread.start()
def _restart_mirror(self, msg):
with self._sync_lock:
rospy.loginfo('Restarting mirror for synchronization...')
self._sub.remove_last_msg(self._outcome_topic, clear_buffer=True)
if self._sm is not None and self._running:
PreemptableState.preempt = True
rate = rospy.Rate(10)
while self._running:
rate.sleep()
self._sm = None
if msg.current_state_checksum in self._state_checksums:
current_state_path = self._state_checksums[
msg.current_state_checksum]
self._starting_path = "/" + current_state_path[1:].replace(
"/", "_mirror/") + "_mirror"
rospy.loginfo('Current state: %s' % current_state_path)
try:
self._mirror_state_machine(self._current_struct)
rospy.loginfo('Mirror built.')
except (AttributeError, smach.InvalidStateError):
rospy.loginfo(
'Stopping synchronization because behavior has stopped.')
self._execute_mirror()
def _execute_mirror(self):
self._running = True
rospy.loginfo("Executing mirror...")
if self._starting_path is not None:
LockableStateMachine.path_for_switch = self._starting_path
rospy.loginfo("Starting mirror in state " + self._starting_path)
self._starting_path = None
result = 'preempted'
try:
result = self._sm.execute()
except Exception as e:
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)
def _mirror_state_machine(self, msg):
self._current_struct = msg
self._state_checksums = dict()
root = None
for con_msg in msg.containers:
if con_msg.path.find('/') == -1:
root = con_msg.path
break
self._add_node(msg, root)
# 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
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')
sm = JumpableStateMachine(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)
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!'
)
def _preempt_callback(self, msg):
if self._sm is not None and self._sm.is_running():
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!')
class LockableState(ManuallyTransitionableState):
"""
A state that can be locked.
When locked, no transition can be done regardless of the resulting outcome.
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._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)
self._sub.remove_last_msg(self._lock_topic)
self._execute_lock(msg.data)
if self._is_controlled and self._sub.has_msg(self._unlock_topic):
msg = self._sub.get_last_msg(self._unlock_topic)
self._sub.remove_last_msg(self._unlock_topic)
self._execute_unlock(msg.data)
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):
outcome = self._stored_outcome
self._stored_outcome = None
return outcome
else:
return None
outcome = self.__execute(*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):
self._stored_outcome = outcome
return None
return outcome
def _execute_lock(self, target):
found_target = False
if target == self._get_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 ""]))
if not found_target:
rospy.logwarn("--> Wanted to lock %s, but could not find it in current path %s.", target, self._get_path())
else:
rospy.loginfo("--> Locking in state %s", target)
def _execute_unlock(self, target):
found_target = False
if target == self._get_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 ""]))
if not found_target:
rospy.logwarn("--> Wanted to unlock %s, but could not find it in current path %s.", target, self._get_path())
else:
rospy.loginfo("--> Unlocking in state %s", target)
def _enable_ros_control(self):
super(LockableState, self)._enable_ros_control()
self._pub.createPublisher(self._feedback_topic, CommandFeedback)
self._sub.subscribe(self._lock_topic, String)
self._sub.subscribe(self._unlock_topic, String)
def _disable_ros_control(self):
super(LockableState, self)._disable_ros_control()
self._sub.unsubscribe_topic(self._lock_topic)
self._sub.unsubscribe_topic(self._unlock_topic)
def is_locked(self):
return self._locked
class CheckForPersonState(EventState):
"""
Checks whether or not the robot sees a Person
-- name String Name of the Person to search for
-- timeout float Timeout in seconds
<= found The Person was found
<= not_found The Person was not found
<= failed Execution failed
"""
def __init__(self, name, timeout=2.0):
"""Constructor"""
super(CheckForPersonState,
self).__init__(outcomes=['found', 'not_found', 'failed'])
self._name = name
self._mutex = Lock()
self._people = []
self._timeout = timeout
self._start_time = None
self._callback_received = False
self._transform_topic = '/clf_perception_vision/people/raw/transform'
self._service_name = 'pepper_face_identification'
self._service_proxy = ProxyServiceCaller(
{self._service_name: DoIKnowThatPerson})
self._transform_listener = ProxySubscriberCached(
{self._transform_topic: ExtendedPeople})
def people_callback(self, data):
self._mutex.acquire()
self._callback_received = True
self._people = []
for p in data.persons:
self._people.append(p)
self._transform_listener.unsubscribe_topic(self._transform_topic)
self._mutex.release()
def execute(self, userdata):
"""wait for transform callback, check all transforms for requested person"""
elapsed = rospy.get_rostime() - self._start_time
if elapsed.to_sec() > self._timeout:
return 'not_found'
self._mutex.acquire()
if self._callback_received:
known = False
for p in self._people:
request = DoIKnowThatPersonRequest()
request.transform_id = p.transformid
response = self._service_proxy.call(self._service_name,
request)
if response.name == self.name:
known = True
self._mutex.release()
if known:
return 'found'
else:
return 'not_found'
self._mutex.release()
def on_enter(self, userdata):
"""Subscribe to transforms, start timeout-timer"""
self._start_time = rospy.get_rostime()
self._transform_listener.subscribe(self._transform_topic,
ExtendedPeople,
self.people_callback)
def on_exit(self, userdata):
self._transform_listener.unsubscribe_topic(self._transform_topic)
def on_stop(self):
self._transform_listener.unsubscribe_topic(self._transform_topic)
