<commit_before># Manually load w2v
# import os
# from nlpia.data.loaders import BIGDATA_PATH
# from gensim.models import KeyedVectors
# path = os.path.join(BIGDATA_PATH, 'GoogleNews-vectors-negative300.bin.gz')
# wv = KeyedVectors.load_word2vec_format(path, binary=True)

# nlpia can now automatically download and load w2v
from nlpia.data.loaders import get_data
from gensim.models import KeyedVectors
wv = get_data('word2vec')
# wv = KeyedVectors.load_word2vec_format(path, binary=True)
len(wv.vocab)
# 3000000
wv.vectors.shape
# (3000000, 300)

import pandas as pd
vocab = pd.Series(wv.vocab)
vocab.iloc[100000:100006]  # different words for new KeyedVector format
# Illington_Fund             Vocab(count:447860, index:2552140)
# Illingworth                 Vocab(count:2905166, index:94834)
# Illingworth_Halifax       Vocab(count:1984281, index:1015719)
# Illini                      Vocab(count:2984391, index:15609)
# IlliniBoard.com           Vocab(count:1481047, index:1518953)
# Illini_Bluffs              Vocab(count:2636947, index:363053)

import numpy as np
np.linalg.norm(wv['Illinois'] - wv['Illini'])  # <1>
# 3.3653798
similarity = np.dot(wv['Illinois'], wv['Illini']) / (
    np.linalg.norm(wv['Illinois']) * np.linalg.norm(wv['Illini']))   # <2>
similarity
# 0.5501352
1 - similarity  # <3>
# 0.4498648
# ----
# <1> Euclidean distance
# <2> Cosine similarity
# <3> Cosine distance

wv['Illini']
# array([ 0.15625   ,  0.18652344,  0.33203125,  0.55859375,  0.03637695,
#        -0.09375   , -0.05029297,  0.16796875, -0.0625    ,  0.09912109,
#        -0.0291748 ,  0.39257812,  0.05395508,  0.35351562, -0.02270508,

from nlpia.data.loaders import get_data
cities = get_data('cities')
cities.head(1).T
# geonameid                       3039154
# name                          El Tarter
# asciiname                     El Tarter
# alternatenames     Ehl Tarter,Эл Тартер
# latitude                        42.5795
# longitude                       1.65362
# feature_class                         P
# feature_code                        PPL
# country_code                         AD
# cc2                                 NaN
# admin1_code                          02
# admin2_code                         NaN
# admin3_code                         NaN
# admin4_code                         NaN
# population                         1052
# elevation                           NaN
# dem                                1721
# timezone                 Europe/Andorra
# modification_date            2012-11-03

us = cities[(cities.country_code == 'US') & (cities.admin1_code.notnull())].copy()
states = pd.read_csv('http://www.fonz.net/blog/wp-content/uploads/2008/04/states.csv')
states = dict(zip(states.Abbreviation, states.State))
us['city'] = us.name.copy()
us['st'] = us.admin1_code.copy()
us['state'] = us.st.map(states)
us[us.columns[-3:]].head()
#                      city  st    state
# geonameid
# 4046255       Bay Minette  AL  Alabama
# 4046274              Edna  TX    Texas
# 4046319    Bayou La Batre  AL  Alabama
# 4046332         Henderson  TX    Texas
# 4046430           Natalia  TX    Texas

import numpy as np
vocab = pd.np.concatenate([us.city, us.st, us.state])
vocab = np.array([word for word in vocab if word in wv])
vocab[:10]
# array(['Edna', 'Henderson', 'Natalia', 'Yorktown', 'Brighton', 'Berry',
#        'Trinity', 'Villas', 'Bessemer', 'Aurora'], dtype='<U15')

# >>> us_300D = pd.DataFrame([[i] + list(wv[c] + (wv[state] if state in vocab else wv[s]) + wv[s]) for i, c, state, s
# ...                         in zip(us.index, us.city, us.state, us.st) if c in vocab])
city_plus_state = []
us = us.sort_values('population', ascending=False)
for c, state, st in zip(us.city, us.state, us.st):
    if c not in vocab:
        continue
    row = []
    if state in vocab:
        row.extend(wv[c] + wv[state])
    else:
        row.extend(wv[c] + wv[st])
    city_plus_state.append(row)
us_300D_sorted = pd.DataFrame(city_plus_state)
del city_plus_state
del wv

######################################################################
# Load the city word vectors and compute the 2D PCA vectors for each

# num_cities = 10  # simplified plot of 10 largest cities at beginning of the chapter
num_cities = 500  # Detailed eye candy plot of US cities

# Original confusing/complicated/detailed plot
from sklearn.decomposition import PCA
pca = PCA(n_components=2)  # <1>
us_300D = get_data('cities_us_wordvectors')
us_2D = pca.fit_transform(us_300D.iloc[:num_cities, :300])  # <2>
# ----
# <1> PCA here is for visualization of high dimensional vectors only, not for calculating Word2vec vectors
# <2> The last column (# 301) of this DataFrame contains the name, which is also stored in the DataFrame index.

#
######################################################################

############################################################################################
# Offline interactive HTML plot example (completely independent of the examples above)

import os
from nlpia.data.loaders import get_data
from nlpia.plots import offline_plotly_scatter_bubble
num_cities = 200

df = get_data('cities_us_wordvectors_pca2_meta')
df = df.sort_values('population', ascending=False)[:num_cities].copy()
# <1> flip East/West & North/South axes to match geography better
flip_east_west, flip_north_south = -1, -1
df['x'] = flip_east_west * df['x']
df['y'] = flip_north_south * df['y']
df = df.sort_values('population', ascending=True)
html = offline_plotly_scatter_bubble(
    df, x='x', y='y',
    size_col='population', text_col='name', category_col='timezone',
    xscale=None, yscale=None,  # 'log' or None
    layout={}, marker={'sizeref': 3000})
filename = 'wordmap_{}_US_cities.html'.format(num_cities)
filepath = os.path.abspath(filename)
print('Saving interactive HTML plot to "{}".'.format(filepath))
with open(filepath, 'w') as fout:
    fout.write(html)
# !firefox ./wordmap_*.html  # linux
# open -a Firefox wordmap_*.html  # Mac

# End of offline interactive visualization
#################################################################################

###################################################
# Simplified plot for beginning of chapter 6

from nlpia.data.loaders import get_data
from nlpia.plots import offline_plotly_scatter_bubble
df = get_data('cities_us_wordvectors_pca2_meta')
df = df.sort_values('population', ascending=False)[:10].copy()
df[['x', 'y']] = - df[['x', 'y']]  # <1>
html = offline_plotly_scatter_bubble(
    df, x='x', y='y',
    size_col=None, text_col='name', category_col=None,
    xscale=None, yscale=None,  # 'log' or None
    layout={}, marker={'sizeref': 3000})
with open('wordmap_10_US_cities.html', 'w') as fout:
    fout.write(html)
# !firefox ./wordmap.html

<1> flips the East/West North/South axes around to match geography better

#
#######################################################<commit_msg>fixed syntax error<commit_after># Manually load w2v
# import os
# from nlpia.data.loaders import BIGDATA_PATH
# from gensim.models import KeyedVectors
# path = os.path.join(BIGDATA_PATH, 'GoogleNews-vectors-negative300.bin.gz')
# wv = KeyedVectors.load_word2vec_format(path, binary=True)

# nlpia can now automatically download and load w2v
from nlpia.data.loaders import get_data
from gensim.models import KeyedVectors
wv = get_data('word2vec')
# wv = KeyedVectors.load_word2vec_format(path, binary=True)
len(wv.vocab)
# 3000000
wv.vectors.shape
# (3000000, 300)

import pandas as pd
vocab = pd.Series(wv.vocab)
vocab.iloc[100000:100006]  # different words for new KeyedVector format
# Illington_Fund             Vocab(count:447860, index:2552140)
# Illingworth                 Vocab(count:2905166, index:94834)
# Illingworth_Halifax       Vocab(count:1984281, index:1015719)
# Illini                      Vocab(count:2984391, index:15609)
# IlliniBoard.com           Vocab(count:1481047, index:1518953)
# Illini_Bluffs              Vocab(count:2636947, index:363053)

import numpy as np
np.linalg.norm(wv['Illinois'] - wv['Illini'])  # <1>
# 3.3653798
similarity = np.dot(wv['Illinois'], wv['Illini']) / (
    np.linalg.norm(wv['Illinois']) * np.linalg.norm(wv['Illini']))   # <2>
similarity
# 0.5501352
1 - similarity  # <3>
# 0.4498648
# ----
# <1> Euclidean distance
# <2> Cosine similarity
# <3> Cosine distance

wv['Illini']
# array([ 0.15625   ,  0.18652344,  0.33203125,  0.55859375,  0.03637695,
#        -0.09375   , -0.05029297,  0.16796875, -0.0625    ,  0.09912109,
#        -0.0291748 ,  0.39257812,  0.05395508,  0.35351562, -0.02270508,

from nlpia.data.loaders import get_data
cities = get_data('cities')
cities.head(1).T
# geonameid                       3039154
# name                          El Tarter
# asciiname                     El Tarter
# alternatenames     Ehl Tarter,Эл Тартер
# latitude                        42.5795
# longitude                       1.65362
# feature_class                         P
# feature_code                        PPL
# country_code                         AD
# cc2                                 NaN
# admin1_code                          02
# admin2_code                         NaN
# admin3_code                         NaN
# admin4_code                         NaN
# population                         1052
# elevation                           NaN
# dem                                1721
# timezone                 Europe/Andorra
# modification_date            2012-11-03

us = cities[(cities.country_code == 'US') & (cities.admin1_code.notnull())].copy()
states = pd.read_csv('http://www.fonz.net/blog/wp-content/uploads/2008/04/states.csv')
states = dict(zip(states.Abbreviation, states.State))
us['city'] = us.name.copy()
us['st'] = us.admin1_code.copy()
us['state'] = us.st.map(states)
us[us.columns[-3:]].head()
#                      city  st    state
# geonameid
# 4046255       Bay Minette  AL  Alabama
# 4046274              Edna  TX    Texas
# 4046319    Bayou La Batre  AL  Alabama
# 4046332         Henderson  TX    Texas
# 4046430           Natalia  TX    Texas

import numpy as np
vocab = pd.np.concatenate([us.city, us.st, us.state])
vocab = np.array([word for word in vocab if word in wv])
vocab[:10]
# array(['Edna', 'Henderson', 'Natalia', 'Yorktown', 'Brighton', 'Berry',
#        'Trinity', 'Villas', 'Bessemer', 'Aurora'], dtype='<U15')

# >>> us_300D = pd.DataFrame([[i] + list(wv[c] + (wv[state] if state in vocab else wv[s]) + wv[s]) for i, c, state, s
# ...                         in zip(us.index, us.city, us.state, us.st) if c in vocab])
city_plus_state = []
us = us.sort_values('population', ascending=False)
for c, state, st in zip(us.city, us.state, us.st):
    if c not in vocab:
        continue
    row = []
    if state in vocab:
        row.extend(wv[c] + wv[state])
    else:
        row.extend(wv[c] + wv[st])
    city_plus_state.append(row)
us_300D_sorted = pd.DataFrame(city_plus_state)
del city_plus_state
del wv

######################################################################
# Load the city word vectors and compute the 2D PCA vectors for each

# num_cities = 10  # simplified plot of 10 largest cities at beginning of the chapter
num_cities = 500  # Detailed eye candy plot of US cities

# Original confusing/complicated/detailed plot
from sklearn.decomposition import PCA
pca = PCA(n_components=2)  # <1>
us_300D = get_data('cities_us_wordvectors')
us_2D = pca.fit_transform(us_300D.iloc[:num_cities, :300])  # <2>
# ----
# <1> PCA here is for visualization of high dimensional vectors only, not for calculating Word2vec vectors
# <2> The last column (# 301) of this DataFrame contains the name, which is also stored in the DataFrame index.

