naps-utilities 0.1.0

Lib to handle ease of use of pointclouds for recovery of pose data

#+title: Utility lib for naps
#+author: Virgile Daugé
#+EMAIL:[e-mail]
#+begin_src emacs-lisp :tangle no :results silent
(setq org-src-preserve-indentation t)
#+end_src


* setup

#+begin_src python :tangle setup.py
# -*- coding: utf-8 -*-

from setuptools import setup, find_packages

with open('readme.org', 'r') as fh:
    long_description = fh.read()

setup(
    name='naps_utilities',
    packages=find_packages(exclude=["examples/*"]),
    version='0.1.0',
    description='Lib to handle ease of use of pointclouds '
                ' for recovery of pose data',
    author=u'Virgile Daugé',
    author_email='[e-mail]',
    url='https://github.com/virgileTN/naps_utilities',
    keywords=['pointclouds', 'filtering'],
    install_requires=['numpy',
                      'pose_transform'],
    long_description=long_description,
    long_description_content_type='text/plain',
    classifiers=[
        'Development Status :: 4 - Beta',
        'Environment :: Console',
        'Intended Audience :: Developers',
        'License :: OSI Approved :: GNU General Public License (GPL)',
        'Operating System :: MacOS :: MacOS X',
        'Operating System :: POSIX',
        'Programming Language :: Python :: 3.6',
        ],
)
#+end_src

#+begin_src bash :results value verbatim :exports both
mkdir naps_utilities
touch naps_utilities/__init__.py
#+end_src

* Classe Pointcloud

** Dependences

#+begin_src python :tangle naps_utilities/pointcloud.py
# Nécessaires pour la lecture/écriture de fichiers
import os
import json
import pickle

#Les données sont stockées sous forme de numpy ndarray
import numpy as np

#Nécessaire pour la conversion vers/depuis ROS2
from builtin_interfaces.msg import Time
from sensor_msgs.msg import PointCloud2
from sensor_msgs.msg import PointField
from std_msgs.msg import Header
from array import array
#+end_src

** Corps de la classe
#+begin_src python :tangle naps_utilities/pointcloud.py
class Pointcloud():
    def __init__(self, ros_msg=None, keep_ring=True, matrix=None, procrastinate=False):
        # PoinCloud Metadata
        self.metadata = {'header': None,
                         'height': None,
                         'width': None,
                         'fields': None,
                         'is_bigendian': None,
                         'point_step': None,
                         'row_step': None,
                         'is_dense': None,
                         'keep_ring': None,
                         'is_populated': False,
                         'procrastinated': True,}
        # Pointcloud DATA
        self.points = None
        self.rings = None
        self.matrix = matrix

        if ros_msg is not None:
            self.from_msg(ros_msg)
            if not procrastinate:
                self.filter()
                self.transform(matrix)
#+end_src
** populate from ROS msg
Un certain nombre de données ne nécessitent pas de conversion :
#+begin_src python :tangle naps_utilities/pointcloud.py
    def from_msg(self, msg):
        #Données conservées "telles quelles"

        self.metadata['height'] = msg.height
        self.metadata['width'] = msg.width

        self.metadata['is_bigendian'] = msg.is_bigendian
        self.metadata['point_step'] = msg.point_step
        self.metadata['row_step'] = msg.row_step

        self.metadata['is_dense'] = msg.is_dense
#+end_src
L'atribut Header est du type std_msgs/Header:
#+begin_src python :tangle naps_utilities/pointcloud.py
        def from_header(header):
            return {'time': {'sec': header.stamp.sec, 'nanosec': header.stamp.nanosec},
               'frame_id': header.frame_id}
        self.metadata['header'] = from_header(msg.header)
#+end_src
L'attribut fields du msg ROS est une liste d'objets PointFields.  Il
convient également de supprimer le fields ring, si l'on choisit de ne
pas les garder.
#+begin_src python :tangle naps_utilities/pointcloud.py
        def from_pointfields(fields):
            return [{'name': field.name,
                'offset': field.offset,
                'datatype': field.datatype,
                'count': field.count}
               for field in fields]

        self.metadata['fields'] = from_pointfields(msg.fields)
#+end_src
Afin de préparer l'extraction, on initialise des numpy ndarray afin
que tous les points soient dans un espace contigu de la mémoire. Ici
on sépare les points en un tableau de float32 (x, y, z) et un tableau
de (ring). Cela pour faciliter l'encodage décodage (c'est plus
difficile avec des types différents imbriqués)

