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)