#
######################################################################

############################################################################################
# Offline interactive HTML plot example (completely independent of the examples above)

import os
from nlpia.data.loaders import get_data
from nlpia.plots import offline_plotly_scatter_bubble
num_cities = 200

df = get_data('cities_us_wordvectors_pca2_meta')
df = df.sort_values('population', ascending=False)[:num_cities].copy()
# <1> flip East/West & North/South axes to match geography better
flip_east_west, flip_north_south = -1, -1
df['x'] = flip_east_west * df['x']
df['y'] = flip_north_south * df['y']
df = df.sort_values('population', ascending=True)
html = offline_plotly_scatter_bubble(
    df, x='x', y='y',
    size_col='population', text_col='name', category_col='timezone',
    xscale=None, yscale=None,  # 'log' or None
    layout={}, marker={'sizeref': 3000})
filename = 'wordmap_{}_US_cities.html'.format(num_cities)
filepath = os.path.abspath(filename)
print('Saving interactive HTML plot to "{}".'.format(filepath))
with open(filepath, 'w') as fout:
    fout.write(html)
# !firefox ./wordmap_*.html  # linux
# open -a Firefox wordmap_*.html  # Mac

# End of offline interactive visualization
#################################################################################

###################################################
# Simplified plot for beginning of chapter 6

from nlpia.data.loaders import get_data
from nlpia.plots import offline_plotly_scatter_bubble
df = get_data('cities_us_wordvectors_pca2_meta')
df = df.sort_values('population', ascending=False)[:10].copy()
df[['x', 'y']] = - df[['x', 'y']]  # <1>
html = offline_plotly_scatter_bubble(
    df, x='x', y='y',
    size_col=None, text_col='name', category_col=None,
    xscale=None, yscale=None,  # 'log' or None
    layout={}, marker={'sizeref': 3000})
with open('wordmap_10_US_cities.html', 'w') as fout:
    fout.write(html)
# !firefox ./wordmap.html

# <1> flips the East/West North/South axes around to match geography better

#
#######################################################
<|endoftext|>


<commit_before># encoding: utf-8
# ---------------------------------------------------------------------------
#  Copyright (C) 2008-2014, IPython Development Team and Enthought, Inc.
#  Distributed under the terms of the BSD License.  See COPYING.rst.
# ---------------------------------------------------------------------------

import operator
from itertools import product
from functools import reduce
from collections import Sequence, Mapping

import numpy

from distarray import utils
from distarray.externals.six import next
from distarray.externals.six.moves import map

class InvalidGridShapeError(Exception):
    pass

class GridShapeError(Exception):
    pass

def check_grid_shape_preconditions(shape, dist, comm_size):
    """
    Verify various distarray parameters are correct before making a grid_shape.
    """
    if comm_size < 1:
        raise ValueError("comm_size >= 1 not satisfied, comm_size = %s" %
                         (comm_size,))
    if len(shape) != len(dist):
        raise ValueError("len(shape) == len(dist) not satisfied, len(shape) ="
                         " %s and len(dist) = %s" % (len(shape), len(dist)))
    if any(i < 0 for i in shape):
        raise ValueError("shape must be a sequence of non-negative integers, "
                         "shape = %s" % (shape,))
    if any(i not in ('b', 'c', 'n', 'u') for i in dist):
        raise ValueError("dist must be a sequence of 'b', 'n', 'c', 'u' "
                         "strings, dist = %s" % (dist,))

def check_grid_shape_postconditions(grid_shape, shape, dist, comm_size):
    if not (len(grid_shape) == len(shape) == len(dist)):
        raise ValueError("len(gird_shape) == len(shape) == len(dist) not "
                         "satisfied, len(grid_shape) = %s and len(shape) = %s "
                         "and len(dist) = %s" % (len(grid_shape), len(shape),
                                                 len(dist)))
    if any(gs < 1 for gs in grid_shape):
        raise ValueError("all(gs >= 1 for gs in grid_shape) not satisfied, "
                         "grid_shape = %s" % (grid_shape,))
    if any(gs != 1 for (d, gs) in zip(dist, grid_shape) if d == 'n'):
        raise ValueError("all(gs == 1 for (d, gs) in zip(dist, grid_shape) if "
                         "d == 'n', not satified dist = %s and grid_shape = "
                         "%s" % (dist, grid_shape))
    if any(gs > s for (s, gs) in zip(shape, grid_shape) if s > 0):
        raise ValueError("all(gs <= s for (s, gs) in zip(shape, grid_shape) "
                         "if s > 0) not satisfied, shape = %s and grid_shape "
                         "= %s" % (shape, grid_shape))
    if reduce(operator.mul, grid_shape, 1) > comm_size:
        raise ValueError("reduce(operator.mul, grid_shape, 1) <= comm_size not"
                         " satisfied, grid_shape product = %s and comm_size = "
                         " %s" % (reduce(operator.mul, grid_shape, 1),
                                  comm_size))

def normalize_grid_shape(grid_shape, shape, dist, comm_size):
    """Adds 1s to grid_shape so it has `ndims` dimensions.  Validates
    `grid_shape` tuple against the `dist` tuple and `comm_size`.
    """
    def check_normalization_preconditions(grid_shape, dist):
        if any(i < 0 for i in grid_shape):
            raise ValueError("grid_shape must be a sequence of non-negative "
                             "integers, grid_shape = %s" % (grid_shape,))
        if len(grid_shape) > len(dist):
            raise ValueError("len(grid_shape) <= len(dist) not satisfied, "
                             "len(grid_shape) = %s and len(dist) = %s" %
                             (len(grid_shape), len(dist)))
    check_grid_shape_preconditions(shape, dist, comm_size)
    check_normalization_preconditions(grid_shape, dist)

    ndims = len(shape)
    grid_shape = tuple(grid_shape) + (1,) * (ndims - len(grid_shape))

    # short circuit for special case
    if all(x == 'n' for x in dist):
        if not all(x == 1 for x in grid_shape):
            raise ValueError("grid shape should be all `1`'s not %s." %
                             grid_shape)
        return grid_shape

    if len(grid_shape) != len(dist):
        msg = "grid_shape's length (%d) not equal to dist's length (%d)"
        raise InvalidGridShapeError(msg % (len(grid_shape), len(dist)))
    if reduce(operator.mul, grid_shape, 1) != comm_size:
        msg = "grid shape %r not compatible with comm size of %d."
        raise InvalidGridShapeError(msg % (grid_shape, comm_size))
    return grid_shape

def make_grid_shape(shape, dist, comm_size):
    """ Generate a `grid_shape` from `shape` tuple and `dist` tuple.

    Does not assume that `dim_data` has `proc_grid_size` set for each
    dimension.

    Attempts to allocate processes optimally for distributed dimensions.

    Parameters
    ----------
    shape : tuple of int
        The global shape of the array.
    dist: tuple of str
        dist_type character per dimension.
    comm_size : int
        Total number of processes to distribute.

    Returns
    -------
    dist_grid_shape : tuple of int

    Raises
    ------
    GridShapeError
        if not possible to distribute `comm_size` processes over number of
        dimensions.
    """
    check_grid_shape_preconditions(shape, dist, comm_size)
    distdims = tuple(i for (i, v) in enumerate(dist) if v != 'n')
    ndistdim = len(distdims)

    if ndistdim == 0:
        dist_grid_shape = ()

    elif ndistdim == 1:
        # Trivial case: all processes used for the one distributed dimension.
        if distdims[0] >= shape[distdims[0]]:
            dist_grid_shape = (comm_size,)
        else:
            dist_grid_shape = (shape[distdims[0]],)

    elif comm_size == 1:
        # Trivial case: only one process to distribute over!
        dist_grid_shape = (1,) * ndistdim

    else:  # Main case: comm_size > 1, ndistdim > 1.
        factors = utils.mult_partitions(comm_size, ndistdim)
        if not factors:  # Can't factorize appropriately.
            raise GridShapeError("Cannot distribute array over processors.")

        reduced_shape = [shape[i] for i in distdims]

        # Reorder factors so they match the relative ordering in reduced_shape
        factors = [utils.mirror_sort(f, reduced_shape) for f in factors]

        # Pick the "best" factoring from `factors` according to which matches
        # the ratios among the dimensions in `shape`.
        rs_ratio = _compute_grid_ratios(reduced_shape)
        f_ratios = [_compute_grid_ratios(f) for f in factors]
        distances = [rs_ratio-f_ratio for f_ratio in f_ratios]
        norms = numpy.array([numpy.linalg.norm(d, 2) for d in distances])
        index = norms.argmin()
        # we now have the grid shape for the distributed dimensions.
        dist_grid_shape = tuple(int(i) for i in factors[index])

    # Create the grid_shape, all 1's for now.
    grid_shape = [1] * len(shape)

    # Fill grid_shape in the distdim slots using dist_grid_shape
    it = iter(dist_grid_shape)
    for distdim in distdims:
        grid_shape[distdim] = next(it)

    out_grid_shape = tuple(grid_shape)
    check_grid_shape_postconditions(out_grid_shape, shape, dist, comm_size)
    return out_grid_shape

def _compute_grid_ratios(shape):
    shape = tuple(map(float, shape))
    n = len(shape)
    ratios = []
    for (i, j) in product(range(n), range(n)):
        if i < j:
            ratios.append(shape[i] / shape[j])
    return numpy.array(ratios)

def normalize_dist(dist, ndim):
    """Return a tuple containing dist-type for each dimension.

    Parameters
    ----------
    dist : str, list, tuple, or dict
    ndim : int

    Returns
    -------
    tuple of str
        Contains string distribution type for each dim.

    Examples
    --------
    >>> normalize_dist({0: 'b', 3: 'c'}, 4)
    ('b', 'n', 'n', 'c')
    """
    if isinstance(dist, Sequence):
        return tuple(dist) + ('n',) * (ndim - len(dist))
    elif isinstance(dist, Mapping):
        return tuple(dist.get(i, 'n') for i in range(ndim))
    else:
        raise TypeError("Dist must be a string, tuple, list or dict")

def _start_stop_block(size, proc_grid_size, proc_grid_rank):
    """Return `start` and `stop` for a regularly distributed block dim."""
    nelements = size // proc_grid_size
    if size % proc_grid_size != 0:
        nelements += 1

    start = proc_grid_rank * nelements
    if start > size:
        start = size

    stop = start + nelements
    if stop > size:
        stop = size

    return start, stop

def distribute_block_indices(dd):
    """Fill in `start` and `stop` in dim dict `dd`."""
    if ('start' in dd) and ('stop' in dd):
        return
    else:
        dd['start'], dd['stop'] = _start_stop_block(dd['size'],
                                                    dd['proc_grid_size'],
                                                    dd['proc_grid_rank'])

def distribute_cyclic_indices(dd):
    """Fill in `start` in dim dict `dd`."""
    if 'start' in dd:
        return
    else:
        dd['start'] = dd['proc_grid_rank']

def distribute_indices(dd):
    """Fill in index related keys in dim dict `dd`."""
    dist_type = dd['dist_type']
    try:
        {'n': lambda dd: None,
         'b': distribute_block_indices,
         'c': distribute_cyclic_indices}[dist_type](dd)
    except KeyError:
        msg = "dist_type %r not supported."
        raise TypeError(msg % dist_type)

def normalize_dim_dict(dd):
    """Fill out some degenerate dim_dicts."""

    # TODO: Fill out empty dim_dict alias here?

    if dd['dist_type'] == 'n':
        dd['proc_grid_size'] = 1
        dd['proc_grid_rank'] = 0

