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): 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 = dict() Logger.initialize() behaviors_package = "flexbe_behaviors" if rospy.has_param("behaviors_package"): behaviors_package = rospy.get_param("behaviors_package") else: rospy.loginfo("Using default behaviors package: %s" % behaviors_package) manifest_folder = os.path.join(self._rp.get_path(behaviors_package), 'behaviors/') file_entries = [os.path.join(manifest_folder, filename) for filename in os.listdir(manifest_folder) if not filename.startswith('#')] manifests = sorted([xmlpath for xmlpath in file_entries if not os.path.isdir(xmlpath)]) for i in range(len(manifests)): m = ET.parse(manifests[i]).getroot() e = m.find("executable") self._behavior_lib[i] = { "name": m.get("name"), "package": e.get("package_path").split(".")[0], "file": e.get("package_path").split(".")[1], "class": e.get("class") } rospy.loginfo("%d behaviors available, ready for start request." % len(self._behavior_lib.items()))
def __init__(self): 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 = dict() Logger.initialize() behaviors_package = "flexbe_behaviors" if rospy.has_param("~behaviors_package"): behaviors_package = rospy.get_param("~behaviors_package") rospy.loginfo("Using custom behaviors package: %s" % behaviors_package) else: rospy.loginfo("Using default behaviors package: %s" % behaviors_package) manifest_folder = os.path.join(self._rp.get_path(behaviors_package), 'behaviors/') file_entries = [ os.path.join(manifest_folder, filename) for filename in os.listdir(manifest_folder) if not filename.startswith('#') ] manifests = sorted([ xmlpath for xmlpath in file_entries if not os.path.isdir(xmlpath) ]) for i in range(len(manifests)): m = ET.parse(manifests[i]).getroot() e = m.find("executable") self._behavior_lib[i] = { "name": m.get("name"), "package": e.get("package_path").split(".")[0], "file": e.get("package_path").split(".")[1], "class": e.get("class") } rospy.loginfo("%d behaviors available, ready for start request." % len(self._behavior_lib.items()))
def __init__(self): ''' Constructor ''' self.be = None Logger.initialize() #ProxyPublisher._simulate_delay = True #ProxySubscriberCached._simulate_delay = True # prepare temp folder rp = rospkg.RosPack() self._tmp_folder = os.path.join(rp.get_path('flexbe_onboard'), 'tmp/') if not os.path.exists(self._tmp_folder): os.makedirs(self._tmp_folder) sys.path.append(self._tmp_folder) # prepare manifest folder access manifest_folder = os.path.join(rp.get_path('flexbe_behaviors'), 'behaviors/') rospy.loginfo("Parsing available behaviors...") file_entries = [os.path.join(manifest_folder, filename) for filename in os.listdir(manifest_folder) if not filename.startswith('#')] manifests = sorted([xmlpath for xmlpath in file_entries if not os.path.isdir(xmlpath)]) self._behavior_lib = dict() for i in range(len(manifests)): m = ET.parse(manifests[i]).getroot() e = m.find("executable") self._behavior_lib[i] = {"name": m.get("name"), "package": e.get("package_path").split(".")[0], "file": e.get("package_path").split(".")[1], "class": e.get("class")} # rospy.loginfo("+++ " + self._behavior_lib[i]["name"]) self._pub = ProxyPublisher() self._sub = ProxySubscriberCached() self.status_topic = '/flexbe/status' self.feedback_topic = '/flexbe/command_feedback' self._pub.createPublisher(self.status_topic, BEStatus, _latch = True) self._pub.createPublisher(self.feedback_topic, CommandFeedback) # listen for new behavior to start self._running = False self._switching = False self._sub.subscribe('/flexbe/start_behavior', BehaviorSelection, self._behavior_callback) # heartbeat self._pub.createPublisher('/flexbe/heartbeat', Empty) self._execute_heartbeat() rospy.sleep(0.5) # wait for publishers etc to really be set up self._pub.publish(self.status_topic, BEStatus(code=BEStatus.READY)) rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m')
def __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')
def __init__(self): ''' Constructor ''' self.be = None self._current_behavior = 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 behaviors_package = "flexbe_behaviors" if rospy.has_param("~behaviors_package"): behaviors_package = rospy.get_param("~behaviors_package") rospy.loginfo("Using custom behaviors package: %s" % behaviors_package) else: rospy.loginfo("Using default behaviors package: %s" % behaviors_package) # prepare temp folder rp = rospkg.RosPack() self._tmp_folder = os.path.join(rp.get_path('flexbe_onboard'), 'tmp/') if not os.path.exists(self._tmp_folder): os.makedirs(self._tmp_folder) sys.path.append(self._tmp_folder) # prepare manifest folder access manifest_folder = os.path.join(rp.get_path(behaviors_package), 'behaviors/') rospy.loginfo("Parsing available behaviors...") file_entries = [ os.path.join(manifest_folder, filename) for filename in os.listdir(manifest_folder) if not filename.startswith('#') ] manifests = sorted([ xmlpath for xmlpath in file_entries if not os.path.isdir(xmlpath) ]) self._behavior_lib = dict() for i in range(len(manifests)): try: m = ET.parse(manifests[i]).getroot() except ET.ParseError: rospy.logerr( 'Failed to parse behavior description xml file: "%s"' % manifests[i]) else: e = m.find("executable") self._behavior_lib[i] = { "name": m.get("name"), "package": e.get("package_path").split(".")[0], "file": e.get("package_path").split(".")[1], "class": e.get("class") } # rospy.loginfo("+++ " + self._behavior_lib[i]["name"]) # enable automatic reloading of all subsequent modules on reload _reload_importer = ReloadImporter() _reload_importer.add_reload_path(self._tmp_folder) behaviors_folder = os.path.abspath( os.path.join(rp.get_path(self._behavior_lib[0]['package']), '..')) _reload_importer.add_reload_path(behaviors_folder) _reload_importer.enable() 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')