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