def positivify(index, size):
    if 0 <= index < size:
        return index
    elif -size <= index < 0:
        return size + index
    else:
        raise IndexError("Index %s out of bounds" % index)

def normalize_reduction_axes(axes, ndim):
    if axes is None:
        axes = tuple(range(ndim))
    elif not isinstance(axes, Sequence):
        axes = (positivify(axes, ndim),)
    else:
        axes = tuple(positivify(a, ndim) for a in axes)
    return axes
<commit_msg>Fix small bugs in normalize_grid_shape and make_grid_shape<commit_after># encoding: utf-8
# ---------------------------------------------------------------------------
#  Copyright (C) 2008-2014, IPython Development Team and Enthought, Inc.
#  Distributed under the terms of the BSD License.  See COPYING.rst.
# ---------------------------------------------------------------------------

import operator
from itertools import product
from functools import reduce
from collections import Sequence, Mapping

import numpy

from distarray import utils
from distarray.externals.six import next
from distarray.externals.six.moves import map

class InvalidGridShapeError(Exception):
    pass

class GridShapeError(Exception):
    pass

def check_grid_shape_preconditions(shape, dist, comm_size):
    """
    Verify various distarray parameters are correct before making a grid_shape.
    """
    if comm_size < 1:
        raise ValueError("comm_size >= 1 not satisfied, comm_size = %s" %
                         (comm_size,))
    if len(shape) != len(dist):
        raise ValueError("len(shape) == len(dist) not satisfied, len(shape) ="
                         " %s and len(dist) = %s" % (len(shape), len(dist)))
    if any(i < 0 for i in shape):
        raise ValueError("shape must be a sequence of non-negative integers, "
                         "shape = %s" % (shape,))
    if any(i not in ('b', 'c', 'n', 'u') for i in dist):
        raise ValueError("dist must be a sequence of 'b', 'n', 'c', 'u' "
                         "strings, dist = %s" % (dist,))

def check_grid_shape_postconditions(grid_shape, shape, dist, comm_size):
    if not (len(grid_shape) == len(shape) == len(dist)):
        raise ValueError("len(gird_shape) == len(shape) == len(dist) not "
                         "satisfied, len(grid_shape) = %s and len(shape) = %s "
                         "and len(dist) = %s" % (len(grid_shape), len(shape),
                                                 len(dist)))
    if any(gs < 1 for gs in grid_shape):
        raise ValueError("all(gs >= 1 for gs in grid_shape) not satisfied, "
                         "grid_shape = %s" % (grid_shape,))
    if any(gs != 1 for (d, gs) in zip(dist, grid_shape) if d == 'n'):
        raise ValueError("all(gs == 1 for (d, gs) in zip(dist, grid_shape) if "
                         "d == 'n', not satified dist = %s and grid_shape = "
                         "%s" % (dist, grid_shape))
    if any(gs > s for (s, gs) in zip(shape, grid_shape) if s > 0):
        raise ValueError("all(gs <= s for (s, gs) in zip(shape, grid_shape) "
                         "if s > 0) not satisfied, shape = %s and grid_shape "
                         "= %s" % (shape, grid_shape))
    if reduce(operator.mul, grid_shape, 1) > comm_size:
        raise ValueError("reduce(operator.mul, grid_shape, 1) <= comm_size not"
                         " satisfied, grid_shape product = %s and comm_size = "
                         " %s" % (reduce(operator.mul, grid_shape, 1),
                                  comm_size))

def normalize_grid_shape(grid_shape, shape, dist, comm_size):
    """Adds 1s to grid_shape so it has `ndims` dimensions.  Validates
    `grid_shape` tuple against the `dist` tuple and `comm_size`.
    """
    def check_normalization_preconditions(grid_shape, dist):
        if any(i < 0 for i in grid_shape):
            raise ValueError("grid_shape must be a sequence of non-negative "
                             "integers, grid_shape = %s" % (grid_shape,))
        if len(grid_shape) > len(dist):
            raise ValueError("len(grid_shape) <= len(dist) not satisfied, "
                             "len(grid_shape) = %s and len(dist) = %s" %
                             (len(grid_shape), len(dist)))
    check_grid_shape_preconditions(shape, dist, comm_size)
    check_normalization_preconditions(grid_shape, dist)

    ndims = len(shape)
    grid_shape = tuple(grid_shape) + (1,) * (ndims - len(grid_shape))

    # short circuit for special case
    if all(x == 'n' for x in dist):
        if not all(x == 1 for x in grid_shape):
            raise ValueError("grid shape should be all `1`'s not %s." %
                             grid_shape)
        return grid_shape

    if len(grid_shape) != len(dist):
        msg = "grid_shape's length (%d) not equal to dist's length (%d)"
        raise InvalidGridShapeError(msg % (len(grid_shape), len(dist)))
    if reduce(operator.mul, grid_shape, 1) > comm_size:
        msg = "grid shape %r not compatible with comm size of %d."
        raise InvalidGridShapeError(msg % (grid_shape, comm_size))
    return grid_shape

def make_grid_shape(shape, dist, comm_size):
    """ Generate a `grid_shape` from `shape` tuple and `dist` tuple.

    Does not assume that `dim_data` has `proc_grid_size` set for each
    dimension.

    Attempts to allocate processes optimally for distributed dimensions.

    Parameters
    ----------
    shape : tuple of int
        The global shape of the array.
    dist: tuple of str
        dist_type character per dimension.
    comm_size : int
        Total number of processes to distribute.

    Returns
    -------
    dist_grid_shape : tuple of int

    Raises
    ------
    GridShapeError
        if not possible to distribute `comm_size` processes over number of
        dimensions.
    """
    check_grid_shape_preconditions(shape, dist, comm_size)
    distdims = tuple(i for (i, v) in enumerate(dist) if v != 'n')
    ndistdim = len(distdims)

    if ndistdim == 0:
        dist_grid_shape = ()

    elif ndistdim == 1:
        # Trivial case: all processes used for the one distributed dimension.
        if comm_size >= shape[distdims[0]]:
            dist_grid_shape = (comm_size,)
        else:
            dist_grid_shape = (shape[distdims[0]],)

    elif comm_size == 1:
        # Trivial case: only one process to distribute over!
        dist_grid_shape = (1,) * ndistdim

    else:  # Main case: comm_size > 1, ndistdim > 1.
        factors = utils.mult_partitions(comm_size, ndistdim)
        if not factors:  # Can't factorize appropriately.
            raise GridShapeError("Cannot distribute array over processors.")

        reduced_shape = [shape[i] for i in distdims]

        # Reorder factors so they match the relative ordering in reduced_shape
        factors = [utils.mirror_sort(f, reduced_shape) for f in factors]

        # Pick the "best" factoring from `factors` according to which matches
        # the ratios among the dimensions in `shape`.
        rs_ratio = _compute_grid_ratios(reduced_shape)
        f_ratios = [_compute_grid_ratios(f) for f in factors]
        distances = [rs_ratio-f_ratio for f_ratio in f_ratios]
        norms = numpy.array([numpy.linalg.norm(d, 2) for d in distances])
        index = norms.argmin()
        # we now have the grid shape for the distributed dimensions.
        dist_grid_shape = tuple(int(i) for i in factors[index])

    # Create the grid_shape, all 1's for now.
    grid_shape = [1] * len(shape)

    # Fill grid_shape in the distdim slots using dist_grid_shape
    it = iter(dist_grid_shape)
    for distdim in distdims:
        grid_shape[distdim] = next(it)

    out_grid_shape = tuple(grid_shape)
    check_grid_shape_postconditions(out_grid_shape, shape, dist, comm_size)
    return out_grid_shape

def _compute_grid_ratios(shape):
    shape = tuple(map(float, shape))
    n = len(shape)
    ratios = []
    for (i, j) in product(range(n), range(n)):
        if i < j:
            ratios.append(shape[i] / shape[j])
    return numpy.array(ratios)

def normalize_dist(dist, ndim):
    """Return a tuple containing dist-type for each dimension.

    Parameters
    ----------
    dist : str, list, tuple, or dict
    ndim : int

    Returns
    -------
    tuple of str
        Contains string distribution type for each dim.

    Examples
    --------
    >>> normalize_dist({0: 'b', 3: 'c'}, 4)
    ('b', 'n', 'n', 'c')
    """
    if isinstance(dist, Sequence):
        return tuple(dist) + ('n',) * (ndim - len(dist))
    elif isinstance(dist, Mapping):
        return tuple(dist.get(i, 'n') for i in range(ndim))
    else:
        raise TypeError("Dist must be a string, tuple, list or dict")

def _start_stop_block(size, proc_grid_size, proc_grid_rank):
    """Return `start` and `stop` for a regularly distributed block dim."""
    nelements = size // proc_grid_size
    if size % proc_grid_size != 0:
        nelements += 1

    start = proc_grid_rank * nelements
    if start > size:
        start = size

    stop = start + nelements
    if stop > size:
        stop = size

    return start, stop

def distribute_block_indices(dd):
    """Fill in `start` and `stop` in dim dict `dd`."""
    if ('start' in dd) and ('stop' in dd):
        return
    else:
        dd['start'], dd['stop'] = _start_stop_block(dd['size'],
                                                    dd['proc_grid_size'],
                                                    dd['proc_grid_rank'])

def distribute_cyclic_indices(dd):
    """Fill in `start` in dim dict `dd`."""
    if 'start' in dd:
        return
    else:
        dd['start'] = dd['proc_grid_rank']

def distribute_indices(dd):
    """Fill in index related keys in dim dict `dd`."""
    dist_type = dd['dist_type']
    try:
        {'n': lambda dd: None,
         'b': distribute_block_indices,
         'c': distribute_cyclic_indices}[dist_type](dd)
    except KeyError:
        msg = "dist_type %r not supported."
        raise TypeError(msg % dist_type)

def normalize_dim_dict(dd):
    """Fill out some degenerate dim_dicts."""

    # TODO: Fill out empty dim_dict alias here?

    if dd['dist_type'] == 'n':
        dd['proc_grid_size'] = 1
        dd['proc_grid_rank'] = 0

def positivify(index, size):
    if 0 <= index < size:
        return index
    elif -size <= index < 0:
        return size + index
    else:
        raise IndexError("Index %s out of bounds" % index)

def normalize_reduction_axes(axes, ndim):
    if axes is None:
        axes = tuple(range(ndim))
    elif not isinstance(axes, Sequence):
        axes = (positivify(axes, ndim),)
    else:
        axes = tuple(positivify(a, ndim) for a in axes)
    return axes
<|endoftext|>


<commit_before>"""
@given:
-------
a key for the machine       --> given_key

@when:
------
I type "{name}" as machine name     --> type_machine_name
I click the "{name}" machine        --> click_machine
I add my bare metal creds       --> aad_bare_metal
I click the "{name}" from the associated keys       --> click_associated_key
I tick the "{name}" machine     --> tick_machine

@then:
------
"{machine_name}" state should be "{state}" within {timeout} seconds      --> check_machine_state
"{machine_name}" should be probed within {timeout} seconds      --> check_probed
I should find the Public IP     --> find_ip
I should see the Bare Metal Backend added within {timeout} seconds      --> see_bare_added
I should see {number} keys associated within {timeout} seconds   --> keys_associated

------

"""
from time import sleep, time

ip = ""

@given(u'a key for the machine')
def given_key(context):
    key = context.personas['NinjaTester']['key_name']

    context.execute_steps(u"""
        When I click the "Keys" button
        And I wait for 2 seconds
        And I click the "Add" button
        And I type "%s" as key name
        And I click the "Generate" button
        And I wait for 5 seconds
        And I click the "Add" button within "add-key-popup" panel
        And I wait for 5 seconds
            Then I should see the "%s" Key added within 5 seconds
    """ % (key, key))

