def test_get_ros_package_path(): from rospkg import get_ros_package_path assert None == get_ros_package_path(env={}) env = {'ROS_PACKAGE_PATH': ':'} assert ':' == get_ros_package_path(env=env) # trip-wire tests. Cannot guarantee that ROS_PACKAGE_PATH is set # to valid value on test machine, just make sure logic doesn't crash assert os.environ.get('ROS_PACKAGE_PATH', None) == get_ros_package_path()
def test_on_ros_path(): from rospkg import on_ros_path, get_ros_root, get_ros_package_path from rospkg.environment import _resolve_paths assert not on_ros_path(tempfile.gettempdir()) if get_ros_root() is not None: assert on_ros_path(get_ros_root()) if get_ros_package_path() is not None: paths = _resolve_paths(get_ros_package_path()).split(os.pathsep) for p in paths: assert on_ros_path(p), "failed: %s, [%s]" % (p, paths)
def test_on_ros_path(): from rospkg import on_ros_path, get_ros_root, get_ros_package_path from rospkg.environment import _resolve_paths assert not on_ros_path(tempfile.gettempdir()) if get_ros_root() is not None: assert on_ros_path(get_ros_root()) if get_ros_package_path() is not None: paths = _resolve_paths(get_ros_package_path()).split(os.pathsep) for p in paths: assert on_ros_path(p), "failed: %s, [%s]"%(p, paths)
def create_msg_package_index(): """ Scans the package paths and creates a package index always taking the highest in the workspace chain (i.e. takes an overlay in preference when there are multiple instances of the package). :returns: the package index :rtype: { name : catkin_pkg.Package } """ # should use this, but it doesn't sequence them properly, so we'd have to make careful version checks # this is inconvenient since it would always mean we should bump the version number in an overlay # when all that is necessary is for it to recognise that it is in an overlay # ros_paths = rospkg.get_ros_paths() package_index = {} ros_paths = rospkg.get_ros_package_path() ros_paths = [x for x in ros_paths.split(':') if x] # packages that don't properly identify themselves as message packages (fix upstream). for path in reversed( ros_paths): # make sure we pick up the source overlays last for unused_package_path, package in find_packages(path).items(): if ('message_generation' in [dep.name for dep in package.build_depends] or 'genmsg' in [dep.name for dep in package.build_depends] or package.name in rosjava_build_tools.catkin.message_package_whitelist): # print(package.name) # print(" version: %s" % package.version) # print(" dependencies: ") # for dep in package.build_depends: # if not (dep.name == 'message_generation'): # print(" : %s" % dep) package_index[package.name] = package return package_index
def get_ros_package_paths(): ''' Get the list of paths from the ROS_PACKAGE_PATH environment variable. :returns: the list of paths :rtype: [str] ''' return rospkg.environment._compute_package_paths(None, rospkg.get_ros_package_path())
def setUp(self): if 'ROSDEP_DEBUG' in os.environ: del os.environ['ROSDEP_DEBUG'] self.old_rr = rospkg.get_ros_root() self.old_rpp = rospkg.get_ros_package_path() if 'ROS_ROOT' in os.environ: del os.environ['ROS_ROOT'] os.environ['ROS_PACKAGE_PATH'] = os.path.join(get_test_tree_dir())
def setUp(self): if 'xylem_DEBUG' in os.environ: del os.environ['xylem_DEBUG'] self.old_rr = rospkg.get_ros_root() self.old_rpp = rospkg.get_ros_package_path() if 'ROS_ROOT' in os.environ: del os.environ['ROS_ROOT'] os.environ['ROS_PACKAGE_PATH'] = os.path.join(get_test_tree_dir())
def ping_cb(self, ping): print rospkg.get_ros_package_path() print "PINGERFINDER: freq={p.freq:.0f} amp={p.amplitude:.0f}".format( p=ping) if abs( ping.freq - self.target_freq ) < self.freq_tol and ping.amplitude > self.min_amp and self.listen: trans, rot = None, None try: self.tf_listener.waitForTransform(self.map_frame, self.hydrophone_frame, ping.header.stamp, rospy.Duration(0.25)) trans, rot = self.tf_listener.lookupTransform( self.map_frame, self.hydrophone_frame, ping.header.stamp) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: print e return p0 = np.array([trans[0], trans[1]]) R = tf.transformations.quaternion_matrix(rot)[:3, :3] delta = R.dot(navigator_tools.rosmsg_to_numpy(ping.position))[:2] p1 = p0 + self.heading_pseudorange * delta / npl.norm(delta) line_coeffs = np.array([[p0[0], p0[1], p1[0], p1[1]]]) # p0 and p1 define a line self.visualize_arrow(Point(p0[0], p0[1], 0), Point(p1[0], p1[1], 0)) self.line_array = np.append(self.line_array, line_coeffs, 0) self.observation_amplitudes = np.append( self.observation_amplitudes, ping.amplitude) if len( self.line_array ) >= self.max_observations: # delete softest samples if we have over max_observations softest_idx = np.argmin(self.observation_amplitudes) self.line_array = np.delete(self.line_array, softest_idx, axis=0) self.observation_amplitudes = np.delete( self.observation_amplitudes, softest_idx) print "PINGERFINDER: Observation collected. Total: {}".format( self.line_array.shape[0])
def setUp(self): if 'ROSDEP_DEBUG' in os.environ: del os.environ['ROSDEP_DEBUG'] self.old_rr = rospkg.get_ros_root() self.old_rpp = rospkg.get_ros_package_path() self.old_app = os.getenv(AMENT_PREFIX_PATH_ENV_VAR, None) if 'ROS_ROOT' in os.environ: del os.environ['ROS_ROOT'] os.environ['ROS_PACKAGE_PATH'] = os.path.join(get_test_tree_dir()) os.environ[AMENT_PREFIX_PATH_ENV_VAR] = os.path.join(get_test_tree_dir(), 'ament') if 'ROS_PYTHON_VERSION' not in os.environ: # avoid `test_check` failure due to warning on stderr os.environ['ROS_PYTHON_VERSION'] = sys.version[0]
def __init__(self): rospy.init_node('tl_detector', log_level=rospy.DEBUG) self.pose = None self.previous_pose = None self.waypoints = None self.waypoints_2d = None self.waypoint_tree = None self.camera_image = None self.lights = [] self.dataset_path = rospkg.get_ros_package_path().split( ':')[0] + '/images/' if GENERATE_DATASET: if not os.path.exists(self.dataset_path): os.makedirs(self.dataset_path) rospy.loginfo("Dataset will be created at %s", self.dataset_path) self.dataset_file = open(self.dataset_path + 'img_dataset.tsv', 'a') self.image_write_ts = rospy.Time() sub1 = rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb) sub2 = rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb) ''' /vehicle/traffic_lights provides you with the location of the traffic light in 3D map space and helps you acquire an accurate ground truth data source for the traffic light classifier by sending the current color state of all traffic lights in the simulator. When testing on the vehicle, the color state will not be available. You'll need to rely on the position of the light and the camera image to predict it. ''' sub3 = rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray, self.traffic_cb) sub6 = rospy.Subscriber('/image_color', Image, self.image_cb) config_string = rospy.get_param("/traffic_light_config") self.config = yaml.load(config_string) self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint', Int32, queue_size=1) self.bridge = CvBridge() self.light_classifier = TLClassifier() self.listener = tf.TransformListener() self.state = TrafficLight.UNKNOWN self.last_state = TrafficLight.UNKNOWN self.last_wp = -1 self.state_count = 0 rospy.spin()
def ping_cb(self, ping): print rospkg.get_ros_package_path() print "PINGERFINDER: freq={p.freq:.0f} amp={p.amplitude:.0f}".format(p=ping) if abs(ping.freq - self.target_freq) < self.freq_tol and ping.amplitude > self.min_amp and self.listen: trans, rot = None, None try: self.tf_listener.waitForTransform(self.map_frame, self.hydrophone_frame, ping.header.stamp, rospy.Duration(0.25)) trans, rot = self.tf_listener.lookupTransform(self.map_frame, self.hydrophone_frame, ping.header.stamp) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: print e return p0 = np.array([trans[0], trans[1]]) R = tf.transformations.quaternion_matrix(rot)[:3, :3] delta = R.dot(navigator_tools.point_to_numpy(ping.position))[:2] p1 = p0 + self.heading_pseudorange * delta / npl.norm(delta) line_coeffs = np.array([[p0[0], p0[1], p1[0], p1[1]]]) # p0 and p1 define a line self.visualize_arrow(Point(p0[0], p0[1], 0), Point(p1[0], p1[1], 0)) self.line_array = np.append(self.line_array, line_coeffs, 0) self.observation_amplitudes = np.append(self.observation_amplitudes, ping.amplitude) if len(self.line_array) >= self.max_observations: # delete softest samples if we have over max_observations softest_idx = np.argmin(self.observation_amplitudes) self.line_array = np.delete(self.line_array, softest_idx, axis=0) self.observation_amplitudes = np.delete(self.observation_amplitudes, softest_idx) print "PINGERFINDER: Observation collected. Total: {}".format(self.line_array.shape[0])
def generate_obstacle(): image = PIL.Image.open( filter(lambda x: "navigation" in x, rospkg.get_ros_package_path().split(':'))[0] + '/src/occupancytest.jpg') g_image = image.convert("L") g_array = np.asarray(g_image) #print('generating obstacle map') for i in range(0, len(g_array[0])): for j in range(0, len(g_array[1])): if g_array[i][j] == 0: ox.append(float(j)) oy.append(float(i)) return ox, oy
def test_get_stack_dir_unary(self): # now manipulate the environment to test precedence # - save original RPP as we popen rosstack in other tests d = roslib.packages.get_pkg_dir('test_roslib') d = os.path.join(d, 'test', 'stack_tests_unary') s1_d = os.path.join(d, 's1') rpp = rospkg.get_ros_package_path() try: paths = [d] os.environ[rospkg.environment.ROS_PACKAGE_PATH] = os.pathsep.join(paths) self.assertEquals(os.path.join(s1_d, 'foo'), roslib.stacks.get_stack_dir('foo')) self.assertEquals(os.path.join(s1_d, 'bar'), roslib.stacks.get_stack_dir('bar')) self.assertEquals(os.path.join(s1_d, 'baz'), roslib.stacks.get_stack_dir('baz')) finally: #restore rpp if rpp is not None: os.environ[rospkg.environment.ROS_PACKAGE_PATH] = rpp else: del os.environ[rospkg.environment.ROS_PACKAGE_PATH]
def test_get_stack_dir_unary(self): # now manipulate the environment to test precedence # - save original RPP as we popen rosstack in other tests d = roslib.packages.get_pkg_dir('roslib') d = os.path.join(d, 'test', 'stack_tests_unary') s1_d = os.path.join(d, 's1') rpp = rospkg.get_ros_package_path() try: paths = [d] os.environ[rospkg.environment.ROS_PACKAGE_PATH] = os.pathsep.join(paths) self.assertEquals(os.path.join(s1_d, 'foo'), roslib.stacks.get_stack_dir('foo')) self.assertEquals(os.path.join(s1_d, 'bar'), roslib.stacks.get_stack_dir('bar')) self.assertEquals(os.path.join(s1_d, 'baz'), roslib.stacks.get_stack_dir('baz')) finally: # restore rpp if rpp is not None: os.environ[rospkg.environment.ROS_PACKAGE_PATH] = rpp else: del os.environ[rospkg.environment.ROS_PACKAGE_PATH]
def rosrunmain(argv=sys.argv): """ main entry point for rosrun command-line tool @param argv: command-line args @type argv: [str] """ if len(argv) < 3: print("""Usage: rosrun PACKAGE EXECUTABLE [ARGS] rosrun will locate PACKAGE and try to find an executable named EXECUTABLE in the PACKAGE tree. If it finds it, it will run it with ARGS.""") sys.exit(0) if argv[1] == "--help": print("""Usage: rosrun PACKAGE EXECUTABLE [ARGS] rosrun will locate PACKAGE and try to find an executable named EXECUTABLE in the PACKAGE tree. If it finds it, it will run it with ARGS.""") sys.exit(1) # find packages paths = os.environ['LD_LIBRARY_PATH'].split( ';') + rospkg.get_ros_package_path().split(';') pkgs = [] for i in paths: pkg = os.path.join(i, argv[1]) if os.path.isdir(pkg): pkgs.append(os.path.normpath(pkg)) if len(pkgs) == 0: print("Couldn't find the package: " + argv[1]) exit(2) # examine executable for Windows executable = os.path.basename(argv[2]) if platform.system() == "Windows": if not executable.lower().endswith('.exe'): executable += ".exe" # make fullpath and choose one execs = [] key = 1 for j in pkgs: if os.path.isfile(os.path.join(j, executable)) == True: execs.append(os.path.join(j, executable)) if len(execs) > 1: print( "You have chosen a non-unique executable, please pick one of the following:" ) for n in range(len(execs)): print(" {0}: {1}".format(n + 1, execs[n])) while True: try: key = int(raw_input('Choose: ')) except: exit(3) if 0 < key <= len(execs): break elif len(execs) < 1: print("Couldn't find an executable in the package.") exit(4) # make command and execute commandline = execs[key - 1] if len(argv) > 3: params = argv[3:] commandline += ''.join([' %s' % d for d in params]) print(commandline) try: os.system(commandline) except KeyboardInterrupt: sys.exit(0)
def get_package_path(self): self.ROS_MASTER_URI = rospkg.get_ros_package_path()