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