@when(u'I type "{name}" as machine name')
def type_machine_name(context, name):
    if name == "tester":
        machine_name = context.personas['NinjaTester']['machine_name']
        context.browser.find_by_css('input#create-machine-name').fill(machine_name)
    elif name == "monitor_tester":
        machine_name = context.personas['MonitorTester']['machine_name']
        context.browser.find_by_css('input#create-machine-name').fill(machine_name)
    elif name == "shell_tester":
        machine_name = context.personas['ShellTester']['machine_name']
        context.browser.find_by_css('input#create-machine-name').fill(machine_name)
    else:
        context.browser.find_by_css('input#create-machine-name').fill(name)

@then(u'"{machine_name}" state should be "{state}" within {timeout} seconds')
def check_machine_state(context, machine_name, state, timeout):
    if machine_name == "tester":
        machine_name = context.personas['NinjaTester']['machine_name']
    elif machine_name == "monitor_tester":
        machine_name = context.personas['MonitorTester']['machine_name']
    elif machine_name == "shell_tester":
        machine_name = context.personas['ShellTester']['machine_name']

    end_time = time() + int(timeout)
    while time() < end_time:
        try:
            machines = context.browser.find_by_css('#machines li')
            for machine in machines:
                if machine_name in machine.text:
                    break
            if state in machine.text:
                return
            sleep(2)
        except:
            pass

    assert False, u'Could not find %s state for machine %s' % (state, machine_name)

@then(u'"{machine_name}" should be probed within {timeout} seconds')
def check_probed(context, machine_name, timeout):

    if machine_name == "tester":
        machine_name = context.personas['NinjaTester']['machine_name']
    elif machine_name == "monitor_tester":
        machine_name = context.personas['MonitorTester']['machine_name']
    elif machine_name == "shell_tester":
        machine_name = context.personas['ShellTester']['machine_name']

    end_time = time() + int(timeout)
    while time() < end_time:
        try:
            machines = context.browser.find_by_css('#machines li')
            for machine in machines:
                if machine_name in machine.text:
                    break

            leds = machine.find_by_css('a .machine-leds > div')
            if leds.has_class('probed'):
                return
        except:
            machines = context.browser.find_by_css('#machines li')
            for machine in machines:
                if machine_name in machine.text:
                    break
            leds = machine.find_by_css('a .machine-leds > div')

        sleep(2)

    assert False, u'%s machine is not probed within %s seconds' % (machine_name, timeout)

@when(u'I click the "{name}" machine')
def click_machine(context, name):
    if name == "tester":
        context.execute_steps(u"""
        When I click the "%s" button
        """ % context.personas['NinjaTester']['machine_name'])
    elif name == "monitor_tester":
        context.execute_steps(u"""
        When I click the "%s" button
        """ % context.personas['MonitorTester']['machine_name'])
    elif name == "shell_tester":
        context.execute_steps(u"""
        When I click the "%s" button
        """ % context.personas['ShellTester']['machine_name'])

@then(u'I should find the Public IP')
def find_ip(context):
    global ip
    infos = context.browser.find_by_css('.ui-collapsible tr td')

    for i in range(len(infos)):
        if "Public" in infos[i].text:
            ip = infos[i+1].text

@when(u'I add my bare metal creds')
def aad_bare_metal(context):
    global ip
    hostname = ip
    key = context.personas['NinjaTester']['key_name']

    context.browser.find_by_css('input#new-backend-first-field').fill(hostname)
    context.browser.find_by_css('input#new-backend-second-field').fill("ec2-user")

@then(u'I should see the Bare Metal Backend added within {timeout} seconds')
def see_bare_added(context, timeout):
    global ip

    hostname = ip
    context.execute_steps(u"""
    Then I should see the "%s" Backend added within %s seconds
    """ % (hostname, timeout))

@then(u'I should see {number} keys associated within {timeout} seconds')
def keys_associated(context, number, timeout):

    end_time = time() + int(timeout)
    while time() < end_time:
        keys = context.browser.find_by_css('#machine-keys-panel ul .small-list-item')
        if len(keys) == int(number):
            return

    assert False, u'Could not find %s keys associated' % number

@when(u'I click the "{name}" from the associated keys')
def click_associated_key(context, name):
    keys = context.browser.find_by_css('#machine-keys-panel ul .small-list-item')

    for key in keys:
        if name in key.text:
            key.click()
            return

    assert False, u'Could not find/click the %s key' % name

@when(u'I tick the "{name}" machine')
def tick_machine(context, name):
    if name == "tester":
        machine_name = context.personas['NinjaTester']['machine_name']
    else:
        machine_name = name

    machines = context.browser.find_by_css('.ui-listview li')

    for machine in machines:
        if machine_name in machine.text.strip():
            break

    try:
        if machine_name in machine.text.strip():
            machine.find_by_css('.ui-btn')[0].click()
        else:
            machines = context.browser.find_by_css('.ui-listview li')

            for machine in machines:
                if machine_name in machine.text.strip():
                    break
            machine.find_by_css('.ui-btn')[0].click()
    except:
        assert False, u'Could not tick/check %s machine' % machine_name<commit_msg>Fix Bare Metal tests when finding Public IP<commit_after>"""
@given:
-------
a key for the machine       --> given_key

@when:
------
I type "{name}" as machine name     --> type_machine_name
I click the "{name}" machine        --> click_machine
I add my bare metal creds       --> aad_bare_metal
I click the "{name}" from the associated keys       --> click_associated_key
I tick the "{name}" machine     --> tick_machine

@then:
------
"{machine_name}" state should be "{state}" within {timeout} seconds      --> check_machine_state
"{machine_name}" should be probed within {timeout} seconds      --> check_probed
I should find the Public IP     --> find_ip
I should see the Bare Metal Backend added within {timeout} seconds      --> see_bare_added
I should see {number} keys associated within {timeout} seconds   --> keys_associated

------

"""
from time import sleep, time

ip = ""

@given(u'a key for the machine')
def given_key(context):
    key = context.personas['NinjaTester']['key_name']

    context.execute_steps(u"""
        When I click the "Keys" button
        And I wait for 2 seconds
        And I click the "Add" button
        And I type "%s" as key name
        And I click the "Generate" button
        And I wait for 5 seconds
        And I click the "Add" button within "add-key-popup" panel
        And I wait for 5 seconds
            Then I should see the "%s" Key added within 5 seconds
    """ % (key, key))

@when(u'I type "{name}" as machine name')
def type_machine_name(context, name):
    if name == "tester":
        machine_name = context.personas['NinjaTester']['machine_name']
        context.browser.find_by_css('input#create-machine-name').fill(machine_name)
    elif name == "monitor_tester":
        machine_name = context.personas['MonitorTester']['machine_name']
        context.browser.find_by_css('input#create-machine-name').fill(machine_name)
    elif name == "shell_tester":
        machine_name = context.personas['ShellTester']['machine_name']
        context.browser.find_by_css('input#create-machine-name').fill(machine_name)
    else:
        context.browser.find_by_css('input#create-machine-name').fill(name)

@then(u'"{machine_name}" state should be "{state}" within {timeout} seconds')
def check_machine_state(context, machine_name, state, timeout):
    if machine_name == "tester":
        machine_name = context.personas['NinjaTester']['machine_name']
    elif machine_name == "monitor_tester":
        machine_name = context.personas['MonitorTester']['machine_name']
    elif machine_name == "shell_tester":
        machine_name = context.personas['ShellTester']['machine_name']

    end_time = time() + int(timeout)
    while time() < end_time:
        try:
            machines = context.browser.find_by_css('#machines li')
            for machine in machines:
                if machine_name in machine.text:
                    break
            if state in machine.text:
                return
            sleep(2)
        except:
            pass

    assert False, u'Could not find %s state for machine %s' % (state, machine_name)

@then(u'"{machine_name}" should be probed within {timeout} seconds')
def check_probed(context, machine_name, timeout):

    if machine_name == "tester":
        machine_name = context.personas['NinjaTester']['machine_name']
    elif machine_name == "monitor_tester":
        machine_name = context.personas['MonitorTester']['machine_name']
    elif machine_name == "shell_tester":
        machine_name = context.personas['ShellTester']['machine_name']

    end_time = time() + int(timeout)
    while time() < end_time:
        try:
            machines = context.browser.find_by_css('#machines li')
            for machine in machines:
                if machine_name in machine.text:
                    break

            leds = machine.find_by_css('a .machine-leds > div')
            if leds.has_class('probed'):
                return
        except:
            machines = context.browser.find_by_css('#machines li')
            for machine in machines:
                if machine_name in machine.text:
                    break
            leds = machine.find_by_css('a .machine-leds > div')

        sleep(2)

    assert False, u'%s machine is not probed within %s seconds' % (machine_name, timeout)

@when(u'I click the "{name}" machine')
def click_machine(context, name):
    if name == "tester":
        context.execute_steps(u"""
        When I click the "%s" button
        """ % context.personas['NinjaTester']['machine_name'])
    elif name == "monitor_tester":
        context.execute_steps(u"""
        When I click the "%s" button
        """ % context.personas['MonitorTester']['machine_name'])
    elif name == "shell_tester":
        context.execute_steps(u"""
        When I click the "%s" button
        """ % context.personas['ShellTester']['machine_name'])

@then(u'I should find the Public IP')
def find_ip(context):
    global ip
    try:
        infos = context.browser.find_by_css('.ui-collapsible tr td')

        for i in range(len(infos)):
            if "Public" in infos[i].text:
                ip = infos[i+1].text
    except:
        sleep(2)
        infos = context.browser.find_by_css('.ui-collapsible tr td')

        for i in range(len(infos)):
            if "Public" in infos[i].text:
                ip = infos[i+1].text

@when(u'I add my bare metal creds')
def aad_bare_metal(context):
    global ip
    hostname = ip
    key = context.personas['NinjaTester']['key_name']

    context.browser.find_by_css('input#new-backend-first-field').fill(hostname)
    context.browser.find_by_css('input#new-backend-second-field').fill("ec2-user")

@then(u'I should see the Bare Metal Backend added within {timeout} seconds')
def see_bare_added(context, timeout):
    global ip

    hostname = ip
    context.execute_steps(u"""
    Then I should see the "%s" Backend added within %s seconds
    """ % (hostname, timeout))

@then(u'I should see {number} keys associated within {timeout} seconds')
def keys_associated(context, number, timeout):

    end_time = time() + int(timeout)
    while time() < end_time:
        keys = context.browser.find_by_css('#machine-keys-panel ul .small-list-item')
        if len(keys) == int(number):
            return

    assert False, u'Could not find %s keys associated' % number

@when(u'I click the "{name}" from the associated keys')
def click_associated_key(context, name):
    keys = context.browser.find_by_css('#machine-keys-panel ul .small-list-item')

    for key in keys:
        if name in key.text:
            key.click()
            return

    assert False, u'Could not find/click the %s key' % name

@when(u'I tick the "{name}" machine')
def tick_machine(context, name):
    if name == "tester":
        machine_name = context.personas['NinjaTester']['machine_name']
    else:
        machine_name = name

    machines = context.browser.find_by_css('.ui-listview li')

    for machine in machines:
        if machine_name in machine.text.strip():
            break

    try:
        if machine_name in machine.text.strip():
            machine.find_by_css('.ui-btn')[0].click()
        else:
            machines = context.browser.find_by_css('.ui-listview li')

            for machine in machines:
                if machine_name in machine.text.strip():
                    break
            machine.find_by_css('.ui-btn')[0].click()
    except:
        assert False, u'Could not tick/check %s machine' % machine_name<|endoftext|>


<commit_before># The MIT License (MIT)
#
# Copyright (c) 2014-2015 WUSTL ZPLAB
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
#
# Authors: Erik Hvatum <ice.rikh@gmail.com>

