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 _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 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 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 _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)
