def getRosIncludePaths(): import os try: from rospkg import RosPack except ImportError: return [] rospack = RosPack() includes = [] cDirectory = os.getcwd() if "firmware" in cDirectory: for p in os.path.expandvars('$ROS_PACKAGE_PATH').split(':'): if os.path.exists(p + '/../build/pas_firmware/ros_lib'): includes.append(p + '/../build/pas_firmware/ros_lib') for p in rospack.list(): if os.path.exists(rospack.get_path(p) + '/firmware/include'): includes.append(rospack.get_path(p) + '/firmware/include') else: for p in os.path.expandvars('$ROS_PACKAGE_PATH').split(':'): if os.path.exists(p + '/../devel/include'): includes.append(p + '/../devel/include') for p in rospack.list(): if os.path.exists(rospack.get_path(p) + '/include'): includes.append(rospack.get_path(p) + '/include') distros = ['melodic', 'lunar', 'kinetic', 'jade', 'indigo', 'hydro'] for distro in distros: if os.path.exists('/opt/ros/' + distro + '/include'): includes.append('/opt/ros/' + distro + '/include') print('ROS Paths: ' + ' '.join(includes)) return includes
def GetRosIncludePaths(): """Return a list of potential include directories The directories are looked for in the ENV_WORKSPACES environment variable. """ try: from rospkg import RosPack except ImportError: return [] rospack = RosPack() includes = [] paths = [] if not ENV_WORKSPACES in os.environ: raise ValueError( "The {} environmental variable is not set!".format(ENV_WORKSPACES)) else: paths = os.environ[ENV_WORKSPACES] workspaces = paths.split() workspaces = paths.split() for workspace in workspaces: includes.append(os.path.expanduser(workspace) + '/devel/include') for p in rospack.list(): if os.path.exists(rospack.get_path(p) + '/include'): includes.append(rospack.get_path(p) + '/include') for distribution in os.listdir('/opt/ros'): includes.append('/opt/ros/' + distribution + '/include') return includes
def GetRosIncludePaths(): """Return a list of potential include directories The directories are looked for in $ROS_WORKSPACE. """ try: from rospkg import RosPack except ImportError: return [] rospack = RosPack() includes = [] rospath = '/home/bradsaund/catkin_ws' includes.append(rospath + '/devel/include') for paths in os.walk(rospath + '/src'): if paths[0].endswith('include'): includes.append(paths[0]) for p in rospack.list(): if os.path.exists(rospack.get_path(p) + '/include'): includes.append(rospack.get_path(p) + '/include') for distribution in os.listdir('/opt/ros'): includes.append('/opt/ros/' + distribution + '/include') return includes
def test_RosPack_list(): from rospkg import RosPack, get_ros_root if get_ros_root() is not None and rospack_is_available(): r = RosPack() pkgs = rospack_list() retval = r.list() assert set(pkgs) == set(retval), "%s vs %s" % (pkgs, retval) # test twice for caching retval = r.list() assert set(pkgs) == set(retval), "%s vs %s" % (pkgs, retval) # make sure stress test works with rospack_cache invalidated delete_cache() r = RosPack() retval = r.list() assert set(pkgs) == set(retval), "%s vs %s" % (pkgs, retval)
def test_RosPack_list(): from rospkg import RosPack, get_ros_root if get_ros_root() is not None and rospack_is_available(): r = RosPack() pkgs = rospack_list() retval = r.list() assert set(pkgs) == set(retval), "%s vs %s"%(pkgs, retval) # test twice for caching retval = r.list() assert set(pkgs) == set(retval), "%s vs %s"%(pkgs, retval) # make sure stress test works with rospack_cache invalidated delete_cache() r = RosPack() retval = r.list() assert set(pkgs) == set(retval), "%s vs %s"%(pkgs, retval)
def gather_candidates(self, context: UserContext) -> Candidates: rosp = RosPack() if len(self.cached) == 0: self.cached = [{ 'word': pkg, 'ros_pkg_path': rosp.get_path(pkg) } for pkg in rosp.list()] if context['args']: return [x for x in self.cached if x['word'] in context['args']] return self.cached
def _get_packages_in_environment(): global _packages_in_environment if _packages_in_environment is None: if ROS_PACKAGE_PATH not in os.environ or not os.environ[ROS_PACKAGE_PATH]: raise RuntimeError("The environment variable '%s' must be set when using '%s'" % (ROS_PACKAGE_PATH, ARG_CURRENT_ENVIRONMENT)) _packages_in_environment = set([]) rs = RosStack() _packages_in_environment.update(set(rs.list())) rp = RosPack() _packages_in_environment.update(set(rp.list())) return _packages_in_environment
def collect_plugins(root_scan_dir): """ Scan directories starting in the designated root to locate all packages that depend on pluginlib. This will indirectly tell us which packages potentially export plugins. Then we search for the plugin manifest file and we parse it to obtain all the exported plugin classes. root_scan_dir indicates the starting point for the scan returns the collected plugin classes """ rp = RosPack([root_scan_dir]) packages = rp.list() debug_print("Found packages:\n") #print packages debug_print() # Find all packages that depend on pluginlib and nodelet explicitely pluginlib_users = rp.get_depends_on('pluginlib', implicit=False) nodelet_users = rp.get_depends_on('nodelet', implicit=False) image_transport_users = rp.get_depends_on('image_transport', implicit=False) # Concatenate both lists removing the duplicates pluginlib_users += list(set(nodelet_users) - set(pluginlib_users)) pluginlib_users += list(set(image_transport_users) - set(pluginlib_users)) debug_print("Packages that depend on pluginlib:\n") debug_print(pluginlib_users) debug_print() # Within the packages that require pluginlib, search all their # dependencies for plugins plugin_classes = [] for p in pluginlib_users: path = rp.get_path(p) debug_print(p + ": ") debug_print(path) exports = rp.get_manifest(p).exports debug_print("Exports: ") for e in exports: s = e.get("plugin") if s: s2 = string.replace(s, "${prefix}", path) debug_print(s2) f = open(s2, 'r') xml_str = f.read() xml_str = escape_xml(xml_str) plugin_classes += parse_plugin_xml(xml_str, p, path) #plugin_classes += parse_plugin_xml(s2) debug_print(plugin_classes) f.close() debug_print() return plugin_classes
def GetRosIncludePaths(): """Return a list of potential include directories The directories are looked for in the ROS environment variables """ try: from rospkg import RosPack except ImportError: return [] rospack = RosPack() includes = GetWorkspaceIncludes() for p in rospack.list(): if os.path.exists(rospack.get_path(p) + '/include'): includes.append(rospack.get_path(p) + '/include') return includes
def GetRosIncludePaths(): """Return a list of potential include directories The directories are looked for in $ROS_WORKSPACE. """ try: from rospkg import RosPack except ImportError: return [] rospack = RosPack() includes = [] includes.append(os.path.expandvars('$ROS_WORKSPACE') + '/devel/include') for p in rospack.list(): if os.path.exists(rospack.get_path(p) + '/include'): includes.append(rospack.get_path(p) + '/include') for distribution in os.listdir('/opt/ros'): includes.append('/opt/ros/' + distribution + '/include') return includes
def getRosIncludePaths(): import os try: from rospkg import RosPack except ImportError: return [] rospack = RosPack() includes = [] includes.append(os.path.expandvars('$ROS_WORKSPACE') + '/devel/include') for p in rospack.list(): if os.path.exists(rospack.get_path(p) + '/include'): includes.append(rospack.get_path(p) + '/include') if os.path.exists('/opt/ros/indigo/include'): includes.append('/opt/ros/indigo/include') if os.path.exists('/opt/ros/hydro/include'): includes.append('/opt/ros/hydro/include') return includes
def _find_plugins(self, export_tag): plugins = [] r = RosPack() for package_name in r.list(): package_path = r.get_path(package_name) manifest_file_path = os.path.join(package_path, MANIFEST_FILE) if os.path.isfile(manifest_file_path): try: manifest = parse_manifest_file(package_path, MANIFEST_FILE) except InvalidManifest as e: qWarning('Could not parse manifest "%s":\n%s' % (manifest_file_path, e)) continue exports = manifest.get_export(export_tag, 'plugin') for export in exports: plugins.append([package_name, str(export)]) continue package_file_path = os.path.join(package_path, PACKAGE_FILE) if os.path.isfile(package_file_path): # only try to import catkin if a PACKAGE_FILE is found try: from catkin_pkg.package import parse_package, InvalidPackage except ImportError as e: qWarning( 'Package "%s" has a package file, but import of parser failed:\n%s' % (package_path, e)) continue try: package = parse_package(package_file_path) except InvalidPackage as e: qWarning('Could not parse package file "%s":\n%s' % (package_file_path, e)) continue for export in package.exports: if export.tagname != export_tag or 'plugin' not in export.attributes: continue plugin_xml_path = export.attributes['plugin'] plugin_xml_path = plugin_xml_path.replace( '${prefix}', package_path) plugins.append([package_name, plugin_xml_path]) continue return plugins
def wrap(dot, pre, post): dot = [' ' * 4 + line for line in dot] dot = [pre] + dot + [post] return dot by_choice = lambda x: x[1] groups = it.groupby(sorted(deplist, key=by_choice), key=by_choice) graph = [] for group in groups: dot = [] for pkg, choice, deps in group[1]: node(dot, pkg) for dep in deps: edge(dot, pkg, dep) if graph: graph.append('') graph.extend(wrap(dot, 'subgraph cluster_%s {' % group[0], '}')) graph = wrap(graph, 'digraph {', '}') return '\n'.join(graph) chosen = [abspathify(x) for x in sys.argv[1:]] rp = RosPack(RosPack().ros_paths + chosen) packages = [x for x in rp.list() if is_chosen(rp, x, chosen)] deplist = get_deplist(rp, packages) dot = make_dot(deplist) print(dot)
# Contains functions for receiving joint topics and sending joint angles import rospy from sensor_msgs.msg import JointState from trajectory_smoothing.srv import GetSmoothTraj import sys from rospkg import RosPack rp = RosPack() rp.list() #path=rp.get_path('optimization_pkg')+'/scripts' #sys.path.insert(0,path) #from trajectory_pub import trajectoryServer import numpy as np import time class robotTrajInterface: def __init__(self, arm_prefix='/lbr4', hand_prefix='/allegro_hand_right', init_node=True, traj_topic='/grasp/plan'): if (init_node): rospy.init_node('robot_node') # subscribers: self.arm_joint_state = JointState() self.arm_joint_sub = None self.arm_joint_sub = rospy.Subscriber(arm_prefix + '/joint_states', JointState, self.arm_joint_state_cb, self.arm_joint_sub)
class BehaviorLibrary(object): ''' Provides a list of all known behavior manifests. ''' def __init__(self): self._rp = RosPack() self._behavior_lib = dict() self.parse_packages() def parse_packages(self): """ Parses all ROS packages to update the internal behavior library. """ self._behavior_lib = dict() for pkg in self._rp.list(): for export in self._rp._load_manifest(pkg).exports: if export.tag == "flexbe_behaviors": self._add_behavior_manifests(self._rp.get_path(pkg), pkg) def _add_behavior_manifests(self, path, pkg=None): """ Recursively add all behavior manifests in the given folder to the internal library. If a package name is specified, only manifests referring to this package are added. @type path: string @param path: Path of the folder to be traversed. @type pkg: string @param pkg: Optional name of a package to only add manifests referring to this package. """ for entry in os.listdir(path): entry_path = os.path.join(path, entry) if os.path.isdir(entry_path): self._add_behavior_manifests(entry_path, pkg) elif entry.endswith(".xml") and not entry.startswith("#"): m = ET.parse(entry_path).getroot() # structure sanity check if m.tag != "behavior" \ or len(m.findall(".//executable")) == 0 \ or m.find("executable").get("package_path") is None \ or len(m.find("executable").get("package_path").split(".")) != 2: continue e = m.find("executable") if pkg is not None and e.get("package_path").split( ".")[0] != pkg: continue # ignore if manifest not in specified package be_id = zlib.adler32(e.get("package_path")) self._behavior_lib[be_id] = { "name": m.get("name"), "package": e.get("package_path").split(".")[0], "file": e.get("package_path").split(".")[1], "class": e.get("class") } def get_behavior(self, be_id): """ Provides the library entry corresponding to the given ID. @type be_id: int @param be_id: Behavior ID to look up. @return Corresponding library entry or None if not found. """ try: return self._behavior_lib[be_id] except KeyError: rospy.logwarn("Did not find ID %d in libary, updating..." % be_id) self.parse_packages() return self._behavior_lib.get(be_id, None) def find_behavior(self, be_name): """ Searches for a behavior with the given name and returns it along with its ID. @type be_name: string @param be_name: Behavior ID to look up. @return Tuple (be_id, be_entry) corresponding to the name or (None, None) if not found. """ find = lambda: next((id, be) for (id, be) in self._behavior_lib.items() if be["name"] == be_name) try: return find() except StopIteration: rospy.logwarn("Did not find behavior '%s' in libary, updating..." % be_name) self.parse_packages() return find() def count_behaviors(self): """ Counts the available behaviors. @return Number of behaviors. """ return len(self._behavior_lib)
class BehaviorLibrary(object): ''' Provides access to all known behaviors. ''' def __init__(self): self._rp = RosPack() self._behavior_lib = dict() self.parse_packages() def parse_packages(self): """ Parses all ROS packages to update the internal behavior library. """ self._behavior_lib = dict() for pkg in self._rp.list(): for export in self._rp._load_manifest(pkg).exports: if export.tag == "flexbe_behaviors": self._add_behavior_manifests(self._rp.get_path(pkg), pkg) def _add_behavior_manifests(self, path, pkg=None): """ Recursively add all behavior manifests in the given folder to the internal library. If a package name is specified, only manifests referring to this package are added. @type path: string @param path: Path of the folder to be traversed. @type pkg: string @param pkg: Optional name of a package to only add manifests referring to this package. """ for entry in os.listdir(path): entry_path = os.path.join(path, entry) if os.path.isdir(entry_path): self._add_behavior_manifests(entry_path, pkg) elif entry.endswith(".xml") and not entry.startswith("#"): m = ET.parse(entry_path).getroot() # structure sanity check if m.tag != "behavior" \ or len(m.findall(".//executable")) == 0 \ or m.find("executable").get("package_path") is None \ or len(m.find("executable").get("package_path").split(".")) != 2: continue e = m.find("executable") if pkg is not None and e.get("package_path").split( ".")[0] != pkg: continue # ignore if manifest not in specified package be_id = zlib.adler32(e.get("package_path")) self._behavior_lib[be_id] = { "name": m.get("name"), "package": e.get("package_path").split(".")[0], "file": e.get("package_path").split(".")[1], "class": e.get("class") } def get_behavior(self, be_id): """ Provides the library entry corresponding to the given ID. @type be_id: int @param be_id: Behavior ID to look up. @return Corresponding library entry or None if not found. """ try: return self._behavior_lib[be_id] except KeyError: rospy.logwarn("Did not find ID %d in libary, updating..." % be_id) self.parse_packages() return self._behavior_lib.get(be_id, None) def find_behavior(self, be_name): """ Searches for a behavior with the given name and returns it along with its ID. @type be_name: string @param be_name: Behavior ID to look up. @return Tuple (be_id, be_entry) corresponding to the name or (None, None) if not found. """ find = lambda: next((id, be) for (id, be) in self._behavior_lib.items() if be["name"] == be_name) try: return find() except StopIteration: rospy.logwarn("Did not find behavior '%s' in libary, updating..." % be_name) self.parse_packages() return find() def count_behaviors(self): """ Counts the available behaviors. @return Number of behaviors. """ return len(self._behavior_lib) def get_sourcecode_filepath(self, be_id, add_tmp=False): """ Constructs a file path to the source code of corresponding to the given ID. @type be_id: int @param be_id: Behavior ID to look up. @type add_tmp: bool @param add_tmp: Append "_tmp" to the file to consider a temporary version. @return String containing the absolute path to the source code file. """ be_entry = self.get_behavior(be_id) if be_entry is None: # rely on get_behavior to handle/log missing package return None try: module_path = __import__(be_entry["package"]).__path__[-1] except ImportError: rospy.logwarn( "Cannot import behavior package '%s', using 'rospack find' instead" % be_entry["package"]) # rp can find it because otherwise, the above entry would not exist module_path = os.path.join(self._rp.get_path(be_entry["package"]), "src", be_entry["package"]) filename = be_entry["file"] + '.py' if not add_tmp else '_tmp.py' return os.path.join(module_path, filename)
if __argcomplete: argcomplete.autocomplete(parser) args = parser.parse_args() print("Collecting information...", end='\r') sys.stdout.flush() rosdep_resolver = RosdepResolver() apt_cache = apt.Cache() rospack = RosPack() git_info_regex = re.compile("(.*\.git)#(.*)") to_replace = args.packages or [] if len(to_replace) == 0: # Collect packages that can be replaced can_be_replaced = [] for pkg in rospack.list(): path = rospack.get_path(pkg) if path.startswith(ws_src_path): continue pkgs = rosdep_resolver.resolve_apt(pkg) if pkgs is None: continue binary_key = pkgs[0] if not binary_key in apt_cache: continue binary = apt_cache[binary_key] git_info = git_info_regex.match(str(binary.versions[0].homepage)) if git_info is None: continue can_be_replaced.append(pkg)