from contextlib import ExitStack
import ctypes
import numpy
from pathlib import Path
import OpenGL
import OpenGL.GL as PyGL
import OpenGL.GL.NV.path_rendering as PR
from PyQt5 import Qt
from ..shared_resources import UNIQUE_QGRAPHICSITEM_TYPE

c_float32_p = ctypes.POINTER(ctypes.c_float)
c_uint8_p = ctypes.POINTER(ctypes.c_uint8)

class ContextualInfoItemNV(Qt.QGraphicsObject):
    QGRAPHICSITEM_TYPE = UNIQUE_QGRAPHICSITEM_TYPE()
    FIXED_POSITION_IN_VIEW = Qt.QPoint(10, 5)

    text_changed = Qt.pyqtSignal()

    def __init__(self, parent_item=None):
        super().__init__(parent_item)
        self.setFlag(Qt.QGraphicsItem.ItemIgnoresTransformations)
#       self._font = Qt.QFont('Courier', pointSize=16, weight=Qt.QFont.Bold)
#       self._font.setKerning(False)
#       self._font.setStyleHint(Qt.QFont.Monospace, Qt.QFont.OpenGLCompatible | Qt.QFont.PreferQuality)
#       self._pen = Qt.QPen(Qt.QColor(Qt.Qt.black))
#       self._pen.setWidth(2)
#       self._pen.setCosmetic(True)
#       self._brush = Qt.QBrush(Qt.QColor(45,255,70,255))
        self._text = None
        self._text_serial = 0
        self._text_lines_encoded = None
        self._text_lines_kerning = None
        self._glyph_base = None
        self._path = None
        self._path_serial = None
#       self._text_flags = Qt.Qt.AlignLeft | Qt.Qt.AlignTop | Qt.Qt.AlignAbsolute
        self._bounding_rect = None
        # Necessary to prevent context information from disappearing when mouse pointer passes over
        # context info text
        self.setAcceptHoverEvents(False)
        self.setAcceptedMouseButtons(Qt.Qt.NoButton)
        # Info text generally should appear over anything else rather than z-fighting
        self.setZValue(10)
        self.hide()

    def __del__(self):
        scene = self.scene()
        if scene is None:
            return
        views = scene.views()
        if not views:
            return
        view = views[0]
        gl_widget = view.gl_widget
        context = gl_widget.context()
        if not context:
            return
        gl_widget.makeCurrent()
        try:
            if self._glyph_base is not None:
                PR.glDeletePathsNV(self._glyph_base, 256)
        finally:
            gl_widget.doneCurrent()

    def type(self):
        return ContextualInfoItem.QGRAPHICSITEM_TYPE

    def boundingRect(self):
        if 0:#self._text:
            self._update_picture()
            return self._bounding_rect
        else:
            # TODO: compute bounding rect in _update_paths()
            return Qt.QRectF(0,0,1,1)

    def paint(self, qpainter, option, widget):
        with ExitStack() as estack:
            qpainter.beginNativePainting()
            estack.callback(qpainter.endNativePainting)
            self._update_paths()
            PyGL.glClearStencil(0)
            PyGL.glClearColor(0,0,0,0)
            PyGL.glStencilMask(~0)
            PyGL.glEnable(PyGL.GL_STENCIL_TEST)
            PyGL.glStencilFunc(PyGL.GL_NOTEQUAL, 0, 0x1)
            PyGL.glStencilOp(PyGL.GL_KEEP, PyGL.GL_KEEP, PyGL.GL_ZERO)
            PyGL.glPushMatrix()
            estack.callback(PyGL.glPopMatrix)
            # TODO: determine why Y axis needs to be inverted for NV_path_drawing
            PyGL.glScale(1,-1,1)
            # TODO: If flipping does turn out to be necessary, translate down by total text height as computed via
            # font metrics from path rendering extension rather than by the fixed number -35
            LINE_OFFSET = -35

            for l, k in zip(self._text_lines_encoded, self._text_lines_kerning):
                PyGL.glTranslate(0,LINE_OFFSET,0)
                l_len = l.shape[0] - 1
                l_p = l.ctypes.data_as(c_uint8_p)
                k_p = k.ctypes.data_as(c_float32_p)

                # Draw text outline
                PR.glStencilStrokePathInstancedNV(
                    l_len, PyGL.GL_UNSIGNED_BYTE, l_p,
                    self._glyph_base,
                    1, ~0,
                    PR.GL_TRANSLATE_X_NV, k_p)
                PyGL.glColor3f(0,0,0)
                PR.glCoverStrokePathInstancedNV(
                    l_len, PyGL.GL_UNSIGNED_BYTE, l_p,
                    self._glyph_base,
                    PR.GL_BOUNDING_BOX_OF_BOUNDING_BOXES_NV,
                    PR.GL_TRANSLATE_X_NV, k_p)

                # Draw filled text interiors
                PR.glStencilFillPathInstancedNV(
                    l_len, PyGL.GL_UNSIGNED_BYTE, l_p,
                    self._glyph_base,
                    PR.GL_PATH_FILL_MODE_NV, ~0,
                    PR.GL_TRANSLATE_X_NV, k_p)
                PyGL.glColor3f(0,1,0)
                PR.glCoverFillPathInstancedNV(
                    l_len, PyGL.GL_UNSIGNED_BYTE, l_p,
                    self._glyph_base,
                    PR.GL_BOUNDING_BOX_OF_BOUNDING_BOXES_NV,
                    PR.GL_TRANSLATE_X_NV, k_p)

    def return_to_fixed_position(self, view):
        """Maintain position self.FIXED_POSITION_IN_VIEW relative to view's top left corner."""
        topleft = self.FIXED_POSITION_IN_VIEW
        if view.mapFromScene(self.pos()) != topleft:
            self.setPos(view.mapToScene(topleft))

    def _update_paths(self):
        if self._path is None:
            self._path = PR.glGenPathsNV(1)
            _ = numpy.zeros((10,), dtype=numpy.uint8)
            PR.glPathCommandsNV(self._path, 0, _.ctypes.data_as(c_uint8_p), 0, PyGL.GL_FLOAT, _.ctypes.data_as(c_float32_p))
            PR.glPathParameterfNV(self._path, PR.GL_PATH_STROKE_WIDTH_NV, 3.2)
            PR.glPathParameteriNV(self._path, PR.GL_PATH_JOIN_STYLE_NV, PR.GL_ROUND_NV)
        if self._glyph_base is None:
            glyph_base = PR.glGenPathsNV(256)
            for fontname in ("Liberation Mono", "Courier", "Terminal"):
                PR.glPathGlyphRangeNV(
                    glyph_base,
                    PR.GL_SYSTEM_FONT_NAME_NV, fontname, PR.GL_BOLD_BIT_NV,
                    0, 256,
                    PR.GL_SKIP_MISSING_GLYPH_NV, self._path, 32)
            PR.glPathGlyphRangeNV(
                    glyph_base,
                    PR.GL_SYSTEM_FONT_NAME_NV, fontname, PR.GL_BOLD_BIT_NV,
                    0, 256,
                    PR.GL_USE_MISSING_GLYPH_NV, self._path, 32)
            # TODO: determine why a) glGetPathMetricRangeNV gives NaN for offsets when called with
            # PR.GL_FONT_Y_MIN_BOUNDS_BIT_NV | PR.GL_FONT_Y_MAX_BOUNDS_BIT_NV b) whether individually computed
            # offsets vary with current projection and/or model and/or view matrices and if not, why results
            # are so different for histogram and general context info items
#           self._glyph_y_minmax_bounds = numpy.empty((2,), dtype=numpy.float32)
#           PR.glGetPathMetricRangeNV(
#               PR.GL_FONT_Y_MIN_BOUNDS_BIT_NV,
#               self._glyph_base, 1,
#               8,#self._glyph_y_minmax_bounds.nbytes,
#               self._glyph_y_minmax_bounds.ctypes.data_as(c_float32_p))
#           print('min', self._glyph_y_minmax_bounds)
#           PR.glGetPathMetricRangeNV(
#               PR.GL_FONT_Y_MAX_BOUNDS_BIT_NV,
#               self._glyph_base, 1,
#               8,#self._glyph_y_minmax_bounds.nbytes,
#               self._glyph_y_minmax_bounds.ctypes.data_as(c_float32_p))
#           print('max', self._glyph_y_minmax_bounds)
            self._glyph_base = glyph_base
        if self._path_serial != self._text_serial:
            self._text_lines_kerning = []
            for l in self._text_lines_encoded:
                tk = numpy.empty(l.shape, dtype=numpy.float32)
                tk[0] = 0
                tko = tk[1:]
                PR.glGetPathSpacingNV(
                    PR.GL_ACCUM_ADJACENT_PAIRS_NV,
                    l.shape[0], PyGL.GL_UNSIGNED_BYTE, l.ctypes.data_as(c_uint8_p),
                    self._glyph_base,
                    1.0, 1.0, PR.GL_TRANSLATE_X_NV,
                    tko.ctypes.data_as(c_float32_p))
                self._text_lines_kerning.append(tk)
            self._path_serial = self._text_serial

    @property
    def text(self):
        return self._text

    @text.setter
    def text(self, v):
        if self._text != v:
            if v:
                # Only ISO-8859-1 is supported at the moment.
                # The '&' is appended owing to a peculiarity of the glGetPathSpacingNV call, which wants a junk character
                # at the end of each line.  The '&' is not actually visible.
                lines_encoded = [numpy.array(list((l + '&').encode('ISO-8859-1')), dtype=numpy.uint8) for l in v.split('\n')]
                self.prepareGeometryChange()
                self._text_lines_encoded = lines_encoded
            else:
                self._text_lines_encoded = None
                self._text_lines_kerning = None
            self._text = v
            self._text_serial += 1
            if self._text:
                self.show()
                self.update()
            else:
                self.hide()
            self.text_changed.emit()

#   @property
#   def font(self):
#       return self._font
#
#   @font.setter
#   def font(self, v):
#       assert isinstance(v, Qt.QFont)
#       self._font = v
#       self._picture = None
#       self.update()
#
#   @property
#   def pen(self):
#       """The pen used to draw text outline to provide contrast against any background.  If None,
#       outline is not drawn."""
#       return self._pen
#
#   @pen.setter
#   def pen(self, v):
#       assert isinstance(v, Qt.QPen) or v is None
#       self._pen = v
#       self._picture = None
#       self.update()
#
#   @property
#   def brush(self):
#       """The brush used to fill text.  If None, text is not filled."""
#       return self._brush
#
#   @brush.setter
#   def brush(self, v):
#       assert isinstance(v, Qt.QBrush) or v is None
#       self._brush = v
#       self._picture = None
#       self.update()
<commit_msg>Tweak parameters for scope machine.<commit_after># The MIT License (MIT)
#
# Copyright (c) 2014-2015 WUSTL ZPLAB
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
#
# Authors: Erik Hvatum <ice.rikh@gmail.com>

from contextlib import ExitStack
import ctypes
import numpy
from pathlib import Path
import OpenGL
import OpenGL.GL as PyGL
import OpenGL.GL.NV.path_rendering as PR
from PyQt5 import Qt
from ..shared_resources import UNIQUE_QGRAPHICSITEM_TYPE

c_float32_p = ctypes.POINTER(ctypes.c_float)
c_uint8_p = ctypes.POINTER(ctypes.c_uint8)

class ContextualInfoItemNV(Qt.QGraphicsObject):
    QGRAPHICSITEM_TYPE = UNIQUE_QGRAPHICSITEM_TYPE()
    FIXED_POSITION_IN_VIEW = Qt.QPoint(10, 5)

    text_changed = Qt.pyqtSignal()

    def __init__(self, parent_item=None):
        super().__init__(parent_item)
        self.setFlag(Qt.QGraphicsItem.ItemIgnoresTransformations)