#+begin_src python :tangle naps_utilities/pointcloud.py
        # Données converties
        self.metadata['nb_points'] = msg.height * msg.width

        data = np.reshape(msg.data, (-1, self.metadata['point_step']))

        self.points = np.ndarray(
            (self.metadata['nb_points'], 4), dtype=np.float32,
            buffer=np.ascontiguousarray(data[:, :16]))

        if self.metadata['keep_ring']:
            self.metadata['rings'] = np.zeros(
                self.metadata['nb_points'], dtype=np.uint16)

            pointcloud['rings'] = np.ndarray(
                (self.metadata['nb_points']), dtype=np.uint16,
                buffer=np.ascontiguousarray(data[:, 16:]))
#+end_src
Mise à jour dé métadonnées si nécessaire :
#+begin_src python :tangle naps_utilities/pointcloud.py
        if not self.metadata['keep_ring']:
            self.metadata['fields'] = [field for field in self.metadata['fields'] if field['name'] != 'ring']
            self.metadata['point_step'] = 16
            self.metadata['row_step'] = self.metadata['point_step'] * len(self.metadata['fields'])
            self.metadata['is_populated'] = True
#+end_src
** convert to msg
Beaucoup de symétrie avec la fonction précedante.
#+begin_src python :tangle naps_utilities/pointcloud.py
    def to_msg(self):
        msg = PointCloud2()
        #Données conservées "telles quelles"

        msg.height = self.metadata['height']
        msg.width = self.metadata['width']

        msg.is_bigendian = self.metadata['is_bigendian']
        msg.point_step = self.metadata['point_step']
        msg.row_step = self.metadata['row_step']

        msg.is_dense = self.metadata['is_dense']
#+end_src

Conversion vers Header ROS:
#+begin_src python :tangle naps_utilities/pointcloud.py
        def to_header(header_data):
            return Header(stamp=Time(
                sec=header_data['time']['sec'],
                nanosec=header_data['time']['nanosec']),
                     frame_id=header_data['frame_id'])
        msg.header = to_header(self.metadata['header'])
#+end_src
Conversion vers Pointfield:
#+begin_src python :tangle naps_utilities/pointcloud.py
        def to_pointfields(pointfields_data):
            return [PointField(name=field['name'],
                          offset=field['offset'],
                          datatype=field['datatype'],
                          count=field['count']) for field in pointfields_data]
        msg.fields = to_pointfields(self.metadata['fields'])
#+end_src
Deux cas, selon la valeur de 'keep_ring':

Si on garde les rings, il faut concatener les deux tableaux et en
faire un array de uint8.
#+begin_src python :tangle naps_utilities/pointcloud.py
        if self.metadata['keep_ring']:
            msg.data = array('B', np.concatenate(
            (self.points.view(dtype=np.uint8),
             self.rings.reshape((self.metadata['nb_points'], -1)).view(dtype=np.uint8)),
            axis=1).ravel().tolist())
#+end_src
Sinon, il suffi de créer une liste de uint8 à partir des points au
niveau des données.
#+begin_src python :tangle naps_utilities/pointcloud.py
        else:
            msg.data = array('B', self.points.view(dtype=np.uint8).ravel().tolist())
        return msg
#+end_src
** filter pointcloud

Il y a deux cas a traiter, si l'on garde les rings auquel cas il faut
les filter aussi.
#+begin_src python :tangle naps_utilities/pointcloud.py
    def filter(self, threshold=10):
        if self.metadata['keep_ring']:
            concat = np.concatenate((self.points, self.rings.reshape((len(points), 1))), axis=1)
            concat = concat[np.logical_and(
                np.logical_not(np.isnan(concat).any(axis=1)),
                concat[:,3]>=threshold)]
            self.points = np.ascontiguousarray(concat[:,:4], dtype=np.float32)
            self.rings = np.ascontiguousarray(concat[:,4:], dtype=np.uint16)
#+end_src
Après avoir été filtré, le poincloud ne peut plus être structuré dans
un tableau 2D.
#+begin_src python :tangle naps_utilities/pointcloud.py
        else:
            self.points = self.points[np.logical_and(
                np.logical_not(np.isnan(self.points).any(axis=1)),
                self.points[:,3]>=threshold)]
        self.metadata['nb_points'] = len(self.points)
        self.metadata['height'] = 1
        self.metadata['width'] = self.metadata['nb_points']
#+end_src

** transform pointcloud
#+begin_src python :tangle naps_utilities/pointcloud.py
    def transform(self, matrix):
        self.points[:,:3] = np.transpose(
            matrix @ np.concatenate((self.points[:,:3].transpose(),
                                     np.ones((1, self.metadata['nb_points'])))))[:,:3]
        self.matrix = matrix
        self.metadata['procrastinated'] = False
