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 __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 __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 __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')
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 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._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
def __init__(self, parent, context): super(FlexBeWidget, self).__init__() self._attached = False self._paused = False # load from ui rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('hector_flexbe_rqt_widget'), 'resource', 'flexbe_interface.ui') loadUi(ui_file, self, {'QWidget': QWidget}) self.setObjectName('FlexBeInterfaceUi') # init table widgets self._argument_table_widget = TableWidget(self.argumentTableWidget) self._userdata_table_widget = TableWidget(self.userdataTableWidget) # connect to signals self.executePushButton.clicked[bool].connect( self._execute_button_pressed) self.pausePushButton.clicked[bool].connect(self._pause_button_pressed) self.abortPushButton.clicked[bool].connect(self._abort_button_pressed) self.attachPushButton.clicked[bool].connect( self._attach_button_pressed) self.autonomyComboBox.currentIndexChanged[int].connect( lambda index: self._autonomy_pub.publish(index)) self.clearArgumentPushButton.clicked[bool].connect( self._argument_table_widget.clear) self.clearUserdataPushButton.clicked[bool].connect( self._userdata_table_widget.clear) self.clearLogPushButton.clicked[bool].connect(self.logTextEdit.clear) self.saveLogPushButton.clicked[bool].connect(self._save_log) # Qt signals self.updateCurrentBehavior.connect(self.behaviorLineEdit.setText) self.updateCurrentState.connect(self.currentStateLineEdit.setText) self.setPauseButtonText.connect(self.pausePushButton.setText) self.updateStateLog.connect(self.logTextEdit.append) self.clearStateLog.connect(self.logTextEdit.clear) # init behavior list self._lib = BehaviorLibrary() self._retrieve_behaviors() # init subscribers self._status_sub = rospy.Subscriber('flexbe/status', BEStatus, self._status_cb) self._state_update_sub = rospy.Subscriber('flexbe/debug/current_state', String, self._state_update_cb) self._behavior_update_sub = rospy.Subscriber('flexbe/behavior_update', String, self._behavior_update_cb) # init publisher self._attach_pub = rospy.Publisher('flexbe/command/attach', UInt8, queue_size=1) self._autonomy_pub = rospy.Publisher('flexbe/command/autonomy', UInt8, queue_size=1) self._command_pause_pub = rospy.Publisher('flexbe/command/pause', Bool, queue_size=1) self._command_preempt_pub = rospy.Publisher('flexbe/command/preempt', Empty, queue_size=1) # init action client self._execute_behavior_client = actionlib.SimpleActionClient( 'flexbe/execute_behavior', BehaviorExecutionAction)
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)
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)