#       self._font = Qt.QFont('Courier', pointSize=16, weight=Qt.QFont.Bold)
#       self._font.setKerning(False)
#       self._font.setStyleHint(Qt.QFont.Monospace, Qt.QFont.OpenGLCompatible | Qt.QFont.PreferQuality)
#       self._pen = Qt.QPen(Qt.QColor(Qt.Qt.black))
#       self._pen.setWidth(2)
#       self._pen.setCosmetic(True)
#       self._brush = Qt.QBrush(Qt.QColor(45,255,70,255))
        self._text = None
        self._text_serial = 0
        self._text_lines_encoded = None
        self._text_lines_kerning = None
        self._glyph_base = None
        self._path = None
        self._path_serial = None
#       self._text_flags = Qt.Qt.AlignLeft | Qt.Qt.AlignTop | Qt.Qt.AlignAbsolute
        self._bounding_rect = None
        # Necessary to prevent context information from disappearing when mouse pointer passes over
        # context info text
        self.setAcceptHoverEvents(False)
        self.setAcceptedMouseButtons(Qt.Qt.NoButton)
        # Info text generally should appear over anything else rather than z-fighting
        self.setZValue(10)
        self.hide()

    def __del__(self):
        scene = self.scene()
        if scene is None:
            return
        views = scene.views()
        if not views:
            return
        view = views[0]
        gl_widget = view.gl_widget
        context = gl_widget.context()
        if not context:
            return
        gl_widget.makeCurrent()
        try:
            if self._glyph_base is not None:
                PR.glDeletePathsNV(self._glyph_base, 256)
        finally:
            gl_widget.doneCurrent()

    def type(self):
        return ContextualInfoItem.QGRAPHICSITEM_TYPE

    def boundingRect(self):
        if 0:#self._text:
            self._update_picture()
            return self._bounding_rect
        else:
            # TODO: compute bounding rect in _update_paths()
            return Qt.QRectF(0,0,1,1)

    def paint(self, qpainter, option, widget):
        with ExitStack() as estack:
            qpainter.beginNativePainting()
            estack.callback(qpainter.endNativePainting)
            self._update_paths()
            PyGL.glClearStencil(0)
            PyGL.glClearColor(0,0,0,0)
            PyGL.glStencilMask(~0)
            PyGL.glEnable(PyGL.GL_STENCIL_TEST)
            PyGL.glStencilFunc(PyGL.GL_NOTEQUAL, 0, 0x1)
            PyGL.glStencilOp(PyGL.GL_KEEP, PyGL.GL_KEEP, PyGL.GL_ZERO)
            PyGL.glPushMatrix()
            estack.callback(PyGL.glPopMatrix)
            # TODO: determine why Y axis needs to be inverted for NV_path_drawing
            PyGL.glScale(1,-1,1)
            # TODO: If flipping does turn out to be necessary, translate down by total text height as computed via
            # font metrics from path rendering extension rather than by the fixed number -35
            LINE_OFFSET = -22

            for l, k in zip(self._text_lines_encoded, self._text_lines_kerning):
                PyGL.glTranslate(0,LINE_OFFSET,0)
                l_len = l.shape[0] - 1
                l_p = l.ctypes.data_as(c_uint8_p)
                k_p = k.ctypes.data_as(c_float32_p)

                # Draw text outline
                PR.glStencilStrokePathInstancedNV(
                    l_len, PyGL.GL_UNSIGNED_BYTE, l_p,
                    self._glyph_base,
                    1, ~0,
                    PR.GL_TRANSLATE_X_NV, k_p)
                PyGL.glColor3f(0,0,0)
                PR.glCoverStrokePathInstancedNV(
                    l_len, PyGL.GL_UNSIGNED_BYTE, l_p,
                    self._glyph_base,
                    PR.GL_BOUNDING_BOX_OF_BOUNDING_BOXES_NV,
                    PR.GL_TRANSLATE_X_NV, k_p)

                # Draw filled text interiors
                PR.glStencilFillPathInstancedNV(
                    l_len, PyGL.GL_UNSIGNED_BYTE, l_p,
                    self._glyph_base,
                    PR.GL_PATH_FILL_MODE_NV, ~0,
                    PR.GL_TRANSLATE_X_NV, k_p)
                PyGL.glColor3f(0,1,0)
                PR.glCoverFillPathInstancedNV(
                    l_len, PyGL.GL_UNSIGNED_BYTE, l_p,
                    self._glyph_base,
                    PR.GL_BOUNDING_BOX_OF_BOUNDING_BOXES_NV,
                    PR.GL_TRANSLATE_X_NV, k_p)

    def return_to_fixed_position(self, view):
        """Maintain position self.FIXED_POSITION_IN_VIEW relative to view's top left corner."""
        topleft = self.FIXED_POSITION_IN_VIEW
        if view.mapFromScene(self.pos()) != topleft:
            self.setPos(view.mapToScene(topleft))

    def _update_paths(self):
        if self._path is None:
            self._path = PR.glGenPathsNV(1)
            _ = numpy.zeros((10,), dtype=numpy.uint8)
            PR.glPathCommandsNV(self._path, 0, _.ctypes.data_as(c_uint8_p), 0, PyGL.GL_FLOAT, _.ctypes.data_as(c_float32_p))
            PR.glPathParameterfNV(self._path, PR.GL_PATH_STROKE_WIDTH_NV, 3.0)
            PR.glPathParameteriNV(self._path, PR.GL_PATH_JOIN_STYLE_NV, PR.GL_ROUND_NV)
        if self._glyph_base is None:
            glyph_base = PR.glGenPathsNV(256)
            for fontname in ("Liberation Mono", "Courier", "Terminal"):
                PR.glPathGlyphRangeNV(
                    glyph_base,
                    PR.GL_SYSTEM_FONT_NAME_NV, fontname, PR.GL_BOLD_BIT_NV,
                    0, 256,
                    PR.GL_SKIP_MISSING_GLYPH_NV, self._path, 20)
            PR.glPathGlyphRangeNV(
                    glyph_base,
                    PR.GL_SYSTEM_FONT_NAME_NV, fontname, PR.GL_BOLD_BIT_NV,
                    0, 256,
                    PR.GL_USE_MISSING_GLYPH_NV, self._path, 20)
            # TODO: determine why a) glGetPathMetricRangeNV gives NaN for offsets when called with
            # PR.GL_FONT_Y_MIN_BOUNDS_BIT_NV | PR.GL_FONT_Y_MAX_BOUNDS_BIT_NV b) whether individually computed
            # offsets vary with current projection and/or model and/or view matrices and if not, why results
            # are so different for histogram and general context info items
#           self._glyph_y_minmax_bounds = numpy.empty((2,), dtype=numpy.float32)
#           PR.glGetPathMetricRangeNV(
#               PR.GL_FONT_Y_MIN_BOUNDS_BIT_NV,
#               self._glyph_base, 1,
#               8,#self._glyph_y_minmax_bounds.nbytes,
#               self._glyph_y_minmax_bounds.ctypes.data_as(c_float32_p))
#           print('min', self._glyph_y_minmax_bounds)
#           PR.glGetPathMetricRangeNV(
#               PR.GL_FONT_Y_MAX_BOUNDS_BIT_NV,
#               self._glyph_base, 1,
#               8,#self._glyph_y_minmax_bounds.nbytes,
#               self._glyph_y_minmax_bounds.ctypes.data_as(c_float32_p))
#           print('max', self._glyph_y_minmax_bounds)
            self._glyph_base = glyph_base
        if self._path_serial != self._text_serial:
            self._text_lines_kerning = []
            for l in self._text_lines_encoded:
                tk = numpy.empty(l.shape, dtype=numpy.float32)
                tk[0] = 0
                tko = tk[1:]
                PR.glGetPathSpacingNV(
                    PR.GL_ACCUM_ADJACENT_PAIRS_NV,
                    l.shape[0], PyGL.GL_UNSIGNED_BYTE, l.ctypes.data_as(c_uint8_p),
                    self._glyph_base,
                    1.0, 1.0, PR.GL_TRANSLATE_X_NV,
                    tko.ctypes.data_as(c_float32_p))
                self._text_lines_kerning.append(tk)
            self._path_serial = self._text_serial

    @property
    def text(self):
        return self._text

    @text.setter
    def text(self, v):
        if self._text != v:
            if v:
                # Only ISO-8859-1 is supported at the moment.
                # The '&' is appended owing to a peculiarity of the glGetPathSpacingNV call, which wants a junk character
                # at the end of each line.  The '&' is not actually visible.
                lines_encoded = [numpy.array(list((l + '&').encode('ISO-8859-1')), dtype=numpy.uint8) for l in v.split('\n')]
                self.prepareGeometryChange()
                self._text_lines_encoded = lines_encoded
            else:
                self._text_lines_encoded = None
                self._text_lines_kerning = None
            self._text = v
            self._text_serial += 1
            if self._text:
                self.show()
                self.update()
            else:
                self.hide()
            self.text_changed.emit()

#   @property
#   def font(self):
#       return self._font
#
#   @font.setter
#   def font(self, v):
#       assert isinstance(v, Qt.QFont)
#       self._font = v
#       self._picture = None
#       self.update()
#
#   @property
#   def pen(self):
#       """The pen used to draw text outline to provide contrast against any background.  If None,
#       outline is not drawn."""
#       return self._pen
#
#   @pen.setter
#   def pen(self, v):
#       assert isinstance(v, Qt.QPen) or v is None
#       self._pen = v
#       self._picture = None
#       self.update()
#
#   @property
#   def brush(self):
#       """The brush used to fill text.  If None, text is not filled."""
#       return self._brush
#
#   @brush.setter
#   def brush(self, v):
#       assert isinstance(v, Qt.QBrush) or v is None
#       self._brush = v
#       self._picture = None
#       self.update()
<|endoftext|>


<commit_before>#!/usr/bin/env python
#################################################################################
# Copyright 2018 ROBOTIS CO., LTD.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
# Modified clc so that frame of reference is always straight ahead.
# I was unable to find a good target with a high reflective intensity,
# so the parking target is between two towers, e.g. stacked cans.
#################################################################################

import rospy
import sys
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
from std_msgs.msg import Empty
from teleop_service.msg import Behavior,TeleopStatus
import numpy as np
import math
import time

MAX_ANGLE   = 0.5
MAX_SPEED   = 0.2
ROBOT_WIDTH = 0.2     # Robot width ~ m
TOLERANCE   = 0.04
PILLAR_WIDTH = 0.09    # Approx tower width ~ m
START_OFFSET= 1.0     # Dist from left tower to start ~ m
IGNORE      = 0.02    # Ignore any distances less than this
INFINITY    = 10.

# A pillar describes the position of a target pillar in coordinates of the LIDAR
# measurements. It carries attributes used in the calculation.
class Pillar:
	def __init__(self):
		self.valid = False
		self.dist = INFINITY
		self.angle= 0.0
		self.width = 0.0
		self.d1 = INFINITY
		self.d2 = INFINITY
		self.a1 = 0.0
		self.a2 = 0.0
		self.inArc = False

	def clone(pillar):
		self.valid = pillar.valid
		self.dist = pillar.dist
		self.angle= pillar.angle
		self.width = pillar.width
		self.d1 = pillar.d1
		self.d2 = pillar.d2
		self.a1 = pillar.a1
		self.a2 = pillar.a2
		self.inArc = pillar.inArc

	# Point represents the start of a group
	def start(dist,angle):
		self.d1 = dist
		self.d2 = dist
		self.a1 = angle
		self.a2 = angle
		self.inArc = True
		self.valid = False

	# Complete ppoint with what we have
	def stop():
		if self.inArc:
			self.dist  = (self.d1+self.d2)/2.
			self.angle = (self.a1+self.a2)/2.
			self.inArc = False
			self.valid = True
			self.width = self.getWidth()
			if self.width<2.*PILLAR_WIDTH or self.width>2*PILLAR_WIDTH:
				rospy.loginfo("Park: Issue: pillar width: {:.2f}".format(width))
				self.valid = False

	# Point represents the continuation or completion of a group
	def append(dist,angle):
		if dist>self.d2+TOLERANCE:
			self.stop()
		else:
			if dist<self.d1:
				self.d1 = dist
			elif dist>d2:
				self.d2 = dist
			self.a2 = angle

	# Compute width of pillar using law of cosines
	# If width is not reasonable, pillar will be discarded.
	def getWidth(self):
		a = self.d1
		b = self.d2
		theta = self.a2 - self.a1
		width = math.sqrt(a*a+b*b-2.*a*b*math.cos(theta))
		return width

