class BehaviorLauncher(object):
MIN_VERSION = '2.2.0'
def __init__(self):
Logger.initialize()
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._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 _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
be_structure = ContainerStructure()
be_structure.containers = msg.structure
try:
be_filepath_new = os.path.join(
self._rp.get_path(behavior["package"]),
'src/' + behavior["package"] + '/' + behavior["file"] + '.py')
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 = os.path.join(
self._rp.get_path(behavior["package"]),
'src/' + behavior["package"] + '/' + behavior["file"] + '_tmp.py')
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 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 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
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 = os.path.join(
self._rp.get_path(behavior["package"]),
'src/' + behavior["package"] + '/' + behavior["file"] + '.py')
with open(be_filepath_new, "r") as f:
be_content_new = f.read()
be_filepath_old = os.path.join(
self._rp.get_path(behavior["package"]),
'src/' + behavior["package"] + '/' + behavior["file"] + '_tmp.py')
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)