def test_with_sourcepath(self):
def create_mock_workspace(root_dir, ws):
ws1 = os.path.join(root_dir, ws)
p1 = os.path.join(ws1, "p1")
p1inc = os.path.join(p1, "include")
p1share = os.path.join(p1, "share")
os.makedirs(ws1)
os.makedirs(p1)
os.makedirs(p1inc)
os.makedirs(p1share)
with open(os.path.join(ws1, CATKIN_WORKSPACE_MARKER_FILE), 'w') as fhand:
fhand.write('loc1;loc2')
try:
root_dir = tempfile.mkdtemp()
create_mock_workspace(root_dir, 'ws1')
create_mock_workspace(root_dir, 'ws2')
(existing, paths) = find_in_workspaces(['share', 'etc'], 'foo', 'foopath', _workspaces=[os.path.join(root_dir, 'ws1')])
self.assertEqual([os.path.join(root_dir, 'ws1', 'share', 'foo', 'foopath'),
'loc1/foo/foopath',
'loc2/foo/foopath',
os.path.join(root_dir, 'ws1', 'etc', 'foo', 'foopath')], paths)
self.assertEqual([], existing)
(existing, paths) = find_in_workspaces(['share', 'etc'], 'p1', None, _workspaces=[os.path.join(root_dir, 'ws1')])
self.assertEqual([os.path.join(root_dir, 'ws1', 'share', 'p1'),
'loc1/p1',
'loc2/p1',
os.path.join(root_dir, 'ws1', 'etc', 'p1')], paths)
self.assertEqual([], existing)
finally:
shutil.rmtree(root_dir)
def _call_mutate(self, sudo, installation_files):
try:
# Installed script location
mutate_files_exec = find_in_workspaces(project="robot_upstart",
path="mutate_files",
first_match_only=True)[0]
except IndexError:
# Devel script location
mutate_files_exec = find_in_workspaces(project="robot_upstart",
path="scripts/mutate_files",
first_match_only=True)[0]
# If sudo is specified, then the user will be prompted at this point.
cmd = [mutate_files_exec]
if sudo:
cmd.insert(0, sudo)
print "Now calling: %s" % ' '.join(cmd)
p = subprocess.Popen(cmd + [pickle.dumps(installation_files)])
p.communicate()
if p.returncode == 0:
print "Filesystem operation succeeded."
else:
print "Error encountered; filesystem operation aborted."
return p.returncode
def test_find_in_workspaces(self):
(existing, paths) = find_in_workspaces([], _workspaces=None)
self.assertEqual(([], []), (existing, paths))
(existing, paths) = find_in_workspaces([], 'foo', _workspaces=None)
self.assertEqual(([], []), (existing, paths))
(existing, paths) = find_in_workspaces([], 'foo', 'foopath', _workspaces=None)
self.assertEqual(([], []), (existing, paths))
(existing, paths) = find_in_workspaces(['include'], 'foo', 'foopath', _workspaces=None)
self.assertEqual(([], []), (existing, paths))
(existing, paths) = find_in_workspaces(['include'], 'foo', 'foopath', _workspaces=['bar'])
self.assertEqual(([], ['bar/include/foo/foopath']), (existing, paths))
(existing, paths) = find_in_workspaces(['include'], 'foo', 'foopath', _workspaces=['bar', 'baz'])
self.assertEqual(([], ['bar/include/foo/foopath', 'baz/include/foo/foopath']), (existing, paths))
(existing, paths) = find_in_workspaces(['include', 'etc', 'libexec'], 'foo', 'foopath', _workspaces=['bar', 'baz'])
self.assertEqual(([], ['bar/include/foo/foopath',
'bar/etc/foo/foopath',
'bar/lib/foo/foopath',
'baz/include/foo/foopath',
'baz/etc/foo/foopath',
'baz/lib/foo/foopath']), (existing, paths))
(existing, paths) = find_in_workspaces(['share', 'etc', 'lib'], None, 'foopath', _workspaces=['bar', 'baz'])
self.assertEqual(([], ['bar/share/foopath',
'bar/etc/foopath',
'bar/lib/foopath',
'baz/share/foopath',
'baz/etc/foopath',
'baz/lib/foopath']), (existing, paths))
(existing, paths) = find_in_workspaces(None, None, None, _workspaces=['bar'])
self.assertEqual(([], ['bar/bin', 'bar/etc', 'bar/include', 'bar/lib', 'bar/share']), (existing, paths))
def _find_executable(resolved, a, args, _context):
"""
process $(find-executable PKG PATH)
It finds the executable with the basename(PATH) in the libexec folder
or under the PATH relative to the package.xml file.
:returns: updated resolved argument, ``str``
:raises: :exc:SubstitutionException: if PKG/PATH invalidly specified or executable is not found for PKG
"""
if len(args) != 2:
raise SubstitutionException("$(find-executable pkg path) command only accepts two argument [%s]" % a)
before, after = _split_command(resolved, a)
path = _sanitize_path(args[1])
# we try to find the specific executable in libexec via catkin
# which will search in install/devel space
full_path = None
from catkin.find_in_workspaces import find_in_workspaces
paths = find_in_workspaces(['libexec'], project=args[0], first_matching_workspace_only=True) # implicitly first_match_only=True
if paths:
full_path = _get_executable_path(paths[0], os.path.basename(path))
if not full_path:
# else we will look for the executable in the source folder of the package
rp = _get_rospack()
full_path = _get_executable_path(rp.get_path(args[0]), path)
if not full_path:
raise SubstitutionException("$(find-executable pkg path) could not find executable [%s]" % a)
return before + full_path + after
def add(self, package=None, filename=None, glob=None):
""" Add launch or other configuration files to Job.
Files may be specified using relative, absolute, or package-relative
paths. Files may also be specified using shell globs.
:param package: Optionally specify a package to search for the file
or glob relative-to.
:type package: str
:param filename: Name of a file to add to the job. Relative to the
package path, if specified.
:type filename: str
:param glob: Shell glob of files to add to the job. Relative to the
package path, if specified.
:type glob: str
"""
if package:
search_paths = reversed(find_in_workspaces(project=package))
else:
search_paths = ('.',)
if glob and filename:
raise RuntimeError("You must specify only an exact filename or a glob, not both.")
# See: https://docs.python.org/2/library/os.html#os.getlogin
if filename:
for path in search_paths:
candidate = os.path.join(path, filename)
if os.path.isfile(candidate):
self.files.append(candidate)
if glob:
for path in search_paths:
self.files.extend(glob_files(os.path.join(path, glob)))
def _fill_template(self, template):
self.interpreter.output = StringIO.StringIO()
self.interpreter.reset()
with open(find_in_workspaces(project="vector_upstart", path=template)[0]) as f:
self.interpreter.file(f)
return self.interpreter.output.getvalue()
self.set_job_path()
def add_morsel(self, morsel_in_camera, morsel_name=None):
camera_in_world = self.robot.GetLink(
'Camera_Depth_Frame').GetTransform()
morsel_in_world = numpy.dot(camera_in_world, morsel_in_camera)
import openravepy
h1 = openravepy.misc.DrawAxes(self.env, camera_in_world)
h2 = openravepy.misc.DrawAxes(self.env, morsel_in_world)
if morsel_name is None:
morsel_name = 'morsel'
object_base_path = find_in_workspaces(search_dirs=['share'],
project='ada_meal_scenario',
path='data',
first_match_only=True)[0]
ball_path = os.path.join(object_base_path, 'objects',
'smallsphere.kinbody.xml')
if self.env.GetKinBody(morsel_name) is None:
with self.env:
morsel = self.env.ReadKinBodyURI(ball_path)
morsel.SetName(morsel_name)
self.env.Add(morsel)
morsel.Enable(False)
else:
morsel = self.env.GetKinBody(morsel_name)
morsel.SetTransform(morsel_in_world)
def interpret_path(path, pwd='.'):
'''
Tries to determine the path of included file. The statement of $(find 'package') will be resolved.
:param str path: the sting which contains the included path
:param str pwd: current working path
:return: `$(find 'package')` will be resolved. The prefixes `file://`, `package://` or `pkg://` are also resolved.
Otherwise the parameter itself normalized by :py:func:`os.path.normpath` will be returned.
:rtype: str
'''
result = path.strip()
# try replace package name by package path
pkg_pattern = re.compile(
r"\$\(find (.*?)\)/|pkg:\/\/(.*?)/|package:\/\/(.*?)/")
for groups in pkg_pattern.finditer(path):
for index in range(groups.lastindex):
pkg_name = groups.groups()[index]
if pkg_name:
pkg = get_pkg_path(pkg_name)
rospy.logdebug("rospkg.RosPack.get_path for '%s': %s" %
(pkg_name, pkg))
path_suffix = path[groups.end():].rstrip("'")
if path_suffix.startswith('/'):
paths = roslib.packages._find_resource(
pkg, path_suffix.strip(os.path.sep))
rospy.logdebug(
" search for resource with roslib.packages._find_resource, suffix '%s': %s"
% (path_suffix.strip(os.path.sep), paths))
if len(paths) > 0:
# if more then one launch file is found, take the first one
return paths[0]
full_path = os.path.normpath(os.path.join(pkg, path_suffix))
if path_suffix:
if not os.path.exists(full_path):
# we try to find the specific path in share via catkin
# which will search in install/devel space and the source folder of the package
try:
from catkin.find_in_workspaces import find_in_workspaces
global SOURCE_PATH_TO_PACKAGES
paths = find_in_workspaces(
['share'],
project=pkg_name,
path=path_suffix.strip(os.path.sep),
first_matching_workspace_only=True,
first_match_only=True,
source_path_to_packages=SOURCE_PATH_TO_PACKAGES
)
if paths:
return paths[0]
except Exception:
import traceback
rospy.logwarn(
"search in install/devel space failed: %s" %
traceback.format_exc())
return full_path
else:
return "%s%s" % (os.path.normpath(pkg), os.path.sep)
if path.startswith('file://'):
result = path[7:]
return os.path.normpath(os.path.join(pwd, result))
def plotMug(env):
# load table into environment
objects_path = find_in_workspaces(
project='iact_control',
path='src/data',
first_match_only=True)[0]
env.Load('{:s}/mug.xml'.format(objects_path))
def _fill_template(self, template):
self.interpreter.output = StringIO.StringIO()
self.interpreter.reset()
with open(find_in_workspaces(project="robot_upstart", path=template)[0]) as f:
self.interpreter.file(f)
return self.interpreter.output.getvalue()
self.set_job_path()
def _find_executable(resolved, a, args, _context):
"""
process $(find-executable PKG PATH)
It finds the executable with the basename(PATH) in the libexec folder
or under the PATH relative to the package.xml file.
:returns: updated resolved argument, ``str``
:raises: :exc:SubstitutionException: if PKG/PATH invalidly specified or executable is not found for PKG
"""
if len(args) != 2:
raise SubstitutionException("$(find-executable pkg path) command only accepts two argument [%s]" % a)
before, after = _split_command(resolved, a)
path = _sanitize_path(args[1])
# we try to find the specific executable in libexec via catkin
# which will search in install/devel space
full_path = None
from catkin.find_in_workspaces import find_in_workspaces
paths = find_in_workspaces(['libexec'], project=args[0], first_matching_workspace_only=True) # implicitly first_match_only=True
if paths:
full_path = _get_executable_path(paths[0], os.path.basename(path))
if not full_path:
# else we will look for the executable in the source folder of the package
rp = _get_rospack()
full_path = _get_executable_path(rp.get_path(args[0]), path)
if not full_path:
raise SubstitutionException("$(find-executable pkg path) could not find executable [%s]" % a)
return before + full_path + after
def _find_resource(resolved, a, args, _context, source_path_to_packages=None):
"""
process $(find-resource PKG PATH)
Resolves the relative PATH from the share folder of the PKG either from install space, devel space or from the source folder.
:returns: updated resolved argument, ``str``
:raises: :exc:SubstitutionException: if PKG and PATH invalidly specified or relative PATH is not found for PKG
"""
if len(args) != 2:
raise SubstitutionException(
"$(find-resource pkg path) command only accepts two argument [%s]"
% a)
before, after = _split_command(resolved, a)
path = _sanitize_path(args[1])
# we try to find the specific path in share via catkin
# which will search in install/devel space and the source folder of the package
from catkin.find_in_workspaces import find_in_workspaces
paths = find_in_workspaces(['share'],
project=args[0],
path=path,
first_matching_workspace_only=True,
first_match_only=True,
source_path_to_packages=source_path_to_packages)
if not paths:
raise SubstitutionException(
"$(find-resource pkg path) could not find path [%s]" % a)
return before + paths[0] + after
def test_no_initialize(self):
paths = find_in_workspaces(search_dirs=['libexec'], project=PKG, path='test_publisher_no_initialize')
self.assertEquals(len(paths), 1)
cmd = paths[0]
p = Popen([cmd], stdout=PIPE, stderr=PIPE)
p.communicate()
self.assertEquals(p.returncode, 0)
def add(self, package=None, filename=None, glob=None):
""" Add launch or other configuration files to Job.
Files may be specified using relative, absolute, or package-relative
paths. Files may also be specified using shell globs.
:param package: Optionally specify a package to search for the file
or glob relative-to.
:type package: str
:param filename: Name of a file to add to the job. Relative to the
package path, if specified.
:type filename: str
:param glob: Shell glob of files to add to the job. Relative to the
package path, if specified.
:type glob: str
"""
if package:
search_paths = reversed(find_in_workspaces(project=package))
else:
search_paths = ('.',)
if glob and filename:
raise RuntimeError("You must specify only an exact filename or a glob, not both.")
# See: https://docs.python.org/2/library/os.html#os.getlogin
if filename:
for path in search_paths:
candidate = os.path.join(path, filename)
if os.path.isfile(candidate):
self.files.append(candidate)
if glob:
for path in search_paths:
self.files.extend(glob_files(os.path.join(path, glob)))
def main():
""" Implementation of the ``install`` script."""
args = get_argument_parser().parse_args()
pkg, pkgpath = args.pkgpath[0].split('/', 1)
job_name = args.job or pkg.split('_', 1)[0]
# Any unspecified arguments are on the args object as None. These are filled
# in by the Job constructor when passed as Nones.
j = robot_upstart.Job(name=job_name,
interface=args.interface,
user=args.user,
workspace_setup=args.setup,
rosdistro=args.rosdistro,
master_uri=args.master,
log_path=args.logdir,
systemd_after=args.systemd_after)
for this_pkgpath in args.pkgpath:
pkg, pkgpath = this_pkgpath.split('/', 1)
if not pkg:
print(
"Unable to locate package your job launch is in."
" Installation aborted. " + DESC_PKGPATH +
"\npkgpath passed: {}.".format(pkgpath))
return 1
found_path = find_in_workspaces(project=pkg,
path=pkgpath,
first_match_only=True)
if not found_path:
print "Unable to locate path %s in package %s. Installation aborted." % (
pkgpath, pkg)
return 1
if os.path.isfile(found_path[0]):
# Single file, install just that.
j.add(package=pkg, filename=pkgpath)
else:
# Directory found, install everything within.
j.add(package=pkg, glob=os.path.join(pkgpath, "*"))
if args.augment:
j.generate_system_files = False
if args.wait:
j.roslaunch_wait = True
provider = providers.detect_provider()
if args.provider == 'upstart':
provider = providers.Upstart
if args.provider == 'systemd':
provider = providers.Systemd
if args.symlink:
j.symlink = True
j.install(Provider=provider)
return 0
def run(self):
rospy.init_node('rospilot_mapnik_server')
os.environ['PGUSER'] = '******'
os.environ['PGPASSWORD'] = '******'
os.environ['PGHOST'] = 'localhost'
style_file = find_in_workspaces(['share'],
'rospilot',
'share/mapnik-style/style.xml',
first_match_only=True)
if not style_file:
rospy.logfatal("Cannot find share/mapnik-style/style.xml")
else:
style_file = style_file[0]
config = expanduser(rospy.get_param('~tilestache_config_file'))
# XXX: Monkey patch Mapnik because Tilestache doesn't work with the newest version
mapnik.FontEngine.instance = staticmethod(lambda: mapnik.FontEngine)
config = buildConfiguration(
{
"cache": {
"name": "Test"
},
"layers": {
"ex": {
"provider": {
"name": "mapnik",
"mapfile": "style.xml"
},
"projection": "spherical mercator"
}
}
}, style_file)
app = TileStache.WSGITileServer(config=config)
# Mount the application
cherrypy.tree.graft(app, "/")
# Unsubscribe the default server
cherrypy.server.unsubscribe()
# Instantiate a new server object
server = cherrypy._cpserver.Server()
# Configure the server object
server.socket_host = "0.0.0.0"
server.socket_port = int(
rospy.get_param('/rospilot/mapnik_server_port'))
server.thread_pool = 5
# Subscribe this server
server.subscribe()
cherrypy.engine.start()
rospy.loginfo("Mapnik server is running")
rospy.loginfo(os.path.dirname(os.path.realpath(__file__)))
rospy.loginfo(os.getcwd())
rospy.spin()
cherrypy.engine.exit()
def _get_package_paths(pkgname, rospack):
paths = []
path = rospack.get_path(pkgname)
paths.append(path)
results = find_in_workspaces(search_dirs=['share'], project=pkgname, first_match_only=True, workspace_to_source_spaces=_catkin_workspace_to_source_spaces, source_path_to_packages=_catkin_source_path_to_packages)
if results and results[0] != path:
paths.append(results[0])
return paths
def get_cmake_path():
relpath = os.path.join(os.path.dirname(__file__), '..', '..', 'cmake')
if os.path.exists(relpath):
return os.path.normpath(relpath)
paths = find_in_workspaces(['share'], 'catkin', first_matching_workspace_only=True, first_match_only=True)
if not paths:
raise RuntimeError('Could not determine catkin cmake path')
return os.path.join(paths[0], 'cmake')
def _get_package_paths(pkgname, rospack):
paths = []
path = rospack.get_path(pkgname)
paths.append(path)
results = find_in_workspaces(search_dirs=['share'], project=pkgname, first_match_only=True, workspace_to_source_spaces=_catkin_workspace_to_source_spaces, source_path_to_packages=_catkin_source_path_to_packages)
if results and results[0] != path:
paths.append(results[0])
return paths
def plotMan(env):
"""
Plots a human in OpenRAVE.
"""
# Load table into environment
objects_path = find_in_workspaces(project='iact_control',
path='src/data',
first_match_only=True)[0]
env.Load('{:s}/manifest.xml'.format(objects_path))
def find_resource(relative_path, package):
from catkin.find_in_workspaces import find_in_workspaces
paths = find_in_workspaces(project=package, search_dirs=['share'],
path=relative_path, first_match_only=True)
if paths and len(paths) == 1:
return paths[0]
else:
raise IOError('Loading resource "{:s}" failed.'.format(
relative_path))
def get_package_executables(package):
executables = []
package_directories = find_in_workspaces(project=package)
for package_directory in package_directories:
for root, dirs, file_names in os.walk(package_directory):
# exclude invisible directories
if '/.' in root:
continue
for file_name in file_names:
if os.stat(os.path.join(root, file_name)).st_mode & stat.S_IEXEC:
executables.append(file_name)
return executables
def test_find_in_workspaces(self):
existing = find_in_workspaces([], _workspaces=None)
self.assertEqual([], existing)
existing = find_in_workspaces([], 'foo', _workspaces=None)
self.assertEqual([], existing)
existing = find_in_workspaces([], 'foo', 'foopath', _workspaces=None)
self.assertEqual([], existing)
existing = find_in_workspaces(['include'], 'foo', 'foopath', _workspaces=None)
self.assertEqual([], existing)
checked = []
existing = find_in_workspaces(['include'], 'foo', 'foopath', _workspaces=['bar'], considered_paths=checked)
self.assertEqual([], existing)
self.assertEqual(['bar/include/foo/foopath'], checked)
checked = []
existing = find_in_workspaces(['include'], 'foo', 'foopath', _workspaces=['bar', 'baz'], considered_paths=checked)
self.assertEqual([], existing)
self.assertEqual(['bar/include/foo/foopath', 'baz/include/foo/foopath'], checked)
checked = []
existing = find_in_workspaces(['include', 'etc', 'libexec'], 'foo', 'foopath', _workspaces=['bar', 'baz'], considered_paths=checked)
self.assertEqual([], existing)
self.assertEqual(['bar/include/foo/foopath',
'bar/etc/foo/foopath',
'bar/lib/foo/foopath',
'bar/libexec/foo/foopath',
'baz/include/foo/foopath',
'baz/etc/foo/foopath',
'baz/lib/foo/foopath',
'baz/lib/foo/foopath',
'baz/libexec/foo/foopath'], checked)
checked = []
existing = find_in_workspaces(['share', 'etc', 'lib'], None, 'foopath', _workspaces=['bar', 'baz'], considered_paths=checked)
self.assertEqual([], existing)
self.assertEqual(['bar/share/foopath',
'bar/etc/foopath',
'bar/lib/foopath',
'baz/share/foopath',
'baz/etc/foopath',
'baz/lib/foopath'], checked)
checked = []
existing = find_in_workspaces(None, None, None, _workspaces=['bar'], considered_paths=checked)
self.assertEqual([], existing)
self.assertEqual(['bar/bin', 'bar/etc', 'bar/include', 'bar/lib', 'bar/share'], checked)
def setup(sim=False, viewer=None, debug=True):
# load the robot and environment for meal serving
# find the openrave environment file
data_base_path = find_in_workspaces(search_dirs=['share'],
project=project_name,
path='data',
first_match_only=True)
if len(data_base_path) == 0:
raise Exception(
'Unable to find environment path. Did you source devel/setup.bash?'
)
env_path = os.path.join(data_base_path[0], 'environments', 'table.env.xml')
# Initialize logging
if debug:
openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Debug)
else:
openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Info)
openravepy.misc.InitOpenRAVELogging()
prpy.logger.initialize_logging()
# Load the environment and robot
env, robot = adapy.initialize(attach_viewer=viewer,
sim=sim,
env_path=env_path)
# Set the active manipulator on the robot
robot.arm.SetActive()
# Now set everything to the right location in the environment
#if using jaco, assume we have the portable mount, and robot should have a different distance to table
using_jaco = robot.GetName() == 'JACO'
if using_jaco:
robot_pose = numpy.array([[1., 0., 0., 0.409], [0., 1., 0., 0.338],
[0., 0., 1., 0.754], [0., 0., 0., 1.]])
else:
robot_pose = numpy.array([[1., 0., 0., 0.409], [0., 1., 0., 0.338],
[0., 0., 1., 0.795], [0., 0., 0., 1.]])
with env:
robot.SetTransform(robot_pose)
# Set the robot joint configurations
ResetTrial(robot)
load_fork_and_tool(env, robot)
# add boxes for constraint to not hit user
AddConstraintBoxes(env, robot)
return env, robot
def run(self):
rospy.init_node('rospilot_mapnik_server')
os.environ['PGUSER'] = '******'
os.environ['PGPASSWORD'] = '******'
os.environ['PGHOST'] = 'localhost'
style_file = find_in_workspaces(['share'], 'rospilot',
'share/mapnik-style/style.xml', first_match_only=True)
if not style_file:
rospy.logfatal("Cannot find share/mapnik-style/style.xml")
else:
style_file = style_file[0]
config = expanduser(rospy.get_param('~tilestache_config_file'))
# XXX: Monkey patch Mapnik because Tilestache doesn't work with the newest version
mapnik.FontEngine.instance = staticmethod(lambda: mapnik.FontEngine)
config = buildConfiguration({
"cache": {"name": "Test"},
"layers": {
"ex": {
"provider": {"name": "mapnik", "mapfile": "style.xml"},
"projection": "spherical mercator"
}
}
}, style_file)
app = TileStache.WSGITileServer(config=config)
# Mount the application
cherrypy.tree.graft(app, "/")
# Unsubscribe the default server
cherrypy.server.unsubscribe()
# Instantiate a new server object
server = cherrypy._cpserver.Server()
# Configure the server object
server.socket_host = "0.0.0.0"
server.socket_port = int(rospy.get_param('/rospilot/mapnik_server_port'))
server.thread_pool = 5
# Subscribe this server
server.subscribe()
cherrypy.engine.start()
rospy.loginfo("Mapnik server is running")
rospy.loginfo(os.path.dirname(os.path.realpath(__file__)))
rospy.loginfo(os.getcwd())
rospy.spin()
cherrypy.engine.exit()
def initialize():
'''Load and configure the Archie robot. Returns robot and environment.'''
env = openravepy.Environment()
env.SetViewer('qtcoin')
# Assumes the robot files are located in the urdf folder of the
# kinova_description package in the catkin workspace.
#env.Load('lab.xml')
urdf_uri = 'package://interactpy/jaco_dynamics.urdf'
srdf_uri = 'package://interactpy/jaco_dynamics.srdf'
objects_path = find_in_workspaces(project='interactpy',
path='envdata',
first_match_only=True)[0]
or_urdf = openravepy.RaveCreateModule(env, 'urdf')
robot_name = or_urdf.SendCommand('load {:s} {:s}'.format(
urdf_uri, srdf_uri))
robot = env.GetRobot(robot_name)
bind_subclass(robot, ArchieRobot)
# Put robot in natural starting config.
# Load environment
#env.Load('{:s}/table.xml'.format(objects_path))
env.Load('{:s}/mug1.kinbody.xml'.format(objects_path))
# table = env.GetKinBody('table')J
# table.SetTransform(np.array([[1.00000000e+00, -2.79931237e-36, 1.41282351e-14, 1.50902510e-01],
# [-2.07944729e-28, 1.00000000e+00, 1.47183799e-14, -1.47385532e-02],
# [-1.41282351e-14, -1.47183799e-14, 1.00000000e+00, -1.00134850e-01],
# [0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]))
mug = env.GetKinBody('mug')
# mug.SetTransform(np.array([[1.00000000e+00, 0.00000000e+00, 0.00000000e+00, 6.57676935e-01],
# [0.00000000e+00, 0.00000000e+00, -1.00000000e+00, -3.07691471e-06],
# [0.00000000e+00, 1.00000000e+00, 0.00000000e+00, 5.46909690e-01],
# [0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]))
#mug.SetTransform(np.array([[1.00000000e+00, 0.00000000e+00, 0.00000000e+00, -3.00000000e-01],
# [0.00000000e+00, 0.00000000e+00, -1.00000000e+00, 1.00000000e+00],
# [0.00000000e+00, 1.00000000e+00, 0.00000000e+00, -1.00000000e-01],
# [0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]))
# mug.SetTransform(np.array([[1.00000000e+00, 0.00000000e+00, 0.00000000e+00, -0.10000000],
# [0.00000000e+00, 0.00000000e+00, -1.00000000e+00, 0.20000000e+00],
# [0.00000000e+00, 1.00000000e+00, 0.00000000e+00, -1.00000000e-01],
# [0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]))
mug.SetTransform([[1, 0, 0, -0.4], [0, 0, -1, 0.3], [0, 1, 0, -0.1]])
#mug.SetTransform([[1,0,0,-0.6],[0,0,-1,-0.6],[0,1,0,.7]])
robot.SetActiveDOFs(np.array([0, 1, 2, 3, 4, 5, 6, 7, 8, 9]))
robot.SetDOFValues(robot_starting_dofs)
return env, robot, mug
def plotCabinet(env):
"""
Plots the cabinet in OpenRAVE.
"""
# Load table into environment
objects_path = find_in_workspaces(project='iact_control',
path='src/data',
first_match_only=True)[0]
env.Load('{:s}/cabinet.xml'.format(objects_path))
cabinet = env.GetKinBody('cabinet')
cabinet.SetTransform(
np.array([[0.0, -1.0, 0.0, 0.6], [1.0, 0.0, 0.0, 0],
[0.0, 0.0, 1.0, 0], [0.0, 0.0, 0.0, 1.0]]))
color = np.array([0.63, 0.32, 0.18])
cabinet.GetLinks()[0].GetGeometries()[0].SetDiffuseColor(color)
def _call_mutate(self, sudo, installation_files):
try:
# Installed script location
mutate_files_exec = find_in_workspaces(
project="segway_upstart", path="mutate_files", first_match_only=True)[0]
except IndexError:
# Devel script location
mutate_files_exec = find_in_workspaces(
project="segway_upstart", path="scripts/mutate_files", first_match_only=True)[0]
# If sudo is specified, then the user will be prompted at this point.
cmd = [mutate_files_exec]
if sudo:
cmd.insert(0, sudo)
print "Now calling: %s" % ' '.join(cmd)
p = subprocess.Popen(cmd + [pickle.dumps(installation_files)])
p.communicate()
if p.returncode == 0:
print "Filesystem operation succeeded."
else:
print "Error encountered; filesystem operation aborted."
return p.returncode
def plotTable(env):
"""
Plots the robot table in OpenRAVE.
"""
# load table into environment
objects_path = find_in_workspaces(project='interactpy',
path='envdata',
first_match_only=True)[0]
env.Load('{:s}/table.xml'.format(objects_path))
table = env.GetKinBody('table')
table.SetTransform(
np.array([[0.0, 1.0, 0.0, 0], [1.0, 0.0, 0.0, 0],
[0.0, 0.0, 1.0, -0.832], [0.0, 0.0, 0.0, 1.0]]))
color = np.array([0.9, 0.75, 0.75])
table.GetLinks()[0].GetGeometries()[0].SetDiffuseColor(color)
def plotMug(env, bodies, transform, color=[1, 0, 0]):
"""
Plots mug at specific transform.
"""
objects_path = find_in_workspaces(project='iact_control',
path='src/data',
first_match_only=True)[0]
env.Load('{:s}/mug.xml'.format(objects_path))
mug = env.GetKinBody('mug')
mug.GetLinks()[0].GetGeometries()[0].SetDiffuseColor(np.array(color))
mug.SetTransform(transform)
body = mug
body.SetName("pt" + str(len(bodies)))
env.Add(body, True)
bodies.append(body)
return mug
def getDirectory():
'''Get the file path for the location of kinbody
@return object_path (string) Path to objects folder'''
from catkin.find_in_workspaces import find_in_workspaces
package_name = 'mcube_objects'
directory = 'data'
objects_path = find_in_workspaces(search_dirs=['share'],
project=package_name,
path=directory,
first_match_only=True)
if len(objects_path) == 0:
raise RuntimeError('Can\'t find directory {}/{}'.format(
package_name, directory))
else:
objects_path = objects_path[0]
return objects_path
def plotCupTraj(env, robot, bodies, waypts, color=[0, 1, 0], increment=1):
"""
Plots trajectory of the cup.
"""
for i in range(0, len(waypts), increment):
waypoint = waypts[i]
print "waypt: " + str(waypoint)
dof = np.append(waypoint, np.array([1, 1, 1]))
dof[2] += math.pi
robot.SetDOFValues(dof)
links = robot.GetLinks()
manipTf = links[9].GetTransform()
# load mug into environment
objects_path = find_in_workspaces(project='iact_control',
path='src/data',
first_match_only=True)[0]
env.Load('{:s}/mug.xml'.format(objects_path))
mug = env.GetKinBody('mug')
mug.GetLinks()[0].GetGeometries()[0].SetDiffuseColor(np.array(color))
angle = -np.pi / 2
rot_x = np.array([[1, 0, 0, 0], [0,
np.cos(angle), -np.sin(angle), 0],
[0, np.sin(angle),
np.cos(angle), 0], [0, 0, 0, 1]])
rot_y = np.array([[np.cos(angle), 0,
np.sin(angle), 0], [0, 1, 0, 0],
[-np.sin(angle), 0,
np.cos(angle), 0], [0, 0, 0, 1]])
rot_z = np.array([[np.cos(angle), -np.sin(angle), 0, 0],
[np.sin(angle), 0,
np.cos(angle), 0], [0, 0, 1, 0], [0, 0, 0, 1]])
trans = np.array([[0, 0, 0, -0.02], [0, 0, 0, 0.02], [0, 0, 0, -0.02],
[0, 0, 0, 1]])
rotated = np.dot(manipTf + trans, rot_x)
mug.SetTransform(rotated)
body = mug
body.SetName("pt" + str(len(bodies)))
env.Add(body, True)
bodies.append(body)
def plotTable(env):
"""
Plots the robot table in OpenRAVE.
"""
# load table into environment
objects_path = find_in_workspaces(
project='iact_control',
path='src/data',
first_match_only=True)[0]
env.Load('{:s}/table.xml'.format(objects_path))
table = env.GetKinBody('table')
table.SetTransform(np.array([[0.0, 1.0, 0.0, -0.8128/2], #should be negative 1?
[1.0, 0.0, 0.0, 0],
[0.0, 0.0, 1.0, -0.1143], #-0.7874
[0.0, 0.0, 0.0, 1.0]]))
color = np.array([0.9, 0.75, 0.75])
table.GetLinks()[0].GetGeometries()[0].SetDiffuseColor(color)
def _find_resource(resolved, a, args, _context):
"""
process $(find-resource PKG PATH)
Resolves the relative PATH from the share folder of the PKG either from install space, devel space or from the source folder.
:returns: updated resolved argument, ``str``
:raises: :exc:SubstitutionException: if PKG and PATH invalidly specified or relative PATH is not found for PKG
"""
if len(args) != 2:
raise SubstitutionException("$(find-resource pkg path) command only accepts two argument [%s]" % a)
before, after = _split_command(resolved, a)
path = _sanitize_path(args[1])
# we try to find the specific path in share via catkin
# which will search in install/devel space and the source folder of the package
from catkin.find_in_workspaces import find_in_workspaces
paths = find_in_workspaces(['share'], project=args[0], path=path, first_matching_workspace_only=True, first_match_only=True)
if not paths:
raise SubstitutionException("$(find-resource pkg path) could not find path [%s]" % a)
return before + paths[0] + after
def test_bag(self):
bagname = os.path.join(
find_in_workspaces(search_dirs=['share'], project='ecto_ros')[0],
'tests', 't01.bag')
counts = bag_counts(bagname)
#test that the counts are the same.
print counts
self.assertEqual(counts['/camera/rgb/image_color'],
counts['/camera/depth/image'])
self.assertNotEqual(counts['/camera/rgb/image_color'], 0)
baggers = dict(
image=ImageBagger(topic_name='/camera/rgb/image_color'),
depth=ImageBagger(topic_name='/camera/depth/image'),
)
bagreader = BagReader(
'Bag Ripper',
baggers=baggers,
bag=bagname,
)
im2mat_rgb = Image2Mat()
im2mat_depth = Image2Mat()
counter_rgb = ecto.Counter()
counter_depth = ecto.Counter()
graph = [
bagreader["image"] >> im2mat_rgb["image"],
bagreader["depth"] >> im2mat_depth["image"],
im2mat_rgb[:] >> counter_rgb[:],
im2mat_depth[:] >> counter_depth[:]
]
plasm = ecto.Plasm()
plasm.connect(graph)
#ecto.view_plasm(plasm)
plasm.execute(niter=0)
print "expecting count:", counts['/camera/rgb/image_color']
print "RGB count:", counter_rgb.outputs.count
print "Depth count:", counter_depth.outputs.count
assert counts['/camera/rgb/image_color'] == counter_rgb.outputs.count
assert counts['/camera/depth/image'] == counter_depth.outputs.count
def process_package(package_name, soft_fail=True):
# type: (str) -> List[str], List[str]
try:
package_path = find_in_workspaces(
project=package_name,
path="package.xml",
first_match_only=True,
)[0]
except IndexError:
if not soft_fail:
raise RuntimeError(
"Unable to process package {}".format(package_name))
else:
# This is not a catkin dependency
return [], []
else:
package = parse_package(package_path)
dependencies = package.build_depends + package.exec_depends + package.test_depends
return parse_exported_requirements(package), dependencies
def parse_exported_requirements(package):
# type: (catkin_pkg.package.Package) -> List[str]
requirements_list = []
for export in package.exports:
if export.tagname == CATKIN_VIRTUALENV_TAGNAME:
try:
requirements_path = find_in_workspaces(
project=package.name,
path=export.content,
first_match_only=True,
)[0]
except:
print(
"Package {package} declares <{tagname}> {file}, which cannot be found in the package"
.format(package=package.name,
tagname=CATKIN_VIRTUALENV_TAGNAME,
file=export.content),
file=sys.stderr)
else:
requirements_list.append(requirements_path)
return requirements_list
def FindCatkinResource(package, relative_path):
'''
Find a Catkin resource in the share directory or
the package source directory. Raises IOError
if resource is not found.
@param relative_path Path relative to share or package
source directory
@param package The package to search in
@return Absolute path to resource
'''
from catkin.find_in_workspaces import find_in_workspaces
paths = find_in_workspaces(project=package, search_dirs=['share'],
path=relative_path, first_match_only=True)
if paths and len(paths) == 1:
return paths[0]
else:
raise IOError('Loading resource "{:s}" failed.'.format(
relative_path))
def test_bag(self):
bagname = os.path.join(find_in_workspaces(search_dirs=['share'], project='ecto_ros')[0], 'tests', 't01.bag')
counts = bag_counts(bagname)
#test that the counts are the same.
print counts
self.assertEqual(counts['/camera/rgb/image_color'], counts['/camera/depth/image'])
self.assertNotEqual(counts['/camera/rgb/image_color'], 0)
baggers = dict(image=ImageBagger(topic_name='/camera/rgb/image_color'),
depth=ImageBagger(topic_name='/camera/depth/image'),
)
bagreader = BagReader('Bag Ripper',
baggers=baggers,
bag=bagname,
)
im2mat_rgb = Image2Mat()
im2mat_depth = Image2Mat()
counter_rgb = ecto.Counter()
counter_depth = ecto.Counter()
graph = [
bagreader["image"] >> im2mat_rgb["image"],
bagreader["depth"] >> im2mat_depth["image"],
im2mat_rgb[:] >> counter_rgb[:],
im2mat_depth[:] >> counter_depth[:]
]
plasm = ecto.Plasm()
plasm.connect(graph)
#ecto.view_plasm(plasm)
plasm.execute(niter=0)
print "expecting count:", counts['/camera/rgb/image_color']
print "RGB count:", counter_rgb.outputs.count
print "Depth count:", counter_depth.outputs.count
assert counts['/camera/rgb/image_color'] == counter_rgb.outputs.count
assert counts['/camera/depth/image'] == counter_depth.outputs.count
def main():
""" Implementation of the ``install`` script."""
args = get_argument_parser().parse_args()
pkg, pkgpath = args.pkgpath[0].split('/', 1)
job_name = args.job or pkg.split('_', 1)[0]
# Any unspecified arguments are on the args object as None. These are filled
# in by the Job constructor when passed as Nones.
j = robot_upstart.Job(name=job_name,
interface=args.interface,
user=args.user,
workspace_setup=args.setup,
rosdistro=args.rosdistro,
master_uri=args.master,
log_path=args.logdir)
found_path = find_in_workspaces(project=pkg,
path=pkgpath,
first_match_only=True)
if not found_path:
print "Unable to locate path %s in package %s. Installation aborted." % (
pkgpath, pkg)
if os.path.isfile(found_path[0]):
# Single file, install just that.
j.add(package=pkg, filename=pkgpath)
else:
# Directory found, install everything within.
j.add(package=pkg, glob=os.path.join(pkgpath, "*"))
if args.augment:
j.generate_system_files = False
j.install()
return 0
def find_adapy_resource(relative_path, package='adapy'):
""" Returns the full path to a desired file given a ROS package, and the relative path
of that file in the package.
@param relative_path: The path and filename relative to the package path
@type relative_path: str
@param package: Name of the ROS package
@type package: str
@return Full path and filename
@rtype str
"""
from catkin.find_in_workspaces import find_in_workspaces
paths = find_in_workspaces(project=package,
search_dirs=['share'],
path=relative_path,
first_match_only=True)
if paths and len(paths) == 1:
return paths[0]
else:
raise IOError(
'Loading AdaPy resource "{:s}" failed.'.format(relative_path))
def __init__(self, env, morsel_file, morsel_topic):
with open(morsel_file, 'rb') as f:
morsels = yaml.load(f)
self.morsels = {}
object_base_path = find_in_workspaces(search_dirs=['share'],
project=project_name,
path='data',
first_match_only=True)[0]
ball_path = os.path.join(object_base_path, 'objects',
'smallsphere.kinbody.xml')
delta_path = os.path.join(object_base_path, 'objects',
'mediumsphere.kinbody.xml')
for name in ['morsel0', 'morsel1', 'morsel2']:
if name not in morsels or morsels[name] is None:
continue
with env:
morsel = env.ReadKinBodyURI(ball_path)
delta = env.ReadKinBodyURI(delta_path)
morsel.SetName(name)
delta.SetName(name + '_delta')
env.Add(morsel)
env.Add(delta)
morsel.Enable(False)
delta.Enable(False)
morsel.SetTransform(morsels[name])
delta.SetTransform(morsels[name])
self.morsels[name] = {'morsel': morsel, 'delta': delta}
self.sub = rospy.Subscriber(morsel_topic, ProbabilityUpdate,
self._callback)
self.marker_publisher = rospy.Publisher("visualization_marker_array",
MarkerArray,
queue_size=50)
def initialize(robot_xml=None, env_path=None, attach_viewer=False,
sim=True, **kw_args):
import prpy, os
prpy.logger.initialize_logging()
# Hide TrajOpt logging.
os.environ.setdefault('TRAJOPT_LOG_THRESH', 'WARN')
# Load plugins.
if prpy.dependency_manager.is_catkin():
prpy.dependency_manager.export()
else:
prpy.dependency_manager.export(PACKAGE)
openravepy.RaveInitialize(True)
# Create the environment.
env = openravepy.Environment()
if env_path is not None:
if not env.Load(env_path):
raise Exception('Unable to load environment frompath %s' % env_path)
# Load the robot
model_name = None
share_directories = None
if robot_xml is None:
if prpy.dependency_manager.is_catkin():
from catkin.find_in_workspaces import find_in_workspaces
share_directories = find_in_workspaces(search_dirs=['share'],
project=PACKAGE)
if not share_directories:
logger.error('Unable to find share directory in catkin workspace.')
raise ValueError('Unable to find share directory in catkin workspace.')
import os.path
model_name = 'barrettsegway.robot.xml'
for share_directory in share_directories:
xml_path = os.path.join(share_directory, 'robots', model_name)
if os.path.exists(xml_path):
robot_xml = xml_path
if robot_xml is None:
err_str = 'Unable to find robot model with name: %s in directories %s.' % (model_name, share_directories)
logger.error(err_str)
raise ValueError(err_str)
# Load the robot model
robot = env.ReadRobotXMLFile(robot_xml)
env.Add(robot)
# Default arguments.
keys = [ 'arm_sim',
'hand_sim',
'ft_sim',
'segway_sim'
]
for key in keys:
if key not in kw_args:
kw_args[key] = sim
from wamrobot import WAMRobot
prpy.bind_subclass(robot, WAMRobot, **kw_args)
if sim:
dof_indices, dof_values \
= robot.configurations.get_configuration('zero')
robot.SetDOFValues(dof_values, dof_indices)
# Start by attempting to load or_rviz.
if attach_viewer == True:
attach_viewer = 'rviz'
env.SetViewer(attach_viewer)
# Fall back on qtcoin if loading or_rviz failed
if env.GetViewer() is None:
logger.warning(
'Loading the RViz viewer failed. Do you have or_interactive'
' marker installed? Falling back on qtcoin.')
attach_viewer = 'qtcoin'
if attach_viewer and env.GetViewer() is None:
env.SetViewer(attach_viewer)
if env.GetViewer() is None:
raise Exception('Failed creating viewer of type "{0:s}".'.format(
attach_viewer))
# Remove the ROS logging handler again.
# It might have been added when we loaded or_rviz.
prpy.logger.remove_ros_logger()
return env, robot
'''
import rospy
import json
import cherrypy
import os
import re
import glob
import std_srvs.srv
import rospilot.srv
import cv2
from optparse import OptionParser
from catkin.find_in_workspaces import find_in_workspaces
STATIC_PATH = find_in_workspaces(['share'],
'rospilot',
'share/web_assets/',
first_match_only=True)[0]
PORT_NUMBER = 8085
class API(object):
def __init__(self, media_path):
self.media_path = media_path
@cherrypy.expose
def video_devices(self):
return json.dumps(glob.glob('/dev/video*'))
@cherrypy.expose
def media(self, id=None):
if cherrypy.request.method == "DELETE":
ecto_ros.init(sys.argv, "image_sub_node")
sub_rgb = ImageSub("image_sub", topic_name='/camera/rgb/image_color', queue_size=0)
im2mat_rgb = ecto_ros.Image2Mat()
counter_rgb = ecto.Counter()
graph = [
sub_rgb["output"] >> im2mat_rgb["image"],
im2mat_rgb[:] >> counter_rgb[:],
]
plasm = ecto.Plasm()
plasm.connect(graph)
sched = ecto.Scheduler(plasm)
sched.execute()
rosbag = play_bag(bagname, delay=0.5)
wait_bag(rosbag)
sched.stop()
print "expecting RGB count:", msg_counts['/camera/rgb/image_color']
print "RGB count:", counter_rgb.outputs.count
assert msg_counts['/camera/rgb/image_color'] >= counter_rgb.outputs.count
assert counter_rgb.outputs.count != 0
if __name__ == "__main__":
bagname = os.path.join(find_in_workspaces(search_dirs=['share'],project='ecto_ros')[0], 'tests', 't01.bag')
msg_counts = bag_counts(bagname)
try:
roscore = start_roscore(delay=1)
for i in range(1, 10):
do_ecto(bagname, msg_counts)
finally:
roscore.terminate()
#!//bin/env python
import numpy
import unittest
import openravepy
import subprocess
import os
import sys
# Find the bh_280 URDF file
from catkin.find_in_workspaces import find_in_workspaces
share_directories = find_in_workspaces(search_dirs=['share'],
project='or_urdf',
path='test_data/'
)
if not share_directories:
raise ValueError('Unable to find BH280 model.')
found_models = False
for share_directory in share_directories:
urdf_path = os.path.join(share_directories[0], 'robots', 'bh280.urdf')
if os.path.exists(urdf_path):
found_models=True
break
if not found_models:
raise ValueError('Unable to find BH280 URDF file.')
class TestORUrdf(unittest.TestCase):
limitations under the License.
'''
import rospy
import json
import cherrypy
import os
import re
import glob
import std_srvs.srv
import rospilot.srv
import cv2
from optparse import OptionParser
from catkin.find_in_workspaces import find_in_workspaces
STATIC_PATH = find_in_workspaces(['share'], 'rospilot',
'share/web_assets/', first_match_only=True)[0]
NODEJS_DEPS_PATH = find_in_workspaces(['share'], 'rospilot',
'nodejs_deps/node_modules/', first_match_only=True)[0]
PORT_NUMBER = 8085
class API(object):
def __init__(self, media_path):
self.media_path = media_path
@cherrypy.expose
def video_devices(self):
return json.dumps(glob.glob('/dev/video*'))
@cherrypy.expose
def media(self, id=None):
def initialize(robot_xml=None, env_path=None, attach_viewer=False,
sim=True, **kw_args):
import prpy, os
prpy.logger.initialize_logging()
# Hide TrajOpt logging.
os.environ.setdefault('TRAJOPT_LOG_THRESH', 'WARN')
# Load plugins.
if prpy.dependency_manager.is_catkin():
prpy.dependency_manager.export()
else:
prpy.dependency_manager.export(PACKAGE)
openravepy.RaveInitialize(True)
# Create the environment.
env = openravepy.Environment()
if env_path is not None:
if not env.Load(env_path):
raise Exception('Unable to load environment frompath %s' % env_path)
if prpy.dependency_manager.is_catkin():
# Find the HERB URDF and SRDF files.
from catkin.find_in_workspaces import find_in_workspaces
share_directories = find_in_workspaces(search_dirs=['share'],
project='herb_description')
if not share_directories:
logger.error('Unable to find the HERB model. Do you have the'
' package herb_description installed?')
raise ValueError('Unable to find HERB model.')
found_models = False
for share_directory in share_directories:
urdf_path = os.path.join(share_directories[0], 'robots', 'herb.urdf')
srdf_path = os.path.join(share_directories[0], 'robots', 'herb.srdf')
if os.path.exists(urdf_path) and os.path.exists(srdf_path):
found_models = True
break
if not found_models:
logger.error('Missing URDF file and/or SRDF file for HERB.'
' Is the herb_description package properly installed?')
raise ValueError('Unable to find HERB URDF and SRDF files.')
# Load the URDF file into OpenRAVE.
urdf_module = openravepy.RaveCreateModule(env, 'urdf')
if urdf_module is None:
logger.error('Unable to load or_urdf module. Do you have or_urdf'
' built and installed in one of your Catkin workspaces?')
raise ValueError('Unable to load or_urdf plugin.')
args = 'Load {:s} {:s}'.format(urdf_path, srdf_path)
herb_name = urdf_module.SendCommand(args)
if herb_name is None:
raise ValueError('Failed loading HERB model using or_urdf.')
robot = env.GetRobot(herb_name)
if robot is None:
raise ValueError('Unable to find robot with name "{:s}".'.format(
herb_name))
else:
if robot_xml is None:
import os, rospkg
rospack = rospkg.RosPack()
base_path = rospack.get_path('herb_description')
robot_xml = os.path.join(base_path, 'ordata', 'robots', 'herb.robot.xml')
robot = env.ReadRobotXMLFile(robot_xml)
env.Add(robot)
# Default arguments.
keys = [ 'left_arm_sim', 'left_hand_sim', 'left_ft_sim',
'right_arm_sim', 'right_hand_sim', 'right_ft_sim',
'head_sim', 'talker_sim', 'segway_sim', 'perception_sim' ]
for key in keys:
if key not in kw_args:
kw_args[key] = sim
from herbrobot import HERBRobot
prpy.bind_subclass(robot, HERBRobot, **kw_args)
if sim:
dof_indices, dof_values \
= robot.configurations.get_configuration('relaxed_home')
robot.SetDOFValues(dof_values, dof_indices)
# Start by attempting to load or_rviz.
if attach_viewer == True:
attach_viewer = 'rviz'
env.SetViewer(attach_viewer)
# Fall back on qtcoin if loading or_rviz failed
if env.GetViewer() is None:
logger.warning(
'Loading the RViz viewer failed. Do you have or_interactive'
' marker installed? Falling back on qtcoin.')
attach_viewer = 'qtcoin'
if attach_viewer and env.GetViewer() is None:
env.SetViewer(attach_viewer)
if env.GetViewer() is None:
raise Exception('Failed creating viewer of type "{0:s}".'.format(
attach_viewer))
# Remove the ROS logging handler again. It might have been added when we
# loaded or_rviz.
prpy.logger.remove_ros_logger()
return env, robot
def BuildApriltagKinbody(family, index, width, thickness, name, path):
family_name = "tag%ih%i" % family
str_index = str(index).zfill(5)
texture_name = "tag%i_%i_%s.png" % (family + (str_index,))
blank_kinbody_path = find_in_workspaces(
search_dirs=["share"], project=PACKAGE, path="data/tags/apriltag_blank.kinbody.xml", first_match_only=True
)
blank_mesh_path = find_in_workspaces(
search_dirs=["share"], project=PACKAGE, path="data/tags/apriltag_blank.dae", first_match_only=True
)
texture_path = find_in_workspaces(
search_dirs=["share"],
project=PACKAGE,
path="data/tags/textures/%s/%s" % (family_name, texture_name),
first_match_only=True,
)
if not blank_kinbody_path:
raise Exception, "Unable to find data/tags/apriltag_blank.kinbody.xml"
if not blank_mesh_path:
raise Exception, "Unable to find data/tags/apriltag_blank.dae"
if not texture_path:
raise Exception, ("Unable to find data/tags/textures/%s/%s" % family_name, texture_name)
# Write out the kinbody
with open(blank_kinbody_path[0], "r") as f:
kinbody_data = f.read()
kinbody_data = kinbody_data.replace("NAME", name)
kinbody_data = kinbody_data.replace("MESH", "%s.dae" % name)
kinbody_path = os.path.join(path, "%s.kinbody.xml" % name)
with open(kinbody_path, "w") as f:
f.write(kinbody_data)
# Write out the mesh
# The width parameter above specifies the width of the black square region
# of the tag. This means the resulting mesh will be a little bit wider
# than what is specified.
block_width = family[0] ** 0.5
tag_xy = ((block_width + 4.0) / (block_width + 2.0)) * width * 0.5
tag_z = thickness / 2.0
mesh_positions = [
(-tag_xy, -tag_xy, tag_z),
(tag_xy, -tag_xy, tag_z),
(-tag_xy, tag_xy, tag_z),
(tag_xy, tag_xy, tag_z),
(-tag_xy, tag_xy, -tag_z),
(tag_xy, tag_xy, -tag_z),
(-tag_xy, -tag_xy, -tag_z),
(tag_xy, -tag_xy, -tag_z),
]
mesh_block = "\n".join([" ".join([str(p) for p in position]) for position in mesh_positions])
with open(blank_mesh_path[0], "r") as f:
dae_data = f.read()
dae_data = dae_data.replace("MESH_VERTICES", mesh_block)
dae_data = dae_data.replace("TEXTURE_PATH", texture_name)
mesh_path = os.path.join(path, "%s.dae" % name)
with open(mesh_path, "w") as f:
f.write(dae_data)
# Write out the texture
hires_texture_path = os.path.join(path, texture_name)
resize_texture(texture_path[0], hires_texture_path)
return kinbody_path
def catkin_find(package, path):
path_list = find_in_workspaces(project=package, path=path, first_matching_workspace_only=True)
if len(path_list) == 0:
return None
return path_list[0]
import herbpy
import os
import numpy
import math
import openravepy
# get the location of some objects (you'll need the pr_ordata package)
from catkin.find_in_workspaces import find_in_workspaces
objects_path = find_in_workspaces(
search_dirs=['share'],
project='pr_ordata',
path='data/objects',
first_match_only=True)[0]
print objects_path # for me this is '/home/eashapir/ros-hydro/src/pr-ordata/data/objects'
# ===========================
# ENVIRONMENT SETUP
# ===========================
env, robot = herbpy.initialize(sim=True, attach_viewer='rviz')
# add a table to the environment
table_file = os.path.join(objects_path, 'table.kinbody.xml')
table = env.ReadKinBodyXMLFile(table_file)
env.AddKinBody(table)
table_pose = numpy.array([[1., 0., 0., 2],
[0., 0., -1., 2],
[0., 1., 0., 0.0],
[0., 0., 0., 1.]])
def test_with_sourcepath(self):
def create_mock_workspace(root_dir, ws):
ws1 = os.path.join(root_dir, ws)
inc = os.path.join(ws1, "include")
share = os.path.join(ws1, "share")
p1inc = os.path.join(inc, "foo")
p1share = os.path.join(share, "foo")
os.makedirs(ws1)
os.makedirs(inc)
os.makedirs(share)
os.makedirs(p1inc)
os.makedirs(p1share)
with open(os.path.join(ws1, CATKIN_MARKER_FILE), 'w') as fhand:
fhand.write('loc1;loc2')
try:
fp_backup = catkin.find_in_workspaces.find_packages
root_dir = tempfile.mkdtemp()
catkin.find_in_workspaces.find_packages = Mock()
foomock = Mock()
foomock.name = 'foo'
barmock = Mock()
barmock.name = 'bar'
catkin.find_in_workspaces.find_packages.return_value = {'bar': barmock, 'foo': foomock}
create_mock_workspace(root_dir, 'ws1')
create_mock_workspace(root_dir, 'ws2')
checked = []
existing = find_in_workspaces(['share', 'etc'], 'foo', 'foopath', _workspaces=[os.path.join(root_dir, 'ws1')], considered_paths=checked)
self.assertEqual([os.path.join(root_dir, 'ws1', 'share', 'foo', 'foopath'),
'loc1/foo/foopath',
'loc2/foo/foopath',
os.path.join(root_dir, 'ws1', 'etc', 'foo', 'foopath')], checked)
self.assertEqual([], existing)
checked = []
existing = find_in_workspaces(['share', 'etc'], 'foo', None, _workspaces=[os.path.join(root_dir, 'ws1')], considered_paths=checked)
self.assertEqual([os.path.join(root_dir, 'ws1', 'share', 'foo'),
'loc1/foo',
'loc2/foo',
os.path.join(root_dir, 'ws1', 'etc', 'foo')], checked)
self.assertEqual([os.path.join(root_dir, 'ws1', 'share', 'foo')], existing)
# first-only option
checked = []
existing = find_in_workspaces(None, None, None, _workspaces=[os.path.join(root_dir, 'ws1'), os.path.join(root_dir, 'ws2')], considered_paths=checked)
self.assertEqual([
os.path.join(root_dir, 'ws1', 'include'),
os.path.join(root_dir, 'ws1', 'share'),
os.path.join(root_dir, 'ws2', 'include'),
os.path.join(root_dir, 'ws2', 'share')], existing)
existing = find_in_workspaces(None, None, None, _workspaces=[os.path.join(root_dir, 'ws1'), os.path.join(root_dir, 'ws2')], considered_paths=checked, first_matching_workspace_only=True)
self.assertEqual([
os.path.join(root_dir, 'ws1', 'include'),
os.path.join(root_dir, 'ws1', 'share')], existing)
existing = find_in_workspaces(None, None, None, _workspaces=[os.path.join(root_dir, 'ws1'), os.path.join(root_dir, 'ws2')], considered_paths=checked, first_match_only=True)
self.assertEqual([
os.path.join(root_dir, 'ws1', 'include')], existing)
# overlay: first_matching_workspace_only=True
checked = []
existing = find_in_workspaces(None, 'foo', None, _workspaces=[os.path.join(root_dir, 'ws1'), os.path.join(root_dir, 'ws2')], considered_paths=checked, first_matching_workspace_only=True)
self.assertEqual([
os.path.join(root_dir, 'ws1', 'include', 'foo'),
os.path.join(root_dir, 'ws1', 'share', 'foo')], existing)
finally:
catkin.find_in_workspaces.find_packages = fp_backup
shutil.rmtree(root_dir)
parser = argparse.ArgumentParser(description="Run the benchmark tests")
parser.add_argument('--engines', type=str, nargs='+', default=['ode','pqp','fcl'],
help="The collision checkers to test")
args = parser.parse_args()
# Self collision
self_collision_data = run_self_collision(args.engines)
out_basename = os.path.join(package_path, 'results', 'self_collision')
analyze(self_collision_data, title='Self Collision', out_basename=out_basename)
# Empty envirnment
empty_collision_data = run_environment_collision(args.engines)
out_basename = os.path.join(package_path, 'results', 'empty_env_collision')
analyze(empty_collision_data, title='Empty Environment Collisions', out_basename=out_basename)
# PR kitchen
from catkin.find_in_workspaces import find_in_workspaces
kitchen_env = find_in_workspaces(
search_dirs=['share'],
project='pr_ordata',
path='data/kitchen/pr_kitchen.env.xml',
first_match_only=True)[0]
kitchen_collision_data = run_environment_collision(args.engines, kitchen_env, 'kitchen')
out_basename = os.path.join(package_path, 'results', 'prkitchen_collision')
analyze(kitchen_collision_data, title='PR Kitchen Environment', out_basename=out_basename)
limitations under the License.
'''
import rospy
import json
import cherrypy
import os
import re
import glob
import std_srvs.srv
import rospilot.srv
import cv2
from optparse import OptionParser
from catkin.find_in_workspaces import find_in_workspaces
STATIC_PATH = find_in_workspaces(['share'], 'rospilot',
'share/web_assets/', first_match_only=True)[0]
PORT_NUMBER = 8085
class API(object):
def __init__(self, media_path):
self.media_path = media_path
@cherrypy.expose
def media(self, id=None):
if cherrypy.request.method == "DELETE":
if not re.match(r"([a-z]+\.)?[a-zA-Z0-9_-]+\.\w{2,5}", id):
rospy.logwarn("Ignoring request to delete %s", id)
return
os.remove(self.media_path + "/" + id)
#!/usr/bin/env python
import herbpy
import os
import numpy
import math
import openravepy
import sys # exit()
# get the location of some objects (you'll need the pr_ordata package)
from catkin.find_in_workspaces import find_in_workspaces
package_name = 'pr_ordata'
directory = 'data/objects'
objects_path = find_in_workspaces(
search_dirs=['share'],
project=package_name,
path=directory,
first_match_only=True)
if len(objects_path) == 0:
print('Can\'t find directory %s/%s' % (package_name, directory))
sys.exit()
else:
print objects_path # for me this is '/home/USERNAME/catkin_workspaces/herb_ws/src/pr-ordata/data/objects'
objects_path = objects_path[0]
# ===========================
# ENVIRONMENT SETUP
# ===========================
env, robot = herbpy.initialize(sim=True, attach_viewer='rviz')