# Position consists of a name, distance and angle from the current
# heading and position of the LIDAR. Distances in meters, angle in radians
class Position:
	def __init__(self, name):
		self.valid = False
		self.name = name
		self.dist = 0.0
		self.angle = 0.0

class Parker:
	def __init__(self):
		self.stopped = True

		# Create a Twist message, and fill in the fields.
		self.twist = Twist()
		self.reset = Empty()
		self.msg = TeleopStatus()

		self.step = 0
		# Publish status so that controller can keep track of state
		self.spub = rospy.Publisher('sb_teleop_status',TeleopStatus,queue_size=1)
		self.reset_pub = rospy.Publisher('/reset',Empty,queue_size=1)
		self.report("Parker: initialized.")
		self.behavior = ""
		self.leftTower = Pillar()
		self.rightTower= Pillar()
		self.towerSeparation = -1
		self.targetX = -1.
		self.targetY = -1.

	def report(self,text):
		if self.msg.status!=text:
			self.msg.status = text
			self.spub.publish(self.msg)

	# Start by finding the parking spot
	# We start here every time.
	def start(self):
		self.stopped = False
		self.report("Parker: started ...");
		# Publish movement commands to the turtlebot's base
		self.pub = rospy.Publisher('/cmd_vel', Twist,queue_size=1)
		self.step = 0
		self.sub  = rospy.Subscriber("/scan_throttle",LaserScan,self.getScan)

	def stop(self):
		if not self.stopped:
			self.stopped = True
			self.report("Parker: stopped.")
			self.sub.unregister()

	# Receive a "throttled" scan message (once per second)
	def getScan(self,scan):
		global behaviorName
		if rospy.is_shutdown() or self.stopped:
			return
		if not behaviorName=="park":
			self.stop()
		else:
			# Proceed with application
			self.find_parking_markers(scan)
			if self.rightTower.valid and self.leftTower.valid:
				self.park()
			else:
				rospy.loginfo("Park: Failed to find towers")

	# =============================== Parking Sequence ========================
	# Note that negative is forward.
	# Raw angles are 0-2*PI. 0 is straight ahead.
	def park(self):
		if self.step == 0:
			self.report("Park0: searching for markers")
			rospy.loginfo(' towers:  {:.2f} {1:.0f}, {:.2f} {:.0f}'.format(\
				self.leftTower.dist, np.rad2deg(self.leftTower.angle),\
				self.rightTower.dist,np.rad2deg(self.rightTower.angle)))
			self.step = 1
			return
		elif self.step == 1:
			self.report("Park1: proceed to reference point")
			self.setTarget(0,START_OFFSET+ROBOT_WIDTH)
			if self.twist.linear.x<IGNORE:
				self.reset_pub.publish(self,reset)
				self.step = 2
				return
		elif self.step == 2:
			self.report("Park2: pivot")
			self.pivot(0.0)
			self.step = 3
		elif self.step == 3:
			self.report("Park3: downwind leg")
			self.setTarget(START_OFFSET,START_OFFSET+ROBOT_WIDTH)
			if self.twist.linear.x<IGNORE:
				self.step = 4
		elif self.step == 4:
			self.report("Park4: base leg")
			self.setTarget(START_OFFSET,1.5*ROBOT_WIDTH)
			if self.twist.linear.x<IGNORE:
				self.step = 5
		elif self.step == 5:
			self.report("Park5: final approach")
			self.setTarget(1.5*ROBOT_WIDTH,1.5*ROBOT_WIDTH)
			if self.twist.linear.x<IGNORE:
				self.step = 6
		elif self.step == 6:
			self.report("Park6: reverse diagonal")
			self.setTarget(self.towerSeparation/2.,0.)
			self.reverse()
			if self.twist.linear.x<IGNORE:
				self.step = 7
		elif self.step==7:
			self.report("Auto_parking complete.")
			self.reset_pub.publish(self,reset)
			self.pivot(0.0)
			self.stop()

		self.pub.publish(self.twist)

	# =============================== End of Steps ========================

	# Search the scan results for the two closest pillars.
	# Reject objects that are too narrow or wide.
	# Handle the wrap by postulating a third pillar. We may combine.
	def find_parking_markers(self,scan):
		delta  = scan.angle_increment
		angle  = scan.angle_min-delta
		pillar1 = Pillar()  # Closest
		pillar2 = Pillar()  # Next closest
		pillar3 = Pillar()  # In case of a wrap around origin.
		for d in scan.ranges:
			angle = angle + delta
			if d < IGNORE:
				continue
			# New minimum, process no matter what
			if d<pillar1.d1-TOLERANCE:
				pillar1.stop()
				pillar3.clone(pillar2)
				pillar2.clone(pillar1)
				pillar1.start(d,angle)
				rospy.loginfo("Park: Pillar1 start arc at "+str(angle))
			elif pillar1.inArc:
				pillar1.append(d,angle)
				rospy.loginfo("Park: Pillar1 in arc at "+str(angle))
			elif d<pillar2.d1-TOLERANCE:
				# Farther than pillar1, but closer than current pillar2
				pillar2.stop()
				pillar3.clone(pillar2)
				pillar2.start(d,angle)
				rospy.loginfo("Park: Pillar2 start arc at "+str(angle))
			elif pillar2.inArc:
				pillar2.append(d,angle)
				rospy.loginfo("Park: Pillar2 in arc at "+str(angle))
			elif d<pillar3.d1-TOLERANCE
				pillar3.stop()
				pillar3.start(d,angle)
				rospy.loginfo("Park: Pillar3 start arc at "+str(angle))
			elif pillar3.inArc:
				pillar3.append(d,angle)
				rospy.loginfo("Park: Pillar3 in arc at "+str(angle))

		# Check for wrap-around
		if pillar1.inArc:
			if pillar2.a1 == 0:
				pillar1.combine(pillar2)
				pillar2 = pillar3
			else pillar1.stop()
		elif pillar2.inArc:
			if pillar1.a1==0:
				pillar2.combine(pillar1)
				pillar2 = pillar3
			elif pillar3.a1==0
				pillar2.combine(pillar3)

		# Now assign the tower positions
		if pillar1.angle<pillar2.angle:
			self.leftTower  = pillar1
			self.rightTower = pillar2
		else:
			self.leftTower  = pillar2
			self.rightTower = pillar1

		rospy.loginfo("Park: towers  {:.2f} {:.0f}, {:.2f} {:.0f}".format(\
				self.leftTower.dist,180*self.leftTower.angle/math.pi,\
				self.rightTower.dist,180*self.rightTower.angle/math.pi))

		# If we've never computed distance between, do it and save it
		# Use law of cosines again
		if self.rightTower.valid and self.leftTower.valid \
		  and  self.towerSeparation<0:
			a = pillar1.dist
			b = pillar2.dist
			angle = pillar1.angle-pillar2.angle
			self.towerSeparation = math.fabs(math.sqrt(a*a+b*b-2.*a*b*math.cos(angle)))
			rospy.loginfo("Park: Tower separation {:.2f}".format(self.towerSeparation))
			time.sleep(0.1)
			rospy.loginfo(" a,b,angle: {:.2f} {:.2f} {:.2f}".format(a,b,angle))
			if self.towerSeparation<3*ROBOT_WIDTH:
				self.towerSeparation = 3*ROBOT_WIDTH

	# Angle is with respect to x-axis (leftTower>rightTower)
	def pivot(self,angle):
		self.twist.linear.x  = 0.0
		self.twist.angular.z = 0.0

	# Compute the current position as a projection on the x-axis
	# We have the LIDAR positions to the tower.
	def projectionX(self):
		# By law of sines, with c as angle at rightTower
		sinc = self.leftTower.dist*math.sin(self.leftTower.angle-self.rightTower.angle)/\
				    					self.towerSeparation
		rospy.loginfo("ProjectionX: {:.2f} {:.2f} {:.2f} {:.2f}".format(\
			sinc,self.leftTower.dist,math.sin(self.leftTower.angle-self.rightTower.angle),\
			self.towerSeparation))
		cosc = math.sqrt(1. - sinc*sinc)
		return cosc*self.leftTower.dist - self.towerSeparation

	def projectionY(self):
		# By law of sines, with c as angle at rightTower
		sinc = self.leftTower.dist*math.sin(self.leftTower.angle-self.rightTower.angle)/\
				    					self.towerSeparation
		return sinc*self.leftTower.dist

	# Turn toward target. Target 0->2PI.
	def rampedAngle(self):
		angle = self.targetDirection
		if angle>math.pi:
			angle = angle - 2*math.pi
		if angle<0.0:
			if angle<-MAX_ANGLE:
				angle = -MAX_ANGLE
		else:
			if angle>MAX_ANGLE:
				angle = MAX_ANGLE
		return -angle

	def reverse(self):
		self.twist.linear.x = -self.twist.linear.x

	# Specify the target destination in terms of a reference system
	# origin at leftTower with x-axis through rightTower.
	# we start the maneuvers. We travel to this point and pivot.
	# If the left tower is the origin and the right tower on the x-axis,
	# then the reference point is at (0,ROBOT_WIDTH+START_OFFSET)
	def setTarget(self,x,y):
		dx = self.projectionX()
		dy = self.projectionY()
		x = x + dx
		y = y + dy
		self.twist.angular.z = math.atan2(x,y)
		self.twist.linear.x  = math.sqrt(x*x+y*y)
		if self.twist.linear.x<MAX_SPEED:
			self.twist.linear.x = MAX_SPEED

# The overall behavior has changed. Start the folower if state is "follow".
def getBehavior(behavior):
	global parker
	global behaviorName
	behaviorName = behavior.state
	if behavior.state=="park":
		parker.start()
	else:
		parker.stop()

if __name__ == "__main__":
	# Initialize the node
	rospy.init_node('sb_park', log_level=rospy.INFO, anonymous=True)
	parker = Parker()
	behaviorName = ""
	rospy.Subscriber("/sb_behavior",Behavior,getBehavior)

	rospy.spin()

<commit_msg>Park debugging<commit_after>#!/usr/bin/env python
#################################################################################
# Copyright 2018 ROBOTIS CO., LTD.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
# Modified clc so that frame of reference is always straight ahead.
# I was unable to find a good target with a high reflective intensity,
# so the parking target is between two towers, e.g. stacked cans.
#################################################################################

import rospy
import sys
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
from std_msgs.msg import Empty
from teleop_service.msg import Behavior,TeleopStatus
import numpy as np
import math
import time

MAX_ANGLE   = 0.5
MAX_SPEED   = 0.2
ROBOT_WIDTH = 0.2     # Robot width ~ m
TOLERANCE   = 0.04
PILLAR_WIDTH = 0.09    # Approx tower width ~ m
START_OFFSET= 1.0     # Dist from left tower to start ~ m
IGNORE      = 0.02    # Ignore any distances less than this
INFINITY    = 10.

