class BehaviorActionServer(object):
def __init__(self):
self._behavior_started = False
self._preempt_requested = False
self._current_state = None
self._active_behavior_id = None
self._pub = rospy.Publisher('flexbe/start_behavior',
BehaviorSelection,
queue_size=100)
self._preempt_pub = rospy.Publisher('flexbe/command/preempt',
Empty,
queue_size=100)
self._status_sub = rospy.Subscriber('flexbe/status', BEStatus,
self._status_cb)
self._state_sub = rospy.Subscriber('flexbe/behavior_update', String,
self._state_cb)
self._as = actionlib.SimpleActionServer('flexbe/execute_behavior',
BehaviorExecutionAction, None,
False)
self._as.register_preempt_callback(self._preempt_cb)
self._as.register_goal_callback(self._goal_cb)
self._rp = RosPack()
self._behavior_lib = BehaviorLibrary()
# start action server after all member variables have been initialized
self._as.start()
rospy.loginfo("%d behaviors available, ready for start request." %
self._behavior_lib.count_behaviors())
def _goal_cb(self):
if self._as.is_active() or not self._as.is_new_goal_available():
return
goal = self._as.accept_new_goal()
rospy.loginfo('Received a new request to start behavior: %s' %
goal.behavior_name)
be_id, behavior = self._behavior_lib.find_behavior(goal.behavior_name)
if be_id is None:
Logger.logerr("Did not find behavior with requested name: %s" %
goal.behavior_name)
self._as.set_preempted()
return
be_selection = BehaviorSelection()
be_selection.behavior_id = be_id
be_selection.autonomy_level = 255
try:
for k, v in zip(goal.arg_keys, goal.arg_values):
if v.startswith('file://'):
v = v.replace('file://', '', 1)
path = v.split(':')[0]
if len(v.split(':')) > 1:
ns = v.split(':')[1]
else:
ns = ''
if path.startswith('~') or path.startswith('/'):
filepath = os.path.expanduser(path)
else:
filepath = os.path.join(
self._rp.get_path(path.split('/')[0]),
'/'.join(path.split('/')[1:]))
with open(filepath, 'r') as f:
content = f.read()
if ns != '':
content = yaml.load(content)
if ns in content:
content = content[ns]
content = yaml.dump(content)
be_selection.arg_keys.append(k)
be_selection.arg_values.append(content)
else:
be_selection.arg_keys.append(k)
be_selection.arg_values.append(v)
except Exception as e:
rospy.logwarn(
'Failed to parse and substitute behavior arguments, will use direct input.\n%s'
% str(e))
be_selection.arg_keys = goal.arg_keys
be_selection.arg_values = goal.arg_values
be_selection.input_keys = goal.input_keys
be_selection.input_values = goal.input_values
# check for local modifications of the behavior to send them to the onboard behavior
be_filepath_new = self._behavior_lib.get_sourcecode_filepath(be_id)
with open(be_filepath_new, "r") as f:
be_content_new = f.read()
be_filepath_old = self._behavior_lib.get_sourcecode_filepath(
be_id, add_tmp=True)
if not os.path.isfile(be_filepath_old):
be_selection.behavior_checksum = zlib.adler32(be_content_new)
else:
with open(be_filepath_old, "r") as f:
be_content_old = f.read()
sqm = difflib.SequenceMatcher(a=be_content_old, b=be_content_new)
diffs = [x[1] for x in sqm.get_grouped_opcodes(0)]
for opcode, a0, a1, b0, b1 in diffs:
content = be_content_new[b0:b1]
be_selection.modifications.append(
BehaviorModification(a0, a1, content))
be_selection.behavior_checksum = zlib.adler32(be_content_new)
# reset state before starting new behavior
self._current_state = None
self._behavior_started = False
self._preempt_requested = False
# start new behavior
self._pub.publish(be_selection)
def _preempt_cb(self):
self._preempt_requested = True
if not self._behavior_started:
return
self._preempt_pub.publish()
rospy.loginfo('Behavior execution preempt requested!')
def _status_cb(self, msg):
if msg.code == BEStatus.ERROR:
rospy.logerr(
'Failed to run behavior! Check onboard terminal for further infos.'
)
self._as.set_aborted('')
# Call goal cb in case there is a queued goal available
self._goal_cb()
return
if not self._behavior_started and msg.code == BEStatus.STARTED:
self._behavior_started = True
self._active_behavior_id = msg.behavior_id
rospy.loginfo('Behavior execution has started!')
# Preempt if the goal was asked to preempt before the behavior started
if self._preempt_requested:
self._preempt_cb()
# Ignore status until behavior start was received
if not self._behavior_started:
return
if msg.behavior_id != self._active_behavior_id:
rospy.logwarn(
'Ignored status because behavior id differed ({} vs {})!'.
format(msg.behavior_id, self._active_behavior_id))
return
elif msg.code == BEStatus.FINISHED:
result = msg.args[0] if len(msg.args) >= 1 else ''
rospy.loginfo('Finished behavior execution with result "%s"!' %
result)
self._as.set_succeeded(BehaviorExecutionResult(outcome=result))
# Call goal cb in case there is a queued goal available
self._goal_cb()
elif msg.code == BEStatus.FAILED:
rospy.logerr('Behavior execution failed in state %s!' %
str(self._current_state))
self._as.set_aborted('')
# Call goal cb in case there is a queued goal available
self._goal_cb()
def _state_cb(self, msg):
self._current_state = msg.data
if self._as.is_active():
self._as.publish_feedback(
BehaviorExecutionFeedback(self._current_state))
rospy.loginfo('Current state: %s' % self._current_state)
class BehaviorLauncher(object):
MIN_VERSION = '2.2.0'
def __init__(self):
Logger.initialize()
self._ready_event = threading.Event()
self._sub = rospy.Subscriber("flexbe/request_behavior",
BehaviorRequest, self._callback)
self._version_sub = rospy.Subscriber("flexbe/ui_version", String,
self._version_callback)
self._pub = rospy.Publisher("flexbe/start_behavior",
BehaviorSelection,
queue_size=100)
self._status_pub = rospy.Publisher("flexbe/status",
BEStatus,
queue_size=100)
self._status_sub = rospy.Subscriber("flexbe/status", BEStatus,
self._status_callback)
self._mirror_pub = rospy.Publisher("flexbe/mirror/structure",
ContainerStructure,
queue_size=100)
self._rp = RosPack()
self._behavior_lib = BehaviorLibrary()
rospy.loginfo("%d behaviors available, ready for start request." %
self._behavior_lib.count_behaviors())
def _status_callback(self, msg):
if msg.code == BEStatus.READY:
self._ready_event.set()
def _callback(self, msg):
be_id, behavior = self._behavior_lib.find_behavior(msg.behavior_name)
if be_id is None:
Logger.logerr("Did not find behavior with requested name: %s" %
msg.behavior_name)
self._status_pub.publish(BEStatus(code=BEStatus.ERROR))
return
rospy.loginfo("Request for behavior " + behavior["name"])
be_selection = BehaviorSelection()
be_selection.behavior_id = be_id
be_selection.autonomy_level = msg.autonomy_level
try:
for k, v in zip(msg.arg_keys, msg.arg_values):
if k.startswith('/YAML:'):
key = k.replace('/YAML:', '/', 1)
path = v.split(':')[0]
ns = v.split(':')[1]
if path.startswith('~') or path.startswith('/'):
yamlpath = os.path.expanduser(path)
else:
yamlpath = os.path.join(
self._rp.get_path(path.split('/')[0]),
'/'.join(path.split('/')[1:]))
with open(yamlpath, 'r') as f:
content = yaml.load(f)
if ns != '' and ns in content:
content = content[ns]
be_selection.arg_keys.append(key)
be_selection.arg_values.append(yaml.dump(content))
else:
be_selection.arg_keys.append(k)
be_selection.arg_values.append(v)
except Exception as e:
rospy.logwarn(
'Failed to parse and substitute behavior arguments, will use direct input.\n%s'
% str(e))
be_selection.arg_keys = msg.arg_keys
be_selection.arg_values = msg.arg_values
# wait until Behavior Engine status is BEStatus.READY
self._ready_event.wait()
be_structure = ContainerStructure()
be_structure.containers = msg.structure
try:
be_filepath_new = self._behavior_lib.get_sourcecode_filepath(be_id)
except ResourceNotFound:
rospy.logerr("Could not find behavior package '%s'" %
(behavior["package"]))
rospy.loginfo(
"Have you updated your ROS_PACKAGE_PATH after creating the behavior?"
)
return
with open(be_filepath_new, "r") as f:
be_content_new = f.read()
be_filepath_old = self._behavior_lib.get_sourcecode_filepath(
be_id, add_tmp=True)
if not os.path.isfile(be_filepath_old):
be_selection.behavior_checksum = zlib.adler32(be_content_new)
if msg.autonomy_level != 255:
be_structure.behavior_id = be_selection.behavior_checksum
self._mirror_pub.publish(be_structure)
self._pub.publish(be_selection)
rospy.loginfo("No changes to behavior version.")
return
with open(be_filepath_old, "r") as f:
be_content_old = f.read()
sqm = difflib.SequenceMatcher(a=be_content_old, b=be_content_new)
diffs = [x[1] for x in sqm.get_grouped_opcodes(0)]
for opcode, a0, a1, b0, b1 in diffs:
content = be_content_new[b0:b1]
be_selection.modifications.append(
BehaviorModification(a0, a1, content))
be_selection.behavior_checksum = zlib.adler32(be_content_new)
if msg.autonomy_level != 255:
be_structure.behavior_id = be_selection.behavior_checksum
self._mirror_pub.publish(be_structure)
self._pub.publish(be_selection)
def _version_callback(self, msg):
vui = self._parse_version(msg.data)
vex = self._parse_version(BehaviorLauncher.MIN_VERSION)
if vui < vex:
Logger.logwarn('FlexBE App needs to be updated!\n' \
+ 'Require at least version %s, but have %s\n' % (BehaviorLauncher.MIN_VERSION, msg.data) \
+ 'Please run a "git pull" in "roscd flexbe_app".')
def _parse_version(self, v):
result = 0
offset = 1
for n in reversed(v.split('.')):
result += int(n) * offset
offset *= 100
return result
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 BehaviorActionServer(object):
def __init__(self):
self._behavior_started = False
self._current_state = None
self._engine_status = None
self._pub = rospy.Publisher('flexbe/start_behavior',
BehaviorSelection,
queue_size=100)
self._preempt_pub = rospy.Publisher('flexbe/command/preempt',
Empty,
queue_size=100)
self._status_sub = rospy.Subscriber('flexbe/status', BEStatus,
self._status_cb)
self._state_sub = rospy.Subscriber('flexbe/behavior_update', String,
self._state_cb)
self._as = actionlib.SimpleActionServer('flexbe/execute_behavior',
BehaviorExecutionAction,
self._execute_cb, False)
self._rp = RosPack()
self._behavior_lib = BehaviorLibrary()
# start action server after all member variables have been initialized
self._as.start()
rospy.loginfo("%d behaviors available, ready for start request." %
self._behavior_lib.count_behaviors())
def _execute_cb(self, goal):
rospy.loginfo('Received a new request to start behavior: %s' %
goal.behavior_name)
be_id, behavior = self._behavior_lib.find_behavior(goal.behavior_name)
if be_id is None:
Logger.logerr("Did not find behavior with requested name: %s" %
goal.behavior_name)
self._as.set_preempted()
return
be_selection = BehaviorSelection()
be_selection.behavior_id = be_id
be_selection.autonomy_level = 255
try:
for k, v in zip(goal.arg_keys, goal.arg_values):
if v.startswith('file://'):
v = v.replace('file://', '', 1)
path = v.split(':')[0]
if len(v.split(':')) > 1:
ns = v.split(':')[1]
else:
ns = ''
if path.startswith('~') or path.startswith('/'):
filepath = os.path.expanduser(path)
else:
filepath = os.path.join(
self._rp.get_path(path.split('/')[0]),
'/'.join(path.split('/')[1:]))
with open(filepath, 'r') as f:
content = f.read()
if ns != '':
content = yaml.load(content)
if ns in content:
content = content[ns]
content = yaml.dump(content)
be_selection.arg_keys.append(k)
be_selection.arg_values.append(content)
else:
be_selection.arg_keys.append(k)
be_selection.arg_values.append(v)
except Exception as e:
rospy.logwarn(
'Failed to parse and substitute behavior arguments, will use direct input.\n%s'
% str(e))
be_selection.arg_keys = goal.arg_keys
be_selection.arg_values = goal.arg_values
be_selection.input_keys = goal.input_keys
be_selection.input_values = goal.input_values
# check for local modifications of the behavior to send them to the onboard behavior
be_filepath_new = self._behavior_lib.get_sourcecode_filepath(be_id)
with open(be_filepath_new, "r") as f:
be_content_new = f.read()
be_filepath_old = self._behavior_lib.get_sourcecode_filepath(
be_id, add_tmp=True)
if not os.path.isfile(be_filepath_old):
be_selection.behavior_checksum = zlib.adler32(be_content_new)
else:
with open(be_filepath_old, "r") as f:
be_content_old = f.read()
sqm = difflib.SequenceMatcher(a=be_content_old, b=be_content_new)
diffs = [x[1] for x in sqm.get_grouped_opcodes(0)]
for opcode, a0, a1, b0, b1 in diffs:
content = be_content_new[b0:b1]
be_selection.modifications.append(
BehaviorModification(a0, a1, content))
be_selection.behavior_checksum = zlib.adler32(be_content_new)
# reset state before starting new behavior
self._engine_status = None
self._current_state = None
self._behavior_started = False
# start new behavior
self._pub.publish(be_selection)
try:
rate = rospy.Rate(10)
while not rospy.is_shutdown():
if self._current_state is not None:
self._as.publish_feedback(
BehaviorExecutionFeedback(self._current_state))
self._current_state = None
# check if goal has been preempted first
if self._as.is_preempt_requested():
rospy.loginfo('Behavior execution preempt requested!')
self._preempt_pub.publish()
rate.sleep()
self._as.set_preempted('')
break
if self._engine_status is None:
rospy.logdebug_throttle(
1,
'No behavior engine status received yet. Waiting for it...'
)
rate.sleep()
continue
if self._engine_status.code == BEStatus.ERROR:
rospy.logerr(
'Failed to run behavior! Check onboard terminal for further infos.'
)
rate.sleep()
self._as.set_aborted('')
break
if not self._behavior_started:
rospy.logdebug_throttle(
1,
'Behavior execution has not yet started. Waiting for it...'
)
rate.sleep()
continue
if self._engine_status.code == BEStatus.FINISHED:
result = self._engine_status.args[0] \
if len(self._engine_status.args) >= 1 else ''
rospy.loginfo(
'Finished behavior execution with result "%s"!' %
result)
self._as.set_succeeded(
BehaviorExecutionResult(outcome=result))
break
if self._engine_status.code == BEStatus.FAILED:
rospy.logerr('Behavior execution failed in state %s!' %
str(self._current_state))
rate.sleep()
self._as.set_aborted('')
break
rate.sleep()
rospy.loginfo('Ready for next behavior start request.')
except rospy.ROSInterruptException:
pass # allow clean exit on ROS shutdown
def _status_cb(self, msg):
self._engine_status = msg
# check for behavior start here, to avoid race condition between execute_cb and status_cb threads
if not self._behavior_started and self._engine_status.code == BEStatus.STARTED:
self._behavior_started = True
rospy.loginfo('Behavior execution has started!')
def _state_cb(self, msg):
self._current_state = msg.data
rospy.loginfo('Current state: %s' % self._current_state)