def from_roslaunch(roslaunch_files, env=None):
"""
@param roslaunch_file: roslaunch_file to load from
@type roslaunch_file: str
"""
if env is None:
env = os.environ
# can't go any further if launch file doesn't validate
l, c = roslaunch.XmlLoader(), roslaunch.ROSLaunchConfig()
for f in roslaunch_files:
try:
l.load(f, c, verbose=False)
except roslaunch.RLException as e:
raise WtfException("Unable to load roslaunch file [%s]: %s" %
(f, str(e)))
ctx = WtfContext()
ctx.rospack = rospkg.RosPack(rospkg.get_ros_paths(env))
ctx.rosstack = rospkg.RosStack(rospkg.get_ros_paths(env))
ctx.launch_files = roslaunch_files
_load_roslaunch(ctx, roslaunch_files)
# ctx.pkg and ctx.stack initialized by _load_roslaunch
_load_pkg(ctx, ctx.pkg)
if ctx.stack:
_load_stack(ctx, ctx.stack)
_load_env(ctx, env)
return ctx
def from_roslaunch(roslaunch_files, env=None):
"""
@param roslaunch_file: roslaunch_file to load from
@type roslaunch_file: str
"""
if env is None:
env = os.environ
# can't go any further if launch file doesn't validate
l, c = roslaunch.XmlLoader(), roslaunch.ROSLaunchConfig()
for f in roslaunch_files:
try:
l.load(f, c, verbose=False)
except roslaunch.RLException as e:
raise WtfException("Unable to load roslaunch file [%s]: %s"%(f, str(e)))
ctx = WtfContext()
ctx.rospack = rospkg.RosPack(rospkg.get_ros_paths(env))
ctx.rosstack = rospkg.RosStack(rospkg.get_ros_paths(env))
ctx.launch_files = roslaunch_files
_load_roslaunch(ctx, roslaunch_files)
# ctx.pkg and ctx.stack initialized by _load_roslaunch
_load_pkg(ctx, ctx.pkg)
if ctx.stack:
_load_stack(ctx, ctx.stack)
_load_env(ctx, env)
return ctx
def from_env(env=None):
"""
Initialize WtfContext from environment.
@raise WtfException: if context state cannot be initialized
"""
if env is None:
env = os.environ
ctx = WtfContext()
ctx.rospack = rospkg.RosPack(rospkg.get_ros_paths(env))
ctx.rosstack = rospkg.RosStack(rospkg.get_ros_paths(env))
_load_env(ctx, env)
return ctx
def from_env(env=None):
"""
Initialize WtfContext from environment.
@raise WtfException: if context state cannot be initialized
"""
if env is None:
env = os.environ
ctx = WtfContext()
ctx.rospack = rospkg.RosPack(rospkg.get_ros_paths(env))
ctx.rosstack = rospkg.RosStack(rospkg.get_ros_paths(env))
_load_env(ctx, env)
return ctx
def parse_arguments(extra_depends=[], is_catkin=True):
"""
Parse the command line arguments - in format <package-name> [dependencies]".
This will need to upgrade from optparse to argparse post python 2.7
"""
parser = OptionParser(usage="usage: %prog <package-name> [dependencies...]")
options, args = parser.parse_args()
if not args:
parser.error("You must specify a package name and optionally also list package dependencies")
package = args[0]
if type(package) == str:
package = package.decode('utf-8')
depends = args[1:] + extra_depends
if not is_legal_resource_base_name(package):
parser.error("illegal package name [%s]\n\nNames must start with a letter and contain only alphanumeric characters\nand underscores."%package)
if os.path.exists(os.path.abspath(package)):
parser.error("%s already exists, aborting"%package)
print "Is_catkin? %r" % is_catkin
if is_catkin == True: # not able to handle dependency checking
print("WARNING: no ros environment found, skipping package and dependency checks\n")
else: # legacy rosbuild, try and handle dependencies
if not rospkg.get_ros_paths():
print("WARNING: no ros environment found, skipping package and dependency checks\n")
else:
rospack = rospkg.RosPack()
if package_already_exists(package, rospack):
parser.error("package already exists on the ROS_PACKAGE_PATH!")
if not rospkg.on_ros_path(package):
parser.error("current working directory is not on ROS_PACKAGE_PATH!\nPlease update your ROS_PACKAGE_PATH environment variable.\n")
if not dependencies_exist(depends, rospack):
parser.error("dependency [%s] cannot be found"%d)
return (package,depends)
def create_local_process_args(node, machine, env=None):
"""
Subroutine for creating node arguments.
:param env: override os.environ. Warning, this does not override
substitution args in node configuration (for now), ``dict``
:returns: arguments for node process, ``[str]``
:raises: :exc:`NodeParamsException` If args cannot be constructed for Node
as specified (e.g. the node type does not exist)
"""
global _rospack
if not node.name:
raise ValueError("node name must be defined")
# create rospack instance if no cached value is available or for custom environments
if not _rospack or env is not None:
rospack = rospkg.RosPack(rospkg.get_ros_paths(env=env))
# cache rospack instance for default environment
if env is None:
_rospack = rospack
else:
rospack = _rospack
# - Construct rosrun command
remap_args = ["%s:=%s"%(src,dst) for src, dst in node.remap_args]
resolve_dict = {}
#resolve args evaluates substitution commands
#shlex parses a command string into a list of args
# - for the local process args, we *do* resolve the anon tag so that the user can execute
# - the node name and args must be resolved together in case the args refer to the anon node name
(node_name) = substitution_args.resolve_args((node.name), context=resolve_dict, resolve_anon=True)
node.name = node_name
remap_args.append('__name:=%s'%node_name)
resolved = substitution_args.resolve_args(node.args, context=resolve_dict, resolve_anon=True)
try:
if type(resolved) == unicode:
resolved = resolved.encode('UTF-8') #attempt to force to string for shlex/subprocess
except NameError:
pass
args = shlex.split(resolved) + remap_args
try:
#TODO:fuerte: pass through rospack and catkin cache
matches = roslib.packages.find_node(node.package, node.type, rospack=rospack)
except rospkg.ResourceNotFound as e:
# multiple nodes, invalid package
raise NodeParamsException(str(e))
if not matches:
raise NodeParamsException("can't locate node [%s] in package [%s]"%(node.type, node.package))
else:
# old behavior was to take first, do we want to change this in Fuerte-style?
cmd = matches[0]
if not cmd:
raise NodeParamsException("Cannot locate node of type [%s] in package [%s]"%(node.type, node.package))
cmd = [cmd]
if sys.platform in ['win32']:
if os.path.splitext(cmd[0])[1] == '.py':
cmd = ['python'] + cmd
return _launch_prefix_args(node) + cmd + args
def _init_rosstack(env=None):
global _rosstack, _ros_paths
if env is None:
env = os.environ
ros_paths = rospkg.get_ros_paths(env)
if ros_paths != _ros_paths:
_ros_paths = ros_paths
_rosstack = rospkg.RosStack(ros_paths)
def _init_rosstack(env=None):
global _rosstack, _ros_paths
if env is None:
env = os.environ
ros_paths = rospkg.get_ros_paths(env)
if ros_paths != _ros_paths:
_ros_paths = ros_paths
_rosstack = rospkg.RosStack(ros_paths)
def create_local_process_args(node, machine, env=None):
"""
Subroutine for creating node arguments.
:param env: override os.environ. Warning, this does not override
substitution args in node configuration (for now), ``dict``
:returns: arguments for node process, ``[str]``
:raises: :exc:`NodeParamsException` If args cannot be constructed for Node
as specified (e.g. the node type does not exist)
"""
if not node.name:
raise ValueError("node name must be defined")
if env is None:
env = os.environ
# - Construct rosrun command
remap_args = ["%s:=%s" % (src, dst) for src, dst in node.remap_args]
resolve_dict = {}
#resolve args evaluates substitution commands
#shlex parses a command string into a list of args
# - for the local process args, we *do* resolve the anon tag so that the user can execute
# - the node name and args must be resolved together in case the args refer to the anon node name
(node_name) = substitution_args.resolve_args((node.name),
context=resolve_dict,
resolve_anon=True)
node.name = node_name
remap_args.append('__name:=%s' % node_name)
resolved = substitution_args.resolve_args(node.args,
context=resolve_dict,
resolve_anon=True)
if type(resolved) == unicode:
resolved = resolved.encode(
'UTF-8') #attempt to force to string for shlex/subprocess
args = shlex.split(resolved) + remap_args
try:
#TODO:fuerte: pass through rospack and catkin cache
rospack = rospkg.RosPack(rospkg.get_ros_paths(env=env))
matches = roslib.packages.find_node(node.package, node.type, rospack)
except rospkg.ResourceNotFound as e:
# multiple nodes, invalid package
raise NodeParamsException(str(e))
if not matches:
raise NodeParamsException("can't locate node [%s] in package [%s]" %
(node.type, node.package))
else:
# old behavior was to take first, do we want to change this in Fuerte-style?
cmd = matches[0]
if not cmd:
raise NodeParamsException(
"Cannot locate node of type [%s] in package [%s]" %
(node.type, node.package))
cmd = [cmd]
if sys.platform in ['win32']:
if os.path.splitext(cmd[0])[1] == '.py':
cmd = ['python'] + cmd
return _launch_prefix_args(node) + cmd + args
def test_RosStack_list():
from rospkg import RosStack, get_ros_paths, get_ros_package_path
print("ROS paths", get_ros_paths())
if get_ros_paths() is not None:
r = RosStack()
l = rosstack_list()
retval = r.list()
assert set(l) == set(retval), "%s vs %s"%(l, retval)
# test twice for caching
retval = r.list()
assert set(l) == set(retval), "%s vs %s"%(l, retval)
# make sure stress test works with rospack_cache invalidated
delete_cache()
r = RosStack()
retval = r.list()
assert set(l) == set(retval), "%s vs %s"%(l, retval)
def test_RosStack_list():
from rospkg import get_ros_paths, RosStack
print("ROS paths", get_ros_paths())
if get_ros_paths() is not None and rosstack_is_available():
r = RosStack()
l = rosstack_list()
retval = r.list()
assert set(l) == set(retval), "%s vs %s" % (l, retval)
# test twice for caching
retval = r.list()
assert set(l) == set(retval), "%s vs %s" % (l, retval)
# make sure stress test works with rospack_cache invalidated
delete_cache()
r = RosStack()
retval = r.list()
assert set(l) == set(retval), "%s vs %s" % (l, retval)
def from_package(pkg, env=None):
"""
Initialize WtfContext from package name.
@param pkg: package name
@type pkg: str
@raise WtfException: if context state cannot be initialized
"""
if env is None:
env = os.environ
ctx = WtfContext()
ctx.rospack = rospkg.RosPack(rospkg.get_ros_paths(env))
ctx.rosstack = rospkg.RosStack(rospkg.get_ros_paths(env))
_load_pkg(ctx, pkg)
stack = ctx.rospack.stack_of(pkg)
if stack:
_load_stack(ctx, stack)
_load_env(ctx, env)
return ctx
def from_package(pkg, env=None):
"""
Initialize WtfContext from package name.
@param pkg: package name
@type pkg: str
@raise WtfException: if context state cannot be initialized
"""
if env is None:
env = os.environ
ctx = WtfContext()
ctx.rospack = rospkg.RosPack(rospkg.get_ros_paths(env))
ctx.rosstack = rospkg.RosStack(rospkg.get_ros_paths(env))
_load_pkg(ctx, pkg)
stack = ctx.rospack.stack_of(pkg)
if stack:
_load_stack(ctx, stack)
_load_env(ctx, env)
return ctx
def from_stack(stack, env=None):
"""
Initialize WtfContext from stack.
@param stack: stack name
@type stack: str
@raise WtfException: if context state cannot be initialized
"""
if env is None:
env = os.environ
ctx = WtfContext()
ctx.rospack = rospkg.RosPack(rospkg.get_ros_paths(env))
ctx.rosstack = rospkg.RosStack(rospkg.get_ros_paths(env))
_load_stack(ctx, stack)
try:
ctx.pkgs = ctx.rosstack.packages_of(stack)
except rospkg.ResourceNotFound:
# this should be handled elsewhere
ctx.pkgs = []
_load_env(ctx, env)
return ctx
def from_stack(stack, env=None):
"""
Initialize WtfContext from stack.
@param stack: stack name
@type stack: str
@raise WtfException: if context state cannot be initialized
"""
if env is None:
env = os.environ
ctx = WtfContext()
ctx.rospack = rospkg.RosPack(rospkg.get_ros_paths(env))
ctx.rosstack = rospkg.RosStack(rospkg.get_ros_paths(env))
_load_stack(ctx, stack)
try:
ctx.pkgs = ctx.rosstack.packages_of(stack)
except rospkg.ResourceNotFound:
# this should be handled elsewhere
ctx.pkgs = []
_load_env(ctx, env)
return ctx
def test_RosStack_get_path():
from rospkg import RosStack, ResourceNotFound, get_ros_paths
path = get_stack_test_path()
bar_path = os.path.join(path, 's1', 'bar')
baz_path = os.path.join(path, 's2', 'baz')
# point ROS_ROOT at top, should spider entire tree
print("ROS_PATHS: %s"%str([path]))
print("ROS_PACKAGE_PATH: ")
r = RosStack(ros_paths=[path])
assert bar_path == r.get_path('bar'), "%s vs. %s"%(bar_path, r.get_path('bar'))
try:
r.get_path('fake')
assert False
except ResourceNotFound:
pass
# divide tree in half to test precedence
print("ROS PATH 1: %s"%(os.path.join(path, 'p1')))
print("ROS PATH 2: %s"%(os.path.join(path, 'p2')))
foo_path = os.path.join(path, 's1', 'foo')
r = RosStack(ros_paths=[os.path.join(path, 's1'), os.path.join(path, 's2')])
assert foo_path == r.get_path('foo'), "%s vs. %s"%(foo_path, r.get_path('foo'))
assert bar_path == r.get_path('bar')
assert baz_path == r.get_path('baz')
# divide tree in half again and test precedence of ROS_PACKAGE_PATH (foo should switch)
print("ROS_ROOT: %s"%(os.path.join(path, 'p1')))
print("ROS_PACKAGE_PATH: %s"%(os.path.join(path, 'p2')))
foo_path = os.path.join(path, 's2', 'foo')
ros_paths = [os.path.join(path, 'notapath'), os.path.join(path, 's2'), os.path.join(path, 's1')]
r = RosStack(ros_paths=ros_paths)
assert foo_path == r.get_path('foo'), "%s vs. %s"%(foo_path, r.get_path('foo'))
if get_ros_paths():
# stresstest against rospack
r = RosStack()
listval = rosstack_list()
for p in listval:
retval = r.get_path(p)
rospackval = rosstack_find(p)
assert retval == rospackval, "[%s]: %s vs. %s"%(p, retval, rospackval)
# stresstest with cache invalidated
delete_cache()
r = RosStack()
for p in listval:
retval = r.get_path(p)
rospackval = rosstack_find(p)
assert retval == rospackval, "[%s]: %s vs. %s"%(p, retval, rospackval)
def test_RosStack_get_path():
from rospkg import RosStack, ResourceNotFound, get_ros_paths
path = get_stack_test_path()
bar_path = os.path.join(path, 's1', 'bar')
baz_path = os.path.join(path, 's2', 'baz')
# point ROS_ROOT at top, should spider entire tree
print("ROS_PATHS: %s"%str([path]))
print("ROS_PACKAGE_PATH: ")
r = RosStack(ros_paths=[path])
assert bar_path == r.get_path('bar'), "%s vs. %s"%(bar_path, r.get_path('bar'))
try:
r.get_path('fake')
assert False
except ResourceNotFound:
pass
# divide tree in half to test precedence
print("ROS PATH 1: %s"%(os.path.join(path, 'p1')))
print("ROS PATH 2: %s"%(os.path.join(path, 'p2')))
foo_path = os.path.join(path, 's1', 'foo')
r = RosStack(ros_paths=[os.path.join(path, 's1'), os.path.join(path, 's2')])
assert foo_path == r.get_path('foo'), "%s vs. %s"%(foo_path, r.get_path('foo'))
assert bar_path == r.get_path('bar')
assert baz_path == r.get_path('baz')
# divide tree in half again and test precedence of ROS_PACKAGE_PATH (foo should switch)
print("ROS_ROOT: %s"%(os.path.join(path, 'p1')))
print("ROS_PACKAGE_PATH: %s"%(os.path.join(path, 'p2')))
foo_path = os.path.join(path, 's2', 'foo')
ros_paths = [os.path.join(path, 'notapath'), os.path.join(path, 's2'), os.path.join(path, 's1')]
r = RosStack(ros_paths=ros_paths)
assert foo_path == r.get_path('foo'), "%s vs. %s"%(foo_path, r.get_path('foo'))
if get_ros_paths() and rosstack_is_available():
# stresstest against rospack
r = RosStack()
listval = rosstack_list()
for p in listval:
retval = r.get_path(p)
rospackval = rosstack_find(p)
assert retval == rospackval, "[%s]: %s vs. %s"%(p, retval, rospackval)
# stresstest with cache invalidated
delete_cache()
r = RosStack()
for p in listval:
retval = r.get_path(p)
rospackval = rosstack_find(p)
assert retval == rospackval, "[%s]: %s vs. %s"%(p, retval, rospackval)
def get_apps(self):
"""Traverses package directories for valid RWS apps.
Returns a list of valid apps.
"""
if self._catkin_ws is not None:
full_paths = [p for p in rospkg.get_ros_paths() if p.startswith(self._catkin_ws)]
app_list = []
for path in full_paths:
try:
app_list.append(App(path))
except ValueError:
continue
return app_list
else:
return []
def expand_to_packages(names, env=None):
"""
Expand names into a list of packages. Names can either be of packages or stacks.
@param names: names of stacks or packages
@type names: [str]
@return: ([packages], [not_found]). expand_packages() returns two
lists. The first is of packages names. The second is a list of
names for which no matching stack or package was found. Lists may have duplicates.
@rtype: ([str], [str])
"""
if env is None:
env = os.environ
ros_paths = rospkg.get_ros_paths(env)
rospack = rospkg.RosPack(ros_paths)
rosstack = rospkg.RosStack(ros_paths)
return rospkg.expand_to_packages(names, rospack, rosstack)
def expand_to_packages(names, env=None):
"""
Expand names into a list of packages. Names can either be of packages or stacks.
@param names: names of stacks or packages
@type names: [str]
@return: ([packages], [not_found]). expand_packages() returns two
lists. The first is of packages names. The second is a list of
names for which no matching stack or package was found. Lists may have duplicates.
@rtype: ([str], [str])
"""
if env is None:
env = os.environ
ros_paths = rospkg.get_ros_paths(env)
rospack = rospkg.RosPack(ros_paths)
rosstack = rospkg.RosStack(ros_paths)
return rospkg.expand_to_packages(names, rospack, rosstack)
def test_ManifestManager_constructor():
from rospkg import RosPack, RosStack, get_ros_paths
r = RosPack()
assert r._manifest_name == 'manifest.xml'
r = RosStack()
assert r._manifest_name == 'stack.xml'
for c in [RosPack, RosStack]:
r = c()
assert r.ros_paths == get_ros_paths()
tmp = tempfile.gettempdir()
r = c(ros_paths=[tmp])
assert r.ros_paths == [tmp]
# make sure we can't accidentally mutate the actual data
r.ros_paths.append('foo')
assert r.ros_paths == [tmp]
def test_ManifestManager_constructor():
from rospkg import RosPack, RosStack, get_ros_paths
r = RosPack()
assert r._manifest_name == "manifest.xml"
r = RosStack()
assert r._manifest_name == "stack.xml"
for c in [RosPack, RosStack]:
r = c()
assert r.ros_paths == get_ros_paths()
tmp = tempfile.gettempdir()
r = c(ros_paths=[tmp])
assert r.ros_paths == [tmp]
# make sure we can't accidentally mutate the actual data
r.ros_paths.append("foo")
assert r.ros_paths == [tmp]
def test_ManifestManager_get_instance():
from rospkg import RosPack, RosStack, get_ros_paths
for c in [RosPack, RosStack]:
# make sure we get the same instance for defaults ros_paths
r1 = c.get_instance()
assert r1.ros_paths == get_ros_paths()
r2 = c.get_instance()
assert r1 is r2
# make sure we get the same instance for identical custom ros_paths
tmp = tempfile.gettempdir()
r3 = c.get_instance(ros_paths=[tmp])
assert r3.ros_paths == [tmp]
r4 = c.get_instance(ros_paths=[tmp])
assert r3 is r4
# make sure for different ros_paths we got different instances
assert r1 is not r3
def test_RosStack_get_depends_explicit():
from rospkg import RosStack, ResourceNotFound, get_ros_paths
path = get_stack_test_path()
s1 = os.path.join(path, 's1')
s3 = os.path.join(path, 's3')
r = RosStack(ros_paths=[s1, s3])
implicit = False
assert set(r.get_depends('baz', implicit)) == set(['bar', 'foo'])
assert r.get_depends('bar', implicit) == ['foo']
assert r.get_depends('foo', implicit) == []
# stress test: test default environment against rospack
if get_ros_paths():
r = RosStack()
for p in rosstack_list():
retval = set(r.get_depends(p, implicit))
rospackval = set(rosstack_depends1(p))
assert retval == rospackval, "[%s]: %s vs. %s"%(p, retval, rospackval)
def test_RosStack_get_depends_explicit():
from rospkg import RosStack, ResourceNotFound, get_ros_paths
path = get_stack_test_path()
s1 = os.path.join(path, 's1')
s3 = os.path.join(path, 's3')
r = RosStack(ros_paths=[s1, s3])
implicit = False
assert set(r.get_depends('baz', implicit)) == set(['bar', 'foo'])
assert r.get_depends('bar', implicit) == ['foo']
assert r.get_depends('foo', implicit) == []
# stress test: test default environment against rospack
if get_ros_paths() and rosstack_is_available():
r = RosStack()
for p in rosstack_list():
retval = set(r.get_depends(p, implicit))
rospackval = set(rosstack_depends1(p))
assert retval == rospackval, "[%s]: %s vs. %s"%(p, retval, rospackval)
def test_ManifestManager_get_instance():
from rospkg import RosPack, RosStack, get_ros_paths
for c in [RosPack, RosStack]:
# make sure we get the same instance for defaults ros_paths
r1 = c.get_instance()
assert r1.ros_paths == get_ros_paths()
r2 = c.get_instance()
assert r1 is r2
# make sure we get the same instance for identical custom ros_paths
tmp = tempfile.gettempdir()
r3 = c.get_instance(ros_paths=[tmp])
assert r3.ros_paths == [tmp]
r4 = c.get_instance(ros_paths=[tmp])
assert r3 is r4
# make sure for different ros_paths we got different instances
assert r1 is not r3
def test_RosStack_get_depends():
from rospkg import RosStack, ResourceNotFound, get_ros_paths
path = get_stack_test_path()
s1 = os.path.join(path, 's1')
s3 = os.path.join(path, 's3')
r = RosStack(ros_paths=[s1, s3])
# TODO: need one more step
assert set(r.get_depends('baz')) == set(['foo', 'bar'])
assert r.get_depends('bar') == ['foo']
assert r.get_depends('foo') == []
if get_ros_paths():
# stress test: test default environment against rosstack
r = RosStack()
for p in rosstack_list():
retval = set(r.get_depends(p))
rospackval = set(rosstack_depends(p))
assert retval == rospackval, "[%s]: %s vs. %s"%(p, retval, rospackval)
def test_RosStack_get_depends():
from rospkg import get_ros_paths, RosStack
path = get_stack_test_path()
s1 = os.path.join(path, 's1')
s3 = os.path.join(path, 's3')
r = RosStack(ros_paths=[s1, s3])
# TODO: need one more step
assert set(r.get_depends('baz')) == set(['foo', 'bar'])
assert r.get_depends('bar') == ['foo']
assert r.get_depends('foo') == []
if get_ros_paths() and rosstack_is_available():
# stress test: test default environment against rosstack
r = RosStack()
for p in rosstack_list():
retval = set(r.get_depends(p))
rospackval = set(rosstack_depends(p))
assert retval == rospackval, "[%s]: %s vs. %s" % (p, retval,
rospackval)
def test_ManifestManager_constructor():
from rospkg import RosPack, RosStack, get_ros_paths
r = RosPack()
assert r._manifest_name == 'manifest.xml'
assert r._cache_name == 'rospack_cache'
r = RosStack()
assert r._manifest_name == 'stack.xml'
assert r._cache_name == 'rosstack_cache'
for c in [RosPack, RosStack]:
r = c()
assert r.ros_paths == get_ros_paths()
import tempfile
tmp = tempfile.gettempdir()
r = c(ros_paths=[tmp])
assert r.ros_paths == [tmp]
# make sure we can't accidentally mutate the actual data
r.ros_paths.append('foo')
assert r.ros_paths == [tmp]
def GetRosIncludePaths(filename):
"""Return a list of potential include directories"""
try:
import rospkg
except ImportError:
return []
rospack = rospkg.RosPack()
includes = []
workspace_paths = {
find_workspace_above(path) for path in rospkg.get_ros_paths()
}.union({ find_workspace_above(filename) }) - { None }
includes.extend(
os.path.join(path, 'devel', 'include')
for path in workspace_paths)
for workspace_dir in workspace_paths:
for dirpath, dirnames, _ in os.walk(
os.path.join(workspace_dir, 'src'), followlinks=False):
if is_ignored(dirpath, workspace_dir): continue
for dirname in dirnames:
if dirname == 'include':
includes.append(os.path.join(dirpath, dirname))
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')
with open('/tmp/ycm.log', 'a') as f:
f.write('########## INCLUDES: ############\n')
for l in includes:
f.write(l + '\n')
return includes
# later.
parser.print_help()
exit(-1)
if len(argv) > 1:
input_file = argv[1]
# collect mapping yamls
# TODO: perhaps we should only load yamls in those pkgs that are listed
# as build_depends in the manifest of the pkg that contains the
# msgs for which we are generating? We'll get these from catkin,
# passed to use via the '-I' option.
# TODO: perhaps first reverse return of get_ros_paths(), to make sure
# overlay workspaces can override assigned IDs in yamls that are
# loaded later.
ros_paths = rospkg.get_ros_paths()
yaml_paths = collect_mapping_file_paths(
search_path=ros_paths.extend(options.mapping_path))
_mapping_dict = load_mapping_yamls(yaml_paths)
# use md5 for msg to retrieve SM assigned ID, then use that to construct template maps
md5sum = get_md5_for_msg(input_file, options.package,
options.includepath)
sm_id = genfrkl.msg_mapping.map_md5_to_sm_id(_mapping_dict, md5sum)
update_template_map(sm_id, msg_template_map)
update_template_map(sm_id, srv_template_map)
# generate the sources
genmsg.template_tools.generate_from_file(input_file, options.package,
options.outdir, options.emdir,
def __init__(self):
self.pkg = rospkg.get_ros_paths()[1]
self.control_json = {}
self.paths = []
self.load_config_files()
self.create_control_json()
else:
raise ValueError('Usage: rosrun multi_tracker copy_configs.py ' +
'<destination_path>. Omit path to use current directory.')
# TODO will have to first find source directory unless i put the default
# configuration files in the install targets of cmake (which i may want to do to
# package this)
# TODO will need to change if i ever want this to be installable via apt / ROS
# build farm (install target, I assume)
only_in_source = True
if only_in_source:
n_src_dirs = 0
for d in rospkg.get_ros_paths():
# TODO how to find if there are multiple catkin workspaces? probably
# need to call get_path on RosPack initialized on both, and handle empty
# output
if 'src' in split(d):
source_dir = d
if n_src_dirs > 0:
raise IOError(
'found multiple directories with package ' +
'sources. only searching first found. this script not ' +
'currently equipped for this case.')
n_src_dirs += 1
if source_dir is None:
raise IOError(
def main(args):
rospy.init_node('allan_variance_node')
t0 = rospy.get_time()
""""""""""""""
" Parameters "
""""""""""""""
bagfile = rospy.get_param('~bagfile_path', '~/data/static.bag')
topic = rospy.get_param('~imu_topic_name', '/imu')
sample_rate = rospy.get_param('~sample_rate', 100)
is_delta_type = rospy.get_param('~delta_measurement', False)
num_tau = rospy.get_param('~number_of_lags', 1000)
results_path = rospy.get_param('~results_directory_path', None)
""""""""""""""""""""""""""
" Results Directory Path "
""""""""""""""""""""""""""
if results_path is None:
paths = rospkg.get_ros_paths()
path = paths[1] # path to workspace's devel
idx = path.find("ws/")
workspace_path = path[0:(idx+3)]
results_path = workspace_path + 'av_results/'
if not os.path.isdir(results_path):
os.mkdir(results_path)
print "\nResults will be saved in the following directory: \n\n\t %s\n" % results_path
""""""""""""""""""
" Form Tau Array "
""""""""""""""""""
taus = np.logspace(-2.0, 5.0, num=num_tau)
"""""""""""""""""
" Parse Bagfile "
"""""""""""""""""
bag = rosbag.Bag(bagfile)
N = bag.get_message_count(topic) # number of measurement samples
data = np.zeros((6, N)) # preallocate vector of measurements
if is_delta_type:
scale = sampleRate
else:
scale = 1.0
cnt = 0
for topic, msg, t in bag.read_messages(topics=[topic]):
data[0, cnt] = msg.linear_acceleration.x * scale
data[1, cnt] = msg.linear_acceleration.y * scale
data[2, cnt] = msg.linear_acceleration.z * scale
data[3, cnt] = msg.angular_velocity.x * scale
data[4, cnt] = msg.angular_velocity.y * scale
data[5, cnt] = msg.angular_velocity.z * scale
cnt = cnt + 1
bag.close()
print "[%0.2f seconds] Bagfile parsed\n" % (rospy.get_time()-t0)
accel_axis_info = []
gyro_axis_info = []
# Acceleration
for axis in range(3):
(taus_used, adev, adev_err, adev_n) = allantools.adev(
data[axis], data_type='freq', rate=float(sample_rate), taus=taus)
rand_walk = random_walk(taus_used, adev)
w_noise = white_noise(taus_used, adev)
accel_axis_info.append(
(taus_used, adev, rand_walk, w_noise))
print "[%0.2f seconds] Finished calculating allan variance for acceleration axis %d" % (rospy.get_time()-t0, axis)
# Gyro
for axis in range(3, 6):
(taus_used, adev, adev_err, adev_n) = allantools.adev(
data[axis], data_type='freq', rate=float(sample_rate), taus=np.array(taus))
rand_walk = random_walk(taus_used, adev)
w_noise = white_noise(taus_used, adev)
gyro_axis_info.append(
(taus_used, adev, rand_walk, w_noise))
print "[%0.2f seconds] Finished calculating allan variance for gyro axis %d" % (rospy.get_time()-t0, axis-3)
plot_n_save(accel_axis_info, 'acceleration', results_path)
plot_n_save(gyro_axis_info, 'gyroscope', results_path)
inp = raw_input("Press Enter key to close figures and end program\n")
Please make sure that all the executables in this command exist and have
executable permission. This is often caused by a bad launch-prefix.
The traceback for the exception was written to the log file
The Python REPL process has exited
#go time:
import roslaunch
roslaunch.main(['roslaunch','--core'])
#testing failure
import rospkg
import os
import roslib
rospack = rospkg.RosPack(rospkg.get_ros_paths(env=os.environ))
matches = roslib.packages.find_node('rosout','rosout', rospack)
#('process[%s]: start w/ args [%s]', 'master', ['rosmaster', '--core', '-p', '11311', '__log:=c:\\ros\\home\\log\\9c890ec0-7cde-11e5-93d1-003ee1c5c62d\\master.log'])
#('process[%s]: cwd will be [%s]', 'master', 'c:\\ros\\home')
#def rosmaster_main(argv=sys.argv, stdout=sys.stdout, env=os.environ):
#rosmaster --core -p 11311 __log:=c:\ROS\log\e5670e0f-7cd6-11e5-8c31-003ee1c5c62d\master.log
#i see whats going on, rosmaster_main() is called without arguments from rosmaster file (which is a py file without extension). It then uses the argv of the command line to launch as a separate process. It doesnt like bening
#run within python. Might need to change the code there.
import rosmaster
rosmaster.rosmaster_main(['--core', '-p', '11311'])
def main(args):
rospy.init_node('allan_variance_node')
t0 = rospy.get_time()
"""""" """""" ""
" Parameters "
"""""" """""" ""
bagfile = rospy.get_param('~bagfile_path', '~/data/static.bag')
topic = rospy.get_param('~imu_topic_name', '/imu')
axis = rospy.get_param('~axis', 0)
sampleRate = rospy.get_param('~sample_rate', 100)
isDeltaType = rospy.get_param('~delta_measurement', False)
numTau = rospy.get_param('~number_of_lags', 1000)
resultsPath = rospy.get_param('~results_directory_path', None)
"""""" """""" """""" """""" ""
" Results Directory Path "
"""""" """""" """""" """""" ""
if resultsPath is None:
paths = rospkg.get_ros_paths()
path = paths[1] # path to workspace's devel
idx = path.find("ws/")
workspacePath = path[0:(idx + 3)]
resultsPath = workspacePath + 'av_results/'
if not os.path.isdir(resultsPath):
os.mkdir(resultsPath)
print "\nResults will be save in the following directory: \n\n\t %s\n" % resultsPath
"""""" """""" """"""
" Form Tau Array "
"""""" """""" """"""
taus = [None] * numTau
cnt = 0
for i in np.linspace(
-2.0, 5.0,
num=numTau): # lags will span from 10^-2 to 10^5, log spaced
taus[cnt] = pow(10, i)
cnt = cnt + 1
"""""" """""" """""
" Parse Bagfile "
""" """""" """""" ""
bag = rosbag.Bag(bagfile)
N = bag.get_message_count(topic) # number of measurement samples
data = np.zeros((6, N)) # preallocate vector of measurements
if isDeltaType:
scale = sampleRate
else:
scale = 1.0
cnt = 0
for topic, msg, t in bag.read_messages(topics=[topic]):
data[0, cnt] = msg.linear_acceleration.x * scale
data[1, cnt] = msg.linear_acceleration.y * scale
data[2, cnt] = msg.linear_acceleration.z * scale
data[3, cnt] = msg.angular_velocity.x * scale
data[4, cnt] = msg.angular_velocity.y * scale
data[5, cnt] = msg.angular_velocity.z * scale
cnt = cnt + 1
bag.close()
print "[%0.2f seconds] Bagfile parsed\n" % (rospy.get_time() - t0)
"""""" """""" """"""
" Allan Variance "
"""""" """""" """"""
if axis is 0:
currentAxis = 1 # loop through all axes 1-6
else:
currentAxis = axis # just loop one time and break
while (currentAxis <= 6):
(taus_used, adev, adev_err,
adev_n) = allantools.oadev(data[currentAxis - 1],
data_type='freq',
rate=float(sampleRate),
taus=np.array(taus))
randomWalkSegment = getRandomWalkSegment(taus_used, adev)
biasInstabilityPoint = getBiasInstabilityPoint(taus_used, adev)
randomWalk = randomWalkSegment[3]
biasInstability = biasInstabilityPoint[1]
"""""" """""" """
" Save as CSV "
""" """""" """"""
if (currentAxis == 1):
fname = 'allan_accel_x'
title = 'Allan Deviation: Accelerometer X'
elif (currentAxis == 2):
fname = 'allan_accel_y'
title = 'Allan Deviation: Accelerometer Y'
elif (currentAxis == 3):
fname = 'allan_accel_z'
title = 'Allan Deviation: Accelerometer Z'
elif (currentAxis == 4):
fname = 'allan_gyro_x'
title = 'Allan Deviation: Gyroscope X'
elif (currentAxis == 5):
fname = 'allan_gyro_y'
title = 'Allan Deviation: Gyroscope Y'
elif (currentAxis == 6):
fname = 'allan_gyro_z'
title = 'Allan Deviation: Gyroscope Z'
print "[%0.2f seconds] Finished calculating allan variance - writing results to %s" % (
rospy.get_time() - t0, fname)
f = open(resultsPath + fname + '.csv', 'wt')
try:
writer = csv.writer(f)
writer.writerow(('Random Walk', 'Bias Instability'))
writer.writerow((randomWalk, biasInstability))
writer.writerow(('Tau', 'AllanDev', 'AllanDevError', 'AllanDevN'))
for i in range(taus_used.size):
writer.writerow(
(taus_used[i], adev[i], adev_err[i], adev_n[i]))
finally:
f.close()
"""""" """""" """
" Plot Result "
""" """""" """"""
plt.figure(figsize=(12, 8))
ax = plt.gca()
ax.set_yscale('log')
ax.set_xscale('log')
plt.plot(taus_used, adev)
plt.plot([randomWalkSegment[0], randomWalkSegment[2]],
[randomWalkSegment[1], randomWalkSegment[3]], 'k--')
plt.plot(1, randomWalk, 'rx', markeredgewidth=2.5, markersize=14.0)
plt.plot(biasInstabilityPoint[0], biasInstabilityPoint[1], 'ro')
plt.grid(True, which="both")
plt.title(title)
plt.xlabel('Tau (s)')
plt.ylabel('ADEV')
for item in ([ax.title, ax.xaxis.label, ax.yaxis.label] +
ax.get_xticklabels() + ax.get_yticklabels()):
item.set_fontsize(20)
plt.show(block=False)
plt.savefig(resultsPath + fname)
currentAxis = currentAxis + 1 + axis * 6 # increment currentAxis also break if axis is not =0
inp = raw_input("Press Enter key to close figures and end program\n")
class WtfContext(object):
"""
WtfContext stores common state about the ROS filesystem and online
environment. The primary use of this is for convenience (not
having to load this state manually) and performance (not having to
do the same calculation repeatedly).
"""
__slots__ = [
'pkg', 'pkg_dir', 'pkgs', 'stack', 'stack_dir', 'stacks',
'manifest_file', 'manifest', 'env', 'ros_root', 'ros_package_path',
'pythonpath', 'ros_master_uri', 'roslaunch_uris', 'launch_files',
'launch_file_deps', 'launch_file_missing_deps', 'system_state',
'service_providers', 'topics', 'services', 'nodes', 'uri_node_map',
'expected_edges', 'actual_edges', 'unconnected_subscriptions',
'use_sim_time', 'warnings', 'errors', 'rospack', 'rosstack'
]
def __init__(self):
# main package we are running
self.pkg = None
self.pkg_dir = None
# main stack we are running
self.stack = None
self.stack_dir = None
# - list of all packages involved in this check
self.pkgs = []
# - list of all stacks involved in this check
self.stacks = []
# manifest location of package that we are running
self.manifest_file = None
# manifest of package that we are running
self.manifest = None
# environment variables
self.env = {}
# provide these for convenience
self.ros_root = None
self.ros_package_path = None
self.pythonpath = None
# launch file that is being run
self.launch_files = None
self.launch_file_deps = None
self.launch_file_missing_deps = None
# online state
self.roslaunch_uris = None
self.system_state = None #master.getSystemState
self.topics = None
self.services = None
self.service_providers = None #names of nodes with services
self.nodes = None
self.uri_node_map = {}
self.expected_edges = None
self.actual_edges = None
self.unconnected_subscriptions = None
self.use_sim_time = None
# caching rospack instance
self.rospack = self.rosstack = None
# warnings that we have collected so far
self.warnings = []
# errors that we have collected so far
self.errors = []
def as_dictionary(self):
"""
@return: dictionary representation of context, which is
useful for producing error messages
@rtype: dict
"""
return dict((s, getattr(self, s)) for s in self.__slots__)
@staticmethod
def from_roslaunch(roslaunch_files, env=None):
"""
@param roslaunch_file: roslaunch_file to load from
@type roslaunch_file: str
"""
if env is None:
env = os.environ
# can't go any further if launch file doesn't validate
l, c = roslaunch.XmlLoader(), roslaunch.ROSLaunchConfig()
for f in roslaunch_files:
try:
l.load(f, c, verbose=False)
except roslaunch.RLException, e:
raise WtfException("Unable to load roslaunch file [%s]: %s" %
(f, str(e)))
ctx = WtfContext()
ctx.rospack = rospkg.RosPack(rospkg.get_ros_paths(env))
ctx.rosstack = rospkg.RosStack(rospkg.get_ros_paths(env))
ctx.launch_files = roslaunch_files
_load_roslaunch(ctx, roslaunch_files)
# ctx.pkg and ctx.stack initialized by _load_roslaunch
_load_pkg(ctx, ctx.pkg)
if ctx.stack:
_load_stack(ctx, ctx.stack)
_load_env(ctx, env)
return ctx
def create_local_process_args(node, machine, env=None):
"""
Subroutine for creating node arguments.
:param env: override os.environ. Warning, this does not override
substitution args in node configuration (for now), ``dict``
:returns: arguments for node process, ``[str]``
:raises: :exc:`NodeParamsException` If args cannot be constructed for Node
as specified (e.g. the node type does not exist)
"""
global _rospack
if not node.name:
raise ValueError("node name must be defined")
# create rospack instance if no cached value is available or for custom environments
if not _rospack or env is not None:
rospack = rospkg.RosPack(rospkg.get_ros_paths(env=env))
# cache rospack instance for default environment
if env is None:
_rospack = rospack
else:
rospack = _rospack
# - Construct rosrun command
remap_args = ["%s:=%s"%(src,dst) for src, dst in node.remap_args]
resolve_dict = {}
#resolve args evaluates substitution commands
#shlex parses a command string into a list of args
# - for the local process args, we *do* resolve the anon tag so that the user can execute
# - the node name and args must be resolved together in case the args refer to the anon node name
(node_name) = substitution_args.resolve_args((node.name), context=resolve_dict, resolve_anon=True)
node.name = node_name
remap_args.append('__name:=%s'%node_name)
resolved = substitution_args.resolve_args(node.args, context=resolve_dict, resolve_anon=True)
try:
if type(resolved) == unicode:
resolved = resolved.encode('UTF-8') #attempt to force to string for shlex/subprocess
except NameError:
pass
os_posix = os.name == "posix"
args = shlex.split(resolved, posix=os_posix) + remap_args
try:
#TODO:fuerte: pass through rospack and catkin cache
matches = roslib.packages.find_node(node.package, node.type, rospack=rospack)
except rospkg.ResourceNotFound as e:
# multiple nodes, invalid package
raise NodeParamsException(str(e))
if not matches:
raise NodeParamsException("Cannot locate node of type [%s] in package [%s]. Make sure file exists in package path and permission is set to executable (chmod +x)"%(node.type, node.package))
else:
# old behavior was to take first, do we want to change this in Fuerte-style?
cmd = matches[0]
if not cmd:
raise NodeParamsException("Cannot locate node of type [%s] in package [%s]"%(node.type, node.package))
cmd = [cmd]
# Python scripts in ROS tend to omit .py extension since they could become executable
# by adding a shebang line (#!/usr/bin/env python) in Linux environments
# special handle this case by executing the script with the Python executable in Windows environment
if sys.platform in ['win32'] and os.path.splitext(cmd[0])[1].lower() in ['.py', '']:
cmd = ['python'] + cmd
return _launch_prefix_args(node) + cmd + args