# A pillar describes the position of a target pillar in coordinates of the LIDAR
# measurements. It carries attributes used in the calculation.
class Pillar:
	def __init__(self):
		self.valid = False
		self.dist = INFINITY
		self.angle= 0.0
		self.width = 0.0
		self.d1 = INFINITY
		self.d2 = INFINITY
		self.a1 = 0.0
		self.a2 = 0.0
		self.inArc = False

	def clone(self,pillar):
		self.valid = pillar.valid
		self.dist = pillar.dist
		self.angle= pillar.angle
		self.width = pillar.width
		self.d1 = pillar.d1
		self.d2 = pillar.d2
		self.a1 = pillar.a1
		self.a2 = pillar.a2
		self.inArc = pillar.inArc

	# Point represents the start of a group
	def start(self,dist,angle):
		self.d1 = dist
		self.d2 = dist
		self.a1 = angle
		self.a2 = angle
		self.inArc = True
		self.valid = False

	# Complete ppoint with what we have
	def stop(self):
		if self.inArc:
			self.dist  = (self.d1+self.d2)/2.
			self.angle = (self.a1+self.a2)/2.
			self.inArc = False
			self.valid = True
			self.width = self.getWidth()
			if self.width<2.*PILLAR_WIDTH or self.width>2*PILLAR_WIDTH:
				rospy.loginfo("Park: Issue: pillar width: {:.2f}".format(width))
				self.valid = False

	# Point represents the continuation or completion of a group
	def append(self,dist,angle):
		if dist>self.d2+TOLERANCE:
			self.stop()
		else:
			if dist<self.d1:
				self.d1 = dist
			elif dist>d2:
				self.d2 = dist
			self.a2 = angle

	# Compute width of pillar using law of cosines
	# If width is not reasonable, pillar will be discarded.
	def getWidth(self):
		a = self.d1
		b = self.d2
		theta = self.a2 - self.a1
		width = math.sqrt(a*a+b*b-2.*a*b*math.cos(theta))
		return width

# Position consists of a name, distance and angle from the current
# heading and position of the LIDAR. Distances in meters, angle in radians
class Position:
	def __init__(self, name):
		self.valid = False
		self.name = name
		self.dist = 0.0
		self.angle = 0.0

class Parker:
	def __init__(self):
		self.stopped = True

		# Create a Twist message, and fill in the fields.
		self.twist = Twist()
		self.reset = Empty()
		self.msg = TeleopStatus()

		self.step = 0
		# Publish status so that controller can keep track of state
		self.spub = rospy.Publisher('sb_teleop_status',TeleopStatus,queue_size=1)
		self.reset_pub = rospy.Publisher('/reset',Empty,queue_size=1)
		self.report("Parker: initialized.")
		self.behavior = ""
		self.leftTower = Pillar()
		self.rightTower= Pillar()
		self.towerSeparation = -1
		self.targetX = -1.
		self.targetY = -1.

	def report(self,text):
		if self.msg.status!=text:
			self.msg.status = text
			self.spub.publish(self.msg)

	# Start by finding the parking spot
	# We start here every time.
	def start(self):
		self.stopped = False
		self.report("Parker: started ...");
		# Publish movement commands to the turtlebot's base
		self.pub = rospy.Publisher('/cmd_vel', Twist,queue_size=1)
		self.step = 0
		self.sub  = rospy.Subscriber("/scan_throttle",LaserScan,self.getScan)

	def stop(self):
		if not self.stopped:
			self.stopped = True
			self.report("Parker: stopped.")
			self.sub.unregister()

	# Receive a "throttled" scan message (once per second)
	def getScan(self,scan):
		global behaviorName
		if rospy.is_shutdown() or self.stopped:
			return
		if not behaviorName=="park":
			self.stop()
		else:
			# Proceed with application
			self.find_parking_markers(scan)
			if self.rightTower.valid and self.leftTower.valid:
				self.park()
			else:
				rospy.loginfo("Park: Failed to find towers")

	# =============================== Parking Sequence ========================
	# Note that negative is forward.
	# Raw angles are 0-2*PI. 0 is straight ahead.
	def park(self):
		if self.step == 0:
			self.report("Park0: searching for markers")
			rospy.loginfo(' towers:  {:.2f} {1:.0f}, {:.2f} {:.0f}'.format(\
				self.leftTower.dist, np.rad2deg(self.leftTower.angle),\
				self.rightTower.dist,np.rad2deg(self.rightTower.angle)))
			self.step = 1
			return
		elif self.step == 1:
			self.report("Park1: proceed to reference point")
			self.setTarget(0,START_OFFSET+ROBOT_WIDTH)
			if self.twist.linear.x<IGNORE:
				self.reset_pub.publish(self,reset)
				self.step = 2
				return
		elif self.step == 2:
			self.report("Park2: pivot")
			self.pivot(0.0)
			self.step = 3
		elif self.step == 3:
			self.report("Park3: downwind leg")
			self.setTarget(START_OFFSET,START_OFFSET+ROBOT_WIDTH)
			if self.twist.linear.x<IGNORE:
				self.step = 4
		elif self.step == 4:
			self.report("Park4: base leg")
			self.setTarget(START_OFFSET,1.5*ROBOT_WIDTH)
			if self.twist.linear.x<IGNORE:
				self.step = 5
		elif self.step == 5:
			self.report("Park5: final approach")
			self.setTarget(1.5*ROBOT_WIDTH,1.5*ROBOT_WIDTH)
			if self.twist.linear.x<IGNORE:
				self.step = 6
		elif self.step == 6:
			self.report("Park6: reverse diagonal")
			self.setTarget(self.towerSeparation/2.,0.)
			self.reverse()
			if self.twist.linear.x<IGNORE:
				self.step = 7
		elif self.step==7:
			self.report("Auto_parking complete.")
			self.reset_pub.publish(self,reset)
			self.pivot(0.0)
			self.stop()

		self.pub.publish(self.twist)

	# =============================== End of Steps ========================

	# Search the scan results for the two closest pillars.
	# Reject objects that are too narrow or wide.
	# Handle the wrap by postulating a third pillar. We may combine.
	def find_parking_markers(self,scan):
		delta  = scan.angle_increment
		angle  = scan.angle_min-delta
		pillar1 = Pillar()  # Closest
		pillar2 = Pillar()  # Next closest
		pillar3 = Pillar()  # In case of a wrap around origin.
		for d in scan.ranges:
			angle = angle + delta
			if d < IGNORE:
				continue
			# New minimum, process no matter what
			if d<pillar1.d1-TOLERANCE:
				pillar1.stop()
				pillar3.clone(pillar2)
				pillar2.clone(pillar1)
				pillar1.start(d,angle)
				rospy.loginfo("Park: Pillar1 start arc at "+str(angle))
			elif pillar1.inArc:
				pillar1.append(d,angle)
				rospy.loginfo("Park: Pillar1 in arc at "+str(angle))
			elif d<pillar2.d1-TOLERANCE:
				# Farther than pillar1, but closer than current pillar2
				pillar2.stop()
				pillar3.clone(pillar2)
				pillar2.start(d,angle)
				rospy.loginfo("Park: Pillar2 start arc at "+str(angle))
			elif pillar2.inArc:
				pillar2.append(d,angle)
				rospy.loginfo("Park: Pillar2 in arc at "+str(angle))
			elif d<pillar3.d1-TOLERANCE:
				pillar3.stop()
				pillar3.start(d,angle)
				rospy.loginfo("Park: Pillar3 start arc at "+str(angle))
			elif pillar3.inArc:
				pillar3.append(d,angle)
				rospy.loginfo("Park: Pillar3 in arc at "+str(angle))

		# Check for wrap-around
		if pillar1.inArc:
			if pillar2.a1 == 0:
				pillar1.combine(pillar2)
				pillar2 = pillar3
			else:
				 pillar1.stop()
		elif pillar2.inArc:
			if pillar1.a1==0:
				pillar2.combine(pillar1)
				pillar2 = pillar3
			elif pillar3.a1==0:
				pillar2.combine(pillar3)

		# Now assign the tower positions
		if pillar1.angle<pillar2.angle:
			self.leftTower  = pillar1
			self.rightTower = pillar2
		else:
			self.leftTower  = pillar2
			self.rightTower = pillar1

		rospy.loginfo("Park: towers  {:.2f} {:.0f}, {:.2f} {:.0f}".format(\
				self.leftTower.dist,180*self.leftTower.angle/math.pi,\
				self.rightTower.dist,180*self.rightTower.angle/math.pi))

		# If we've never computed distance between, do it and save it
		# Use law of cosines again
		if self.rightTower.valid and self.leftTower.valid \
		  and  self.towerSeparation<0:
			a = pillar1.dist
			b = pillar2.dist
			angle = pillar1.angle-pillar2.angle
			self.towerSeparation = math.fabs(math.sqrt(a*a+b*b-2.*a*b*math.cos(angle)))
			rospy.loginfo("Park: Tower separation {:.2f}".format(self.towerSeparation))
			time.sleep(0.1)
			rospy.loginfo(" a,b,angle: {:.2f} {:.2f} {:.2f}".format(a,b,angle))
			if self.towerSeparation<3*ROBOT_WIDTH:
				self.towerSeparation = 3*ROBOT_WIDTH

	# Angle is with respect to x-axis (leftTower>rightTower)
	def pivot(self,angle):
		self.twist.linear.x  = 0.0
		self.twist.angular.z = 0.0

	# Compute the current position as a projection on the x-axis
	# We have the LIDAR positions to the tower.
	def projectionX(self):
		# By law of sines, with c as angle at rightTower
		sinc = self.leftTower.dist*math.sin(self.leftTower.angle-self.rightTower.angle)/\
				    					self.towerSeparation
		rospy.loginfo("ProjectionX: {:.2f} {:.2f} {:.2f} {:.2f}".format(\
			sinc,self.leftTower.dist,math.sin(self.leftTower.angle-self.rightTower.angle),\
			self.towerSeparation))
		cosc = math.sqrt(1. - sinc*sinc)
		return cosc*self.leftTower.dist - self.towerSeparation

	def projectionY(self):
		# By law of sines, with c as angle at rightTower
		sinc = self.leftTower.dist*math.sin(self.leftTower.angle-self.rightTower.angle)/\
				    					self.towerSeparation
		return sinc*self.leftTower.dist

	# Turn toward target. Target 0->2PI.
	def rampedAngle(self):
		angle = self.targetDirection
		if angle>math.pi:
			angle = angle - 2*math.pi
		if angle<0.0:
			if angle<-MAX_ANGLE:
				angle = -MAX_ANGLE
		else:
			if angle>MAX_ANGLE:
				angle = MAX_ANGLE
		return -angle

	def reverse(self):
		self.twist.linear.x = -self.twist.linear.x

	# Specify the target destination in terms of a reference system
	# origin at leftTower with x-axis through rightTower.
	# we start the maneuvers. We travel to this point and pivot.
	# If the left tower is the origin and the right tower on the x-axis,
	# then the reference point is at (0,ROBOT_WIDTH+START_OFFSET)
	def setTarget(self,x,y):
		dx = self.projectionX()
		dy = self.projectionY()
		x = x + dx
		y = y + dy
		self.twist.angular.z = math.atan2(x,y)
		self.twist.linear.x  = math.sqrt(x*x+y*y)
		if self.twist.linear.x<MAX_SPEED:
			self.twist.linear.x = MAX_SPEED

# The overall behavior has changed. Start the folower if state is "follow".
def getBehavior(behavior):
	global parker
	global behaviorName
	behaviorName = behavior.state
	if behavior.state=="park":
		parker.start()
	else:
		parker.stop()

if __name__ == "__main__":
	# Initialize the node
	rospy.init_node('sb_park', log_level=rospy.INFO, anonymous=True)
	parker = Parker()
	behaviorName = ""
	rospy.Subscriber("/sb_behavior",Behavior,getBehavior)

	rospy.spin()

<|endoftext|>