def test_get_test_results_dir():
from rospkg import get_ros_root, get_test_results_dir
base = tempfile.gettempdir()
ros_test_results_dir = os.path.join(base, 'ros_test_results_dir')
ros_home_dir = os.path.join(base, 'ros_home_dir')
home_dir = os.path.expanduser('~')
# ROS_TEST_RESULTS_DIR has precedence
env = {
'ROS_ROOT': get_ros_root(),
'ROS_TEST_RESULTS_DIR': ros_test_results_dir,
'ROS_HOME': ros_home_dir
}
assert ros_test_results_dir == get_test_results_dir(env=env)
env = {'ROS_ROOT': get_ros_root(), 'ROS_HOME': ros_home_dir}
assert os.path.join(ros_home_dir,
'test_results') == get_test_results_dir(env=env)
env = {'ROS_ROOT': get_ros_root()}
assert os.path.join(home_dir, '.ros',
'test_results') == get_test_results_dir(env=env)
# test default assignment of env. Don't both checking return value as we would duplicate get_test_results_dir
assert get_test_results_dir() is not None
def test_on_ros_path():
from rospkg import on_ros_path, get_ros_root, get_ros_package_path
from rospkg.environment import _resolve_paths
assert not on_ros_path(tempfile.gettempdir())
if get_ros_root() is not None:
assert on_ros_path(get_ros_root())
paths = _resolve_paths(get_ros_package_path()).split(os.pathsep)
for p in paths:
assert on_ros_path(p), "failed: %s, [%s]"%(p, paths)
def test_on_ros_path():
from rospkg import on_ros_path, get_ros_root, get_ros_package_path
from rospkg.environment import _resolve_paths
assert not on_ros_path(tempfile.gettempdir())
if get_ros_root() is not None:
assert on_ros_path(get_ros_root())
if get_ros_package_path() is not None:
paths = _resolve_paths(get_ros_package_path()).split(os.pathsep)
for p in paths:
assert on_ros_path(p), "failed: %s, [%s]" % (p, paths)
def test_get_ros_root():
from rospkg import get_ros_root
assert None == get_ros_root(env={})
env = {'ROS_ROOT': '/fake/path'}
assert '/fake/path' == get_ros_root(env=env)
real_ros_root = get_ros_root()
if real_ros_root is not None:
# make sure that ros root is a directory
p = os.path.join(real_ros_root, 'Makefile')
env = {'ROS_ROOT': p}
assert p == get_ros_root(env=env)
def test_get_ros_root():
from rospkg import get_ros_root
assert None == get_ros_root(env={})
env = {'ROS_ROOT': '/fake/path'}
assert '/fake/path' == get_ros_root(env=env)
real_ros_root = get_ros_root()
if real_ros_root is not None:
# make sure that ros root is a directory
p = os.path.join(real_ros_root, 'Makefile')
env = {'ROS_ROOT': p}
assert p == get_ros_root(env=env)
def test_get_ros_home():
from rospkg import get_ros_root, get_ros_home
base = tempfile.gettempdir()
ros_home_dir = os.path.join(base, 'ros_home_dir')
home_dir = os.path.expanduser('~')
# ROS_HOME has precedence
env = {'ROS_ROOT': get_ros_root(), 'ROS_HOME': ros_home_dir}
assert ros_home_dir == get_ros_home(env=env)
env = {'ROS_ROOT': get_ros_root()}
assert os.path.join(home_dir, '.ros') == get_ros_home(env=env)
# test default assignment of env. Don't both checking return value
assert get_ros_home() is not None
def test_get_ros_home():
from rospkg import get_ros_root, get_ros_home
base = tempfile.gettempdir()
ros_home_dir = os.path.join(base, 'ros_home_dir')
home_dir = os.path.expanduser('~')
# ROS_HOME has precedence
env = {'ROS_ROOT': get_ros_root(), 'ROS_HOME': ros_home_dir }
assert ros_home_dir == get_ros_home(env=env)
env = {'ROS_ROOT': get_ros_root()}
assert os.path.join(home_dir, '.ros') == get_ros_home(env=env)
# test default assignment of env. Don't both checking return value
assert get_ros_home() is not None
def connect(self):
ros_root = rospkg.get_ros_root()
r = rospkg.RosPack()
manifest = r.get_manifest('rowma_ros')
rowma_ros_version = manifest.version
launch_commands = utils.list_launch_commands()
rosrun_commands = utils.list_rosorun_commands()
rostopics = utils.list_rostopics()
uuid = os.environ.get('UUID', self.id)
msg = {
'uuid': uuid,
'launch_commands': launch_commands,
'rosnodes': rosnode.get_node_names(),
'rosrun_commands': rosrun_commands,
'rowma_ros_version': rowma_ros_version,
'rostopics': rostopics,
'reconnection': self.reconnecting
}
api_key = os.environ.get('API_KEY')
if api_key:
msg['api_key'] = api_key
self.sio.emit('register_robot', json.dumps(msg), namespace=self.nms)
utils.print_success('connection established')
def test_RosPackage_get_depends():
from rospkg import RosPack, ResourceNotFound, get_ros_root
path = get_package_test_path()
r = RosPack(ros_paths=[path])
# test on multiple calls to bad package -- there was an ordering
# issue in the logic that caused get_depends() to return an empty
# set on the second call.
for i in range(1, 4):
try:
r.get_depends("bad", implicit=True)
assert False, "should have raised"
except ResourceNotFound:
pass
# 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_root() and rospack_is_available():
# stress test: test default environment against rospack
r = RosPack()
for p in rospack_list():
retval = set(r.get_depends(p))
rospackval = set(rospack_depends(p))
assert retval == rospackval, "[%s]: %s vs. %s" % (p, retval, rospackval)
def test_list_stacks_by_path(self):
from roslib.stacks import list_stacks_by_path
# test with the ros stack
rr = rospkg.get_ros_root()
self.assertEquals(['ros'], list_stacks_by_path(rr))
stacks = []
self.assertEquals(['ros'], list_stacks_by_path(rr, stacks))
self.assertEquals(['ros'], stacks)
self.assertEquals(['ros'], list_stacks_by_path(rr, stacks))
stacks.extend(['fake_stack', 'fake_stack2'])
self.assertEquals(['ros', 'fake_stack', 'fake_stack2'], list_stacks_by_path(rr, stacks))
cache = {}
self.assertEquals(['ros'], list_stacks_by_path(rr, cache=cache))
self.assertEquals({'ros': rr}, cache)
# test with synthetic stacks
test_dir = os.path.join(roslib.packages.get_pkg_dir('test_roslib'), 'test', 'stack_tests')
self.assertEquals(set(['bar', 'foo']), set(list_stacks_by_path(test_dir)))
test_dir = os.path.join(roslib.packages.get_pkg_dir('test_roslib'), 'test', 'stack_tests', 's1')
self.assertEquals(set(['bar', 'foo']), set(list_stacks_by_path(test_dir)))
test_dir = os.path.join(roslib.packages.get_pkg_dir('test_roslib'), 'test', 'stack_tests', 's1', 'bar')
self.assertEquals(['bar'], list_stacks_by_path(test_dir))
# test symlink following
test_dir = os.path.join(roslib.packages.get_pkg_dir('test_roslib'), 'test', 'stack_tests2')
self.assertEquals(set(['foo', 'bar']), set(list_stacks_by_path(test_dir)))
def test_RosPackage_get_depends():
from rospkg import RosPack, ResourceNotFound, get_ros_root
path = get_package_test_path()
r = RosPack(ros_paths=[path])
# test on multiple calls to bad package -- there was an ordering
# issue in the logic that caused get_depends() to return an empty
# set on the second call.
for i in range(1, 4):
try:
r.get_depends('bad', implicit=True)
assert False, "should have raised"
except ResourceNotFound:
pass
# 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_root() and rospack_is_available():
# stress test: test default environment against rospack
r = RosPack()
for p in rospack_list():
retval = set(r.get_depends(p))
rospackval = set(rospack_depends(p))
assert retval == rospackval, "[%s]: %s vs. %s" % (p, retval,
rospackval)
def test_get_depends_on():
from rospkg import RosPack, get_ros_root
test_dir = get_package_test_path()
rp = RosPack(ros_paths=[test_dir])
# test direct depends
val = rp.get_depends_on("foo", implicit=False)
assert set(["bar", "baz"]) == set(val), val
val = rp.get_depends_on("bar", implicit=False)
assert ["baz"] == val, val
val = rp.get_depends_on("baz", implicit=False)
assert [] == val, val
# test implicit depends
val = rp.get_depends_on("foo", implicit=True)
assert set(["bar", "baz"]) == set(val), val
val = rp.get_depends_on("bar", implicit=True)
assert ["baz"] == val, val
val = rp.get_depends_on("baz", implicit=True)
assert [] == val, val
if get_ros_root() and rospack_is_available():
# stress test: test default environment against rospack
r = RosPack()
for p in rospack_list():
retval = set(r.get_depends_on(p, False))
rospackval = set(rospack_depends_on1(p))
assert retval == rospackval, "[%s]: %s vs. %s" % (p, retval, rospackval)
for p in rospack_list():
retval = set(r.get_depends_on(p, True))
rospackval = set(rospack_depends_on(p))
assert retval == rospackval, "[%s]: %s vs. %s" % (p, retval, rospackval)
def test_get_cwd(self):
test_path = '/this/is/path/to'
result_path = get_cwd('node', '%s/bin' % test_path)
self.assertEqual(
test_path, result_path,
"wrong get_cwd from 'node' type, expected: %s, got: %s" %
(test_path, result_path))
test_path = os.getcwd()
result_path = get_cwd('cwd', '')
self.assertEqual(
test_path, result_path,
"wrong get_cwd from 'cwd' type, expected: %s, got: %s" %
(test_path, result_path))
test_path = rospkg.get_ros_root()
result_path = get_cwd('ros-root', '')
self.assertEqual(
test_path, result_path,
"wrong get_cwd from 'ros-root' type, expected: %s, got: %s" %
(test_path, result_path))
test_path = rospkg.get_ros_home()
result_path = get_cwd('', '')
self.assertEqual(
test_path, result_path,
"wrong get_cwd from empty type, expected: %s, got: %s" %
(test_path, result_path))
def test_get_depends_on():
from rospkg import RosPack, get_ros_root
test_dir = get_package_test_path()
rp = RosPack(ros_paths=[test_dir])
# test direct depends
val = rp.get_depends_on('foo', implicit=False)
assert set(['bar', 'baz']) == set(val), val
val = rp.get_depends_on('bar', implicit=False)
assert ['baz'] == val, val
val = rp.get_depends_on('baz', implicit=False)
assert [] == val, val
# test implicit depends
val = rp.get_depends_on('foo', implicit=True)
assert set(['bar', 'baz']) == set(val), val
val = rp.get_depends_on('bar', implicit=True)
assert ['baz'] == val, val
val = rp.get_depends_on('baz', implicit=True)
assert [] == val, val
if get_ros_root() and rospack_is_available():
# stress test: test default environment against rospack
r = RosPack()
for p in rospack_list():
retval = set(r.get_depends_on(p, False))
rospackval = set(rospack_depends_on1(p))
assert retval == rospackval, "[%s]: %s vs. %s" % (p, retval,
rospackval)
for p in rospack_list():
retval = set(r.get_depends_on(p, True))
rospackval = set(rospack_depends_on(p))
assert retval == rospackval, "[%s]: %s vs. %s" % (p, retval,
rospackval)
def test_create_master_process2(self):
# accidentally wrote two versions of this, need to merge
from roslaunch.core import Master, RLException
import rospkg
from roslaunch.nodeprocess import create_master_process
ros_root = rospkg.get_ros_root()
# test failures
failed = False
try:
create_master_process('runid-unittest', Master.ROSMASTER,
rospkg.get_ros_root(), 0)
failed = True
except RLException:
pass
self.failIf(failed, "invalid port should have triggered error")
# test success with ROSMASTER
m1 = create_master_process('runid-unittest', Master.ROSMASTER,
ros_root, 1234)
self.assertEquals('runid-unittest', m1.run_id)
self.failIf(m1.started)
self.failIf(m1.stopped)
self.assertEquals(None, m1.cwd)
self.assertEquals('master', m1.name)
master_p = 'rosmaster'
self.assert_(master_p in m1.args)
# - it should have the default environment
self.assertEquals(os.environ, m1.env)
# - check args
self.assert_('--core' in m1.args)
# - make sure port arguent is correct
idx = m1.args.index('-p')
self.assertEquals('1234', m1.args[idx + 1])
# test port argument
m2 = create_master_process('runid-unittest', Master.ROSMASTER,
ros_root, 1234)
self.assertEquals('runid-unittest', m2.run_id)
# test ros_root argument
m3 = create_master_process('runid-unittest', Master.ROSMASTER,
ros_root, 1234)
self.assertEquals('runid-unittest', m3.run_id)
master_p = 'rosmaster'
self.assert_(master_p in m3.args)
def test_get_stack_dir(self):
import roslib.packages
from roslib.stacks import get_stack_dir, InvalidROSStackException, list_stacks
self.assertEquals(rospkg.get_ros_root(), get_stack_dir('ros'))
try:
get_stack_dir('non_existent')
self.fail("should have raised")
except roslib.stacks.InvalidROSStackException:
pass
# make sure it agrees with rosstack
stacks = list_stacks()
from roslib.rospack import rosstackexec
for s in stacks:
d1 = get_stack_dir(s)
d2 = rosstackexec(['find', s])
self.assertEquals(d1, d2, "%s vs %s"%(d1, d2))
# now manipulate the environment to test precedence
# - save original RPP as we popen rosstack in other tests
rpp = os.environ.get(rospkg.environment.ROS_PACKAGE_PATH, None)
try:
d = roslib.packages.get_pkg_dir('test_roslib')
d = os.path.join(d, 'test', 'stack_tests')
# - s1/s2/s3
print "s1/s2/s3"
paths = [os.path.join(d, p) for p in ['s1', 's2', 's3']]
os.environ[rospkg.environment.ROS_PACKAGE_PATH] = os.pathsep.join(paths)
# - run multiple times to test caching
for i in xrange(2):
stacks = roslib.stacks.list_stacks()
self.assert_('foo' in stacks)
self.assert_('bar' in stacks)
foo_p = os.path.join(d, 's1', 'foo')
bar_p = os.path.join(d, 's1', 'bar')
self.assertEquals(foo_p, roslib.stacks.get_stack_dir('foo'))
self.assertEquals(bar_p, roslib.stacks.get_stack_dir('bar'))
# - s2/s3/s1
print "s2/s3/s1"
paths = [os.path.join(d, p) for p in ['s2', 's3', 's1']]
os.environ[rospkg.environment.ROS_PACKAGE_PATH] = os.pathsep.join(paths)
stacks = roslib.stacks.list_stacks()
self.assert_('foo' in stacks)
self.assert_('bar' in stacks)
foo_p = os.path.join(d, 's2', 'foo')
bar_p = os.path.join(d, 's1', 'bar')
self.assertEquals(foo_p, roslib.stacks.get_stack_dir('foo'))
self.assertEquals(bar_p, roslib.stacks.get_stack_dir('bar'))
finally:
#restore rpp
if rpp is not None:
os.environ[rospkg.environment.ROS_PACKAGE_PATH] = rpp
else:
del os.environ[rospkg.environment.ROS_PACKAGE_PATH]
def setUp(self):
if 'xylem_DEBUG' in os.environ:
del os.environ['xylem_DEBUG']
self.old_rr = rospkg.get_ros_root()
self.old_rpp = rospkg.get_ros_package_path()
if 'ROS_ROOT' in os.environ:
del os.environ['ROS_ROOT']
os.environ['ROS_PACKAGE_PATH'] = os.path.join(get_test_tree_dir())
def setUp(self):
if 'ROSDEP_DEBUG' in os.environ:
del os.environ['ROSDEP_DEBUG']
self.old_rr = rospkg.get_ros_root()
self.old_rpp = rospkg.get_ros_package_path()
if 'ROS_ROOT' in os.environ:
del os.environ['ROS_ROOT']
os.environ['ROS_PACKAGE_PATH'] = os.path.join(get_test_tree_dir())
def test_get_test_results_dir():
from rospkg import get_ros_root, get_test_results_dir
base = tempfile.gettempdir()
ros_test_results_dir = os.path.join(base, 'ros_test_results_dir')
ros_home_dir = os.path.join(base, 'ros_home_dir')
home_dir = os.path.expanduser('~')
# ROS_TEST_RESULTS_DIR has precedence
env = {'ROS_ROOT': get_ros_root(), 'ROS_TEST_RESULTS_DIR': ros_test_results_dir, 'ROS_HOME': ros_home_dir }
assert ros_test_results_dir == get_test_results_dir(env=env)
env = {'ROS_ROOT': get_ros_root(), 'ROS_HOME': ros_home_dir }
assert os.path.join(ros_home_dir, 'test_results') == get_test_results_dir(env=env)
env = {'ROS_ROOT': get_ros_root()}
assert os.path.join(home_dir, '.ros', 'test_results') == get_test_results_dir(env=env)
# test default assignment of env. Don't both checking return value as we would duplicate get_test_results_dir
assert get_test_results_dir() is not None
def __init__(self):
self.ros_root = rospkg.get_ros_root()
self.rospkg = rospkg.RosPack()
self.pkg_path = self.rospkg.get_path('worldmodel_bullet')
self.urdf_path = os.path.join(self.pkg_path, 'urdf/')
self.tracks_path = os.path.join(self.pkg_path, 'tracks/')
p.connect(p.GUI)
p.resetSimulation()
p.setGravity(0,0,-9.81)
p.setRealTimeSimulation(0)
def __init__(self):
#TODO load classifier
ros_path = rospkg.get_ros_root()
rospack = rospkg.RosPack()
rospath = rospack.get_path('tl_detector')
print("Current path: " + rospath)
self.model = load_model(rospath + '/newdata-resnet-18.h5')
print("Model successfully loaded!")
self.model._make_predict_function()
self.graph = tf.get_default_graph()
def __init__(self):
ros_root = rospkg.get_ros_root()
r = rospkg.RosPack()
path = r.get_path('tl_detector')
print(path)
self.model = load_model(path + '/model.h5')
self.model._make_predict_function()
self.graph = tf.get_default_graph()
pass
def test_create_master_process2(self):
# accidentally wrote two versions of this, need to merge
from roslaunch.core import Master, RLException
import rospkg
from roslaunch.nodeprocess import create_master_process
ros_root = rospkg.get_ros_root()
# test failures
failed = False
try:
create_master_process('runid-unittest', Master.ROSMASTER, rospkg.get_ros_root(), 0)
failed = True
except RLException: pass
self.failIf(failed, "invalid port should have triggered error")
# test success with ROSMASTER
m1 = create_master_process('runid-unittest', Master.ROSMASTER, ros_root, 1234)
self.assertEquals('runid-unittest', m1.run_id)
self.failIf(m1.started)
self.failIf(m1.stopped)
self.assertEquals(None, m1.cwd)
self.assertEquals('master', m1.name)
master_p = 'rosmaster'
self.assert_(master_p in m1.args)
# - it should have the default environment
self.assertEquals(os.environ, m1.env)
# - check args
self.assert_('--core' in m1.args)
# - make sure port arguent is correct
idx = m1.args.index('-p')
self.assertEquals('1234', m1.args[idx+1])
# test port argument
m2 = create_master_process('runid-unittest', Master.ROSMASTER, ros_root, 1234)
self.assertEquals('runid-unittest', m2.run_id)
# test ros_root argument
m3 = create_master_process('runid-unittest', Master.ROSMASTER, ros_root, 1234)
self.assertEquals('runid-unittest', m3.run_id)
master_p = 'rosmaster'
self.assert_(master_p in m3.args)
def configure_logging(logname, level=logging.INFO, filename=None, env=None):
"""
Configure Python logging package to send log files to ROS-specific log directory
@param logname str: name of logger
@type logname: str
@param filename: filename to log to. If not set, a log filename
will be generated using logname
@type filename: str
@param env: override os.environ dictionary
@type env: dict
@return: log file name
@rtype: str
@raise roslib.exceptions.ROSLibException: if logging cannot be configured as specified
"""
if env is None:
env = os.environ
logname = logname or 'unknown'
log_dir = get_log_dir(env=env)
# if filename is not explicitly provided, generate one using logname
if not filename:
log_filename = os.path.join(log_dir, '%s-%s.log'%(logname, os.getpid()))
else:
log_filename = os.path.join(log_dir, filename)
logfile_dir = os.path.dirname(log_filename)
if not os.path.exists(logfile_dir):
try:
makedirs_with_parent_perms(logfile_dir)
except OSError:
# cannot print to screen because command-line tools with output use this
sys.stderr.write("WARNING: cannot create log directory [%s]. Please set %s to a writable location.\n"%(logfile_dir, ROS_LOG_DIR))
return None
elif os.path.isfile(logfile_dir):
raise roslib.exceptions.ROSLibException("Cannot save log files: file [%s] is in the way"%logfile_dir)
if 'ROS_PYTHON_LOG_CONFIG_FILE' in os.environ:
config_file = os.environ['ROS_PYTHON_LOG_CONFIG_FILE']
else:
config_file = os.path.join(get_ros_root(env=env), 'config', 'python_logging.conf')
if not os.path.isfile(config_file):
# logging is considered soft-fail
sys.stderr.write("WARNING: cannot load logging configuration file, logging is disabled\n")
return log_filename
# pass in log_filename as argument to pylogging.conf
os.environ['ROS_LOG_FILENAME'] = log_filename
# #3625: disabling_existing_loggers=False
logging.config.fileConfig(config_file, disable_existing_loggers=False)
return log_filename
def setUp(self):
if 'ROSDEP_DEBUG' in os.environ:
del os.environ['ROSDEP_DEBUG']
self.old_rr = rospkg.get_ros_root()
self.old_rpp = rospkg.get_ros_package_path()
self.old_app = os.getenv(AMENT_PREFIX_PATH_ENV_VAR, None)
if 'ROS_ROOT' in os.environ:
del os.environ['ROS_ROOT']
os.environ['ROS_PACKAGE_PATH'] = os.path.join(get_test_tree_dir())
os.environ[AMENT_PREFIX_PATH_ENV_VAR] = os.path.join(get_test_tree_dir(), 'ament')
if 'ROS_PYTHON_VERSION' not in os.environ:
# avoid `test_check` failure due to warning on stderr
os.environ['ROS_PYTHON_VERSION'] = sys.version[0]
def handle_inference(self,req):
print req.model
print req.num_new_clusters
print req.num_old_clusters
print req.similarAppearance
print req.similarPose
print req.out_of_view
print req.queries
self.create_db(req)
ros_root = rospkg.get_ros_root()
r = rospkg.RosPack()
depends = r.get_depends('roscpp')
path = r.get_path('rospy')
model_dir = r.get_path('entres')
model_file = os.path.join (model_dir, "models" ,req.model)
if not os.path.exists (model_file):
raise Exception ("model file '%s' does not exist" % model_file)
return
input_files = [model_file]
db = ""
with tempfile.NamedTemporaryFile(mode='w', prefix='tmp_er_', delete=False) as f:
for line in self.db:
f.write(line)
db = f.name
method = "MC-SAT"
params = "{}"
self.settings = {"cwPreds":"outOfView"}
try:
results = self.inference.run(input_files, db, method, req.queries, params=params, **self.settings)
except:
os.remove(db)
cls, e, tb = sys.exc_info()
sys.stderr.write("Error: %s\n" % str(e))
traceback.print_tb(tb)
raise Exception ("MLN inference failed: %s" % str (e))
return
os.remove(db)
return self.parse_results(results, req)
def get_ros_root(required=False, env=None):
"""
Get the value of ROS_ROOT.
@param env: override environment dictionary
@type env: dict
@param required: if True, fails with ROSException
@return: Value of ROS_ROOT environment
@rtype: str
@raise ROSException: if require is True and ROS_ROOT is not set
"""
if env is None:
env = os.environ
ros_root = rospkg.get_ros_root(env)
if required and not ros_root:
raise rospy.exceptions.ROSException('%s is not set'%rospkg.environment.ROS_ROOT)
return ros_root
def get_ros_root(required=False, env=None):
"""
Get the value of ROS_ROOT.
@param env: override environment dictionary
@type env: dict
@param required: if True, fails with ROSException
@return: Value of ROS_ROOT environment
@rtype: str
@raise ROSException: if require is True and ROS_ROOT is not set
"""
if env is None:
env = os.environ
ros_root = rospkg.get_ros_root(env)
if required and not ros_root:
raise rospy.exceptions.ROSException('%s is not set'%rospkg.environment.ROS_ROOT)
return ros_root
def test_RosPackage_get_depends_explicit():
from rospkg import RosPack, get_ros_root
path = get_package_test_path()
r = RosPack(ros_paths=[path])
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) == []
if get_ros_root() and rospack_is_available():
# stress test: test default environment against rospack
r = RosPack()
for p in rospack_list():
retval = set(r.get_depends(p, implicit))
rospackval = set(rospack_depends1(p))
assert retval == rospackval, "[%s]: %s vs. %s" % (p, retval, rospackval)
def get_cwd(cwd, binary=''):
result = ''
if cwd == 'node':
result = os.path.dirname(binary)
elif cwd == 'cwd':
result = os.getcwd()
elif cwd == 'ros-root':
result = rospkg.get_ros_root()
else:
result = rospkg.get_ros_home()
if not os.path.exists(result):
try:
os.makedirs(result)
except OSError:
# exist_ok=True
pass
return result
def test_RosPackage_get_depends_explicit():
from rospkg import RosPack, get_ros_root
path = get_package_test_path()
r = RosPack(ros_paths=[path])
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) == []
if get_ros_root() and rospack_is_available():
# stress test: test default environment against rospack
r = RosPack()
for p in rospack_list():
retval = set(r.get_depends(p, implicit))
rospackval = set(rospack_depends1(p))
assert retval == rospackval, "[%s]: %s vs. %s"%(p, retval, rospackval)
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 rospackexec(args):
"""
@return: result of executing rospack command (via subprocess). string will be strip()ed.
@rtype: str
@raise roslib.exceptions.ROSLibException: if rospack command fails
"""
rospack_bin = os.path.join(rospkg.get_ros_root(), "bin", "rospack")
if python3:
val = subprocess.Popen([rospack_bin] + args, stdout=subprocess.PIPE, stderr=subprocess.PIPE).communicate()[0]
val = val.decode().strip()
else:
val = (
subprocess.Popen([rospack_bin] + args, stdout=subprocess.PIPE, stderr=subprocess.PIPE).communicate()[0]
or ""
).strip()
if val.startswith("rospack:"): # rospack error message
raise roslib.exceptions.ROSLibException(val)
return val
def __init__(self):
self.signal_classes = ['Red', 'Green', 'Yellow']
self.signal_status = None
self.tl_box = None
ros_root = rospkg.get_ros_root()
r = rospkg.RosPack()
path = r.get_path('tl_detector')
self.cls_model = load_model(path + '/model_f_lr-4_ep140_ba32.h5')
#'ssd_mobilenet_v1_coco_11_06_2017'
PATH_TO_CKPT = path + '/frozen_inference_graph.pb'
self.detection_graph = tf.Graph()
config = tf.ConfigProto()
config.gpu_options.allow_growth = True
with self.detection_graph.as_default():
od_graph_def = tf.GraphDef()
with tf.gfile.GFile(PATH_TO_CKPT, 'rb') as fid:
serialized_graph = fid.read()
od_graph_def.ParseFromString(serialized_graph)
tf.import_graph_def(od_graph_def, name='')
self.sess = tf.Session(graph=self.detection_graph, config=config)
self.image_tensor = self.detection_graph.get_tensor_by_name(
'image_tensor:0')
self.boxes = self.detection_graph.get_tensor_by_name(
'detection_boxes:0')
self.scores = self.detection_graph.get_tensor_by_name(
'detection_scores:0')
self.classes = self.detection_graph.get_tensor_by_name(
'detection_classes:0')
self.num_detections = self.detection_graph.get_tensor_by_name(
'num_detections:0')
def test_RosPack_get_path():
from rospkg import RosPack, ResourceNotFound, get_ros_root
path = get_package_test_path()
foo_path = os.path.join(path, 'p1', 'foo')
foo_path_alt = os.path.join(path, 'p2', 'foo')
bar_path = os.path.join(path, 'p1', 'bar')
baz_path = os.path.join(path, 'p2', 'baz')
# point ROS_ROOT at top, should spider entire tree
print("ROS path: %s" % (path))
r = RosPack(ros_paths=[path])
# precedence in this case is undefined as there are two 'foo's in the same path
assert r.get_path('foo') in [foo_path, foo_path_alt]
assert bar_path == r.get_path('bar')
assert baz_path == r.get_path('baz')
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')))
r = RosPack(ros_paths=[os.path.join(path, 'p1'), os.path.join(path, 'p2')])
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')
if get_ros_root() and rospack_is_available():
# stresstest against rospack
r = RosPack()
for p in rospack_list():
retval = r.get_path(p)
rospackval = rospack_find(p)
assert retval == rospackval, "[%s]: %s vs. %s" % (p, retval,
rospackval)
def test_RosPack_get_path():
from rospkg import RosPack, ResourceNotFound, get_ros_root
path = get_package_test_path()
foo_path = os.path.join(path, "p1", "foo")
foo_path_alt = os.path.join(path, "p2", "foo")
bar_path = os.path.join(path, "p1", "bar")
baz_path = os.path.join(path, "p2", "baz")
# point ROS_ROOT at top, should spider entire tree
print("ROS path: %s" % (path))
r = RosPack(ros_paths=[path])
# precedence in this case is undefined as there are two 'foo's in the same path
assert r.get_path("foo") in [foo_path, foo_path_alt]
assert bar_path == r.get_path("bar")
assert baz_path == r.get_path("baz")
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")))
r = RosPack(ros_paths=[os.path.join(path, "p1"), os.path.join(path, "p2")])
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")
if get_ros_root() and rospack_is_available():
# stresstest against rospack
r = RosPack()
for p in rospack_list():
retval = r.get_path(p)
rospackval = rospack_find(p)
assert retval == rospackval, "[%s]: %s vs. %s" % (p, retval, rospackval)
frame("Package Info", 0.1)
####################### Query user for details ###########################
## Package name ##
pkg = raw_input('Enter your package name: default [' + pkg + ']:') or pkg
my_rospkg = ros_package(pkg, rws, ros_build_system)
my_rospkg.create_package_folder()
frame("Ros Environment: ", 0.2)
text.rws = rws
####################### Setup and Check the ROS environment #######################
rosOK = 0
## test for proper ROS environment setting of the package using rospkg library ##
try:
print 'Testing ros core setup.'
ros_root = rospkg.get_ros_root()
rp = rospkg.RosPack()
path = rp.get_path(pkg)
print 'Your package path is: ' + path
print 'Ros system working properly!'
rosOK = 1
except Exception as exc:
name_of_exception = type(exc).__name__
print 'Could not "rospack.get_path" of package: ' + pkg
print ' in ROS workspace: ' + rws
print 'Ros exception: ' + name_of_exception
# here we try to fix up ROS environment using shell commands
# which are in rosstart_bash.txt
print ''' I'm trying to get ROS fixed ...'''
with open('templates/rosstart_bash.txt') as ros_script:
script = ros_script.readlines()
def get_classification(self, image_np):
"""Determines the color of the traffic light in the image
Args:
image_np (cv::Mat): image containing the traffic light
Returns:
int: ID of traffic light color (specified in styx_msgs/TrafficLight)
"""
# Tensorflow Model Object Detection API classifier.
# See: https://github.com/tensorflow/models/blob/master/research/object_detection/object_detection_tutorial.ipynb
# Transfer learning with SSD Mobile Net + Coco data
# Trained on data from: https://drive.google.com/file/d/0B-Eiyn-CUQtxdUZWMkFfQzdObUE/view
# we thank Anthony S. for the annotations to the bag data made available through link in slack channel for this project
# using these feature set sepcs:
# https://github.com/tensorflow/models/blob/master/research/object_detection/g3doc/using_your_own_dataset.md
ros_root = rospkg.get_ros_root()
r = rospkg.RosPack()
path = r.get_path('tl_detector')
# set up tensorflow and traffic light classifier
if self.tf_session is None:
# get the traffic light classifier
self.config = tf.ConfigProto(log_device_placement=True)
self.config.gpu_options.per_process_gpu_memory_fraction = 0.5 # don't hog all the VRAM!
self.config.operation_timeout_in_ms = 50000 # terminate anything that don't return in 50 seconds
self.tf_graph = load_graph(path + self.model_path + '.pb')
with self.tf_graph.as_default():
self.tf_session = tf.Session(graph=self.tf_graph,
config=self.config)
# Definite input and output Tensors for self.tf_graph
self.image_tensor = self.tf_graph.get_tensor_by_name(
'prefix/image_tensor:0')
# Each score represent how level of confidence for each of the objects.
# Score is shown on the result image, together with the class label.
self.detection_scores = self.tf_graph.get_tensor_by_name(
'prefix/detection_scores:0')
self.detection_classes = self.tf_graph.get_tensor_by_name(
'prefix/detection_classes:0')
self.num_detections = self.tf_graph.get_tensor_by_name(
'prefix/num_detections:0')
self.predict = True
predict = TrafficLight.UNKNOWN
if self.predict is not None:
# Expand dimensions since the model expects images to have shape: [1, None, None, 3]
image_np_expanded = np.expand_dims(image_np, axis=0)
# Actual detection
(scores, classes, num) = self.tf_session.run(
[
self.detection_scores, self.detection_classes,
self.num_detections
],
feed_dict={self.image_tensor: image_np_expanded})
# Visualization of the results of a detection.
scores = np.squeeze(scores)
classes = np.squeeze(classes).astype(np.int32)
# calculate prediction
c = 5
predict = self.clabels[c]
cc = classes[0]
confidence = scores[0]
if cc > 0 and cc < 4 and confidence is not None and confidence > THRESHOLD:
c = cc
predict = self.clabels[c]
print('Confidence of state prediction: ', confidence)
return predict
def get_classification(self, image_np):
"""Determines the color of the traffic light in the image
Args:
image_np (cv::Mat): image containing the traffic light
Returns:
int: ID of traffic light color (specified in styx_msgs/TrafficLight)
"""
ros_root = rospkg.get_ros_root()
r = rospkg.RosPack()
path = r.get_path('tl_detector')
print(path)
if self.tf_session is None:
#setup the tensorflow environment
self.config = tf.ConfigProto(log_device_placement=True)
self.config.gpu_options.per_process_gpu_memory_fraction = 0.5
self.config.operation_timeout_in_ms = 50000
self.tf_graph = tf.Graph()
with self.tf_graph.as_default():
od_graph_def = tf.GraphDef()
# read model from the model file
with tf.gfile.GFile(path + '/frozen_inference_graph.pb',
'rb') as fid:
serialized_graph = fid.read()
od_graph_def.ParseFromString(serialized_graph)
tf.import_graph_def(od_graph_def, name='')
self.tf_session = tf.Session(graph=self.tf_graph,
config=self.config)
# get the placeholder of input image, output scores, classes.
self.image_tensor = self.tf_graph.get_tensor_by_name(
'image_tensor:0')
self.detection_scores = self.tf_graph.get_tensor_by_name(
'detection_scores:0')
self.detection_classes = self.tf_graph.get_tensor_by_name(
'detection_classes:0')
self.num_detections = self.tf_graph.get_tensor_by_name(
'num_detections:0')
self.predict = True
predict = TrafficLight.UNKNOWN
if self.predict is not None:
# expand the image dimension
image_np_expanded = np.expand_dims(image_np, axis=0)
# Run the actual detection
(scores, classes, num) = self.tf_session.run(
[
self.detection_scores, self.detection_classes,
self.num_detections
],
feed_dict={self.image_tensor: image_np_expanded})
# reduce the dimensions
scores = np.squeeze(scores)
classes = np.squeeze(classes).astype(np.int32)
# calculate prediction
c = 5
predict = self.clabels[c]
cc = classes[0]
confidence = scores[0]
print('id:', cc)
print('confidence:', confidence)
print('predict is:', predict)
if cc > 0 and cc < 4 and confidence is not None and confidence > THRESHOLD:
c = cc
predict = self.clabels[c]
print('Light:', predict)
return predict
####################### Query user for details ###########################
## Package name ##
pkg = raw_input('Enter your package name: default [' + pkg +']:') or pkg
my_rospkg = ros_package(pkg,rws,ros_build_system)
my_rospkg.create_package_folder()
frame("Ros Environment: ", 0.2)
text.rws = rws
####################### Setup and Check the ROS environment #######################
rosOK = 0
## test for proper ROS environment setting of the package using rospkg library ##
try:
print 'Testing ros core setup.'
ros_root = rospkg.get_ros_root()
rp = rospkg.RosPack()
path = rp.get_path(pkg)
print 'Your package path is: '+ path
print 'Ros system working properly!'
rosOK = 1
except Exception as exc:
name_of_exception = type(exc).__name__
print 'Could not "rospack.get_path" of package: '+pkg
print ' in ROS workspace: '+rws
print 'Ros exception: '+name_of_exception
# here we try to fix up ROS environment using shell commands
# which are in rosstart_bash.txt
print ''' I'm trying to get ROS fixed ...'''
with open('templates/rosstart_bash.txt') as ros_script:
script = ros_script.readlines()
def main(urdf_file):
proc = subprocess.Popen(['./run_xacro.sh'])
proc.wait()
robot = URDF.from_xml_file(os.path.join(os.path.dirname(__file__),'../world_model/kinova_fel_with_objects.urdf'))
assert isinstance(robot, URDF)
r = rospkg.RosPack()
ros_root = rospkg.get_ros_root()
# path = r.get_path('rospy')
meshes = []
for l in robot.links:
assert isinstance(l, Link)
for visual in l.visuals + l.collisions: # type: Visual
if isinstance(visual.geometry, Mesh):
meshes.append(visual.geometry.filename)
meshes = list(set(meshes))
dst_path = os.path.join(os.path.dirname(__file__), "../meshes")
for path in meshes:
assert (isinstance(path, str))
rel_path = path.replace('package://', '', 1)
pkg_name = rel_path.split('/')[0]
rel_path = rel_path.replace(pkg_name, '', 1)
pkg_path = r.get_path(pkg_name)
mesh_path_abs = os.path.join(pkg_path, rel_path.replace('/', '', 1))
shutil.copy(mesh_path_abs, dst_path)
mesh_name = mesh_path_abs.split('/')[-1]
if '.dae' in mesh_name:
dst_file_path = os.path.join(dst_path, mesh_name.replace('.dae', '.STL'))
mesh = trimesh.load(mesh_path_abs)
if isinstance(mesh, trimesh.Scene):
mesh = mesh.geometry.values()[0]
mesh.export(file_obj=dst_file_path, file_type='stl')
# for l in robot.links:
# assert isinstance(l, Link)
# for visual in l.visuals + l.collisions: # type: Visual
# if isinstance(visual.geometry, Mesh) and '.dae' in visual.geometry.filename:
# assert isinstance(visual.geometry.filename, str)
# filename = visual.geometry.filename.replace('.dae', '.stl')
# visual.geometry.filename = filename
# robot = URDF.from_xml_file(os.path.join(os.path.dirname(__file__),'../world_model/kinova_fel_with_objects.urdf'))
file_object = open(os.path.join(os.path.dirname(__file__),'../world_model/kinova_fel_with_objects.urdf'), 'r+')
lines = file_object.readlines()
file_object.close()
res_str = "package://kinova_mujoco/"
# ' <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/base_link.STL"/>'
new_lines = []
for line in lines:
# take over comment lines unchanged
if '<!--' in line:
new_lines.append(line + "\n")
elif '<mesh filename="package://' in line:
# write new stl mesh location in robot mujoco package
link_name = line.split('/')[-2]
if 'scale' in link_name:
pass
# link_name.
new_line = line.split('//')[0] + '//' + 'kinova_mujoco/meshes/' + link_name.replace('.dae', '.STL') + '/>'
# line = line.replace('.dae', '.stl')
new_lines.append(new_line + "\n")
elif '<material name="">' in line:
# mujoco wants everything to have a filled material tag
new_line = line.replace('""', '"DUMMY_MATERIAL"')
new_lines.append(new_line + "\n")
elif '<mimic joint=' in line:
# mujoco does not support mimic joints. we have to use a custom controller to make the mimic functionality.
pass
else:
# take over normal lines
new_lines.append(line + "\n")
file_object = open(os.path.join(os.path.dirname(__file__),'../world_model/kinova_fel_with_objects_NoDae.urdf'), 'w+')
file_object.writelines(new_lines)
file_object.close()
print("robot")
