feat(slam): add rtabmap_ros

rtabmap_ros/rtabmap_python/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rtabmap_python</name>
+  <version>0.22.0</version>
+  <description>RTAB-Map's python package.</description>
+  <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
+  <author>Mathieu Labbe</author>
+  <license>BSD</license>
+  <url type="bugtracker">https://github.com/introlab/rtabmap_ros/issues</url>
+  <url type="repository">https://github.com/introlab/rtabmap_ros</url>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

rtabmap_ros/rtabmap_python/rtabmap_python/.gitignore

@@ -0,0 +1 @@
+*.pyc

rtabmap_ros/rtabmap_python/rtabmap_python/__init__.py

@@ -0,0 +1,2 @@
+
+

rtabmap_ros/rtabmap_python/rtabmap_python/compression.py

@@ -0,0 +1,34 @@
+
+import zlib
+import struct
+import numpy as np
+
+def compress(data):
+    assert data.ndim == 1 or data.ndim == 2
+
+    dim1 = 1
+    if data.ndim == 1:
+        dim1 = 1
+        dim2 = len(data)
+    else:
+        dim1 = data.shape[0]
+        dim2 = data.shape[1]
+
+    numpy_type_to_cvtype = {'uint8': 0, 'int8': 1, 'uint16': 2,
+                            'int16': 3, 'int32': 4, 'float32': 5,
+                            'float64': 6}
+
+    compressed_data = bytearray(zlib.compress(data.tobytes()))
+    compressed_data.extend(struct.pack("iii", dim1, dim2, numpy_type_to_cvtype[data.dtype.name]))
+
+    return compressed_data
+
+def uncompress(bytes):
+    cvtype_to_numpy_type = {0: 'uint8', 1: 'int8', 2: 'uint16',
+                            3: 'int16', 4: 'int32', 5: 'float32',
+                            6: 'float64'}
+    out = zlib.decompress(bytes[:len(bytes)-3*4])
+    rows, cols, datatype = struct.unpack_from("iii", bytes, offset=len(bytes)-3*4)
+    data = np.frombuffer(out, dtype=cvtype_to_numpy_type[datatype])
+    return data.reshape((rows, cols))
+

rtabmap_ros/rtabmap_python/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/rtabmap_python
+[install]
+install_scripts=$base/lib/rtabmap_python

rtabmap_ros/rtabmap_python/setup.py

@@ -0,0 +1,25 @@
+from setuptools import setup
+
+package_name = 'rtabmap_python'
+
+setup(
+    name=package_name,
+    version='0.1.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='Mathieu Labbe',
+    maintainer_email='matlabbe@gmail.com',
+    description='RTAB-Map\'s python package.',
+    license='BSD',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+        ],
+    },
+)

rtabmap_ros/rtabmap_python/test/test_copyright.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+from ament_copyright.main import main
+import pytest
+
+
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

rtabmap_ros/rtabmap_python/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

rtabmap_ros/rtabmap_python/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'