#+end_src
** add points
#+begin_src python :tangle naps_utilities/pointcloud.py
    def update(self, pointcloud):
        if self.metadata['keep_ring']:
            if pointcloud.metadata['keep_ring']:
                self.rings = np.ascontiguousarray(np.concatenate((self.rings, pointcloud.rings)))
            else:
                return False
        self.points = np.ascontiguousarray(np.concatenate((self.points, pointcloud.points)))
        self.metadata['nb_points'] = len(self.points)
        self.metadata['height'] = 1
        self.metadata['width'] = self.metadata['nb_points']
        return True
#+end_src

** Export
#+begin_src python :tangle naps_utilities/pointcloud.py
    def save(self, path):
        save_path = os.path.expanduser(path)
        with open('{}_meta.json'.format(save_path), 'w') as outfile:
            json.dump(self.metadata, outfile, indent=4)

        np.savez_compressed('{}_data'.format(save_path), points=self.points, rings=self.rings, matrix=self.matrix)
#+end_src

* Build et disctribution
#+begin_src bash :results value verbatim :exports both
python setup.py bdist_wheel sdist
#+end_src

#+RESULTS:
#+begin_example
running bdist_wheel
running build
running build_py
creating build
creating build/lib
creating build/lib/naps_utilities
copying naps_utilities/pointcloud.py -> build/lib/naps_utilities
copying naps_utilities/__init__.py -> build/lib/naps_utilities
installing to build/bdist.linux-x86_64/wheel
running install
running install_lib
creating build/bdist.linux-x86_64
creating build/bdist.linux-x86_64/wheel
creating build/bdist.linux-x86_64/wheel/naps_utilities
copying build/lib/naps_utilities/pointcloud.py -> build/bdist.linux-x86_64/wheel/naps_utilities
copying build/lib/naps_utilities/__init__.py -> build/bdist.linux-x86_64/wheel/naps_utilities
running install_egg_info
running egg_info
creating naps_utilities.egg-info
writing naps_utilities.egg-info/PKG-INFO
writing dependency_links to naps_utilities.egg-info/dependency_links.txt
writing requirements to naps_utilities.egg-info/requires.txt
writing top-level names to naps_utilities.egg-info/top_level.txt
writing manifest file 'naps_utilities.egg-info/SOURCES.txt'
reading manifest file 'naps_utilities.egg-info/SOURCES.txt'
writing manifest file 'naps_utilities.egg-info/SOURCES.txt'
Copying naps_utilities.egg-info to build/bdist.linux-x86_64/wheel/naps_utilities-0.1.0-py3.7.egg-info
running install_scripts
creating build/bdist.linux-x86_64/wheel/naps_utilities-0.1.0.dist-info/WHEEL
creating 'dist/naps_utilities-0.1.0-py3-none-any.whl' and adding 'build/bdist.linux-x86_64/wheel' to it
adding 'naps_utilities/__init__.py'
adding 'naps_utilities/pointcloud.py'
adding 'naps_utilities-0.1.0.dist-info/METADATA'
adding 'naps_utilities-0.1.0.dist-info/WHEEL'
adding 'naps_utilities-0.1.0.dist-info/top_level.txt'
adding 'naps_utilities-0.1.0.dist-info/RECORD'
removing build/bdist.linux-x86_64/wheel
running sdist
running check
creating naps_utilities-0.1.0
creating naps_utilities-0.1.0/naps_utilities
creating naps_utilities-0.1.0/naps_utilities.egg-info
copying files to naps_utilities-0.1.0...
copying setup.py -> naps_utilities-0.1.0
copying naps_utilities/__init__.py -> naps_utilities-0.1.0/naps_utilities
copying naps_utilities/pointcloud.py -> naps_utilities-0.1.0/naps_utilities
copying naps_utilities.egg-info/PKG-INFO -> naps_utilities-0.1.0/naps_utilities.egg-info
copying naps_utilities.egg-info/SOURCES.txt -> naps_utilities-0.1.0/naps_utilities.egg-info
copying naps_utilities.egg-info/dependency_links.txt -> naps_utilities-0.1.0/naps_utilities.egg-info
copying naps_utilities.egg-info/requires.txt -> naps_utilities-0.1.0/naps_utilities.egg-info
copying naps_utilities.egg-info/top_level.txt -> naps_utilities-0.1.0/naps_utilities.egg-info
Writing naps_utilities-0.1.0/setup.cfg
Creating tar archive
removing 'naps_utilities-0.1.0' (and everything under it)
#+end_example


#+begin_src bash :results value verbatim :exports both
twine upload dist/*
#+end_src

#+RESULTS: