kompass-core 0.2.2__tar.gz

kompass_core-0.2.2/LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2024 Automatika Robotics
+
+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.

kompass_core-0.2.2/MANIFEST.in

@@ -0,0 +1 @@
+recursive-include src *.h

kompass_core-0.2.2/PKG-INFO

@@ -0,0 +1,156 @@
+Metadata-Version: 2.1
+Name: kompass_core
+Version: 0.2.2
+Summary: Navigation Algorithms for Kompass
+Author-email: Automatika Robotics <contact@automatikarobotics.com>
+License: MIT License
+        
+        Copyright (c) 2024 Automatika Robotics
+        
+        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.
+        
+Keywords: robotics,robots,navigation,control,planning,mapping
+Classifier: License :: OSI Approved :: MIT License
+Classifier: Programming Language :: Python
+Classifier: Programming Language :: Python :: 3
+Requires-Python: <3.13,>=3.8.4
+Description-Content-Type: text/markdown
+License-File: LICENSE
+Requires-Dist: numpy<=1.26.4
+Requires-Dist: numpy-quaternion>=2023.0.3
+Requires-Dist: python-fcl-fork
+Requires-Dist: pybind11
+Requires-Dist: omegaconf
+Requires-Dist: attrs>=23.2.0
+Requires-Dist: numba
+Requires-Dist: pyyaml
+Requires-Dist: scipy
+Requires-Dist: matplotlib
+Requires-Dist: pandas
+Provides-Extra: dev
+Requires-Dist: pip-tools; extra == "dev"
+Requires-Dist: pytest==8.0.0; extra == "dev"
+Requires-Dist: pre-commit; extra == "dev"
+
+# Kompass Core
+
+Kompass Core is a motion planning and control package for robot navigation. The package contains C++ implementation for core algorithms along with Python wrappers. It also implements third party integrations with [OMPL](https://ompl.kavrakilab.org/) and [FCL](https://github.com/flexible-collision-library/fcl). This package is developed to be used with [Kompass](https://github.com/automatika-robotics/kompass) package for creating navigation stacks in [ROS2](https://docs.ros.org/en/rolling/index.html). For detailed usage documentation, check Kompass [docs](https://automatika-robotics.github.io/kompass/).
+
+## Installation
+
+### Install Dependencies
+
+- `sudo apt-get install libompl-dev libfcl-dev libpcl-dev`
+
+### Install with pip
+
+Wheels are available on Pypi for linux x86_64 and aarch64 architectures.
+
+- `pip install kompass-core`
+
+### Install from Source
+
+- `git clone https://github.com/automatika-robotics/kompass-core`
+- `cd kompass-core`
+- `pip install .`
+
+Installation with pip will install three Python package:
+
+- `kompass_core`: The main Python API containing all the wrappers and utilities for motion planning and control for navigation in 2D spaces.
+- `kompass_cpp`: Pybind11 python bindings for Kompass core C++ library containing the algorithms implementation for path tracking and motion control.
+- `ompl`: Bespoke Pybind11 python bindings for the Open Motion Planning Library (OMPL).
+
+## Testing
+
+### Run Planning Test
+
+- `cd tests`
+- `python3 test_ompl.py`
+
+To test path planning using OMPL bindings a reference planning problem is provided using Turtlebot3 Waffle map and fixed start and end position. The test will simulate the planning problem for all geometric planners for the number of desired repetitions to get average values.
+
+### Run Controllers Test
+
+- `cd tests`
+- `python3 test_controllers.py`
+
+The test will simulate path tracking using a reference global path. The results plot for each available controller will be generated in tests/resources/control
+
+## Usage Example
+
+```python
+from kompass_core.control import DVZ
+
+from kompass_core.models import (
+    AngularCtrlLimits,
+    LinearCtrlLimits,
+    Robot,
+    RobotCtrlLimits,
+    RobotGeometry,
+    RobotType,
+)
+from nav_msgs.msg import Path
+
+# Setup the robot
+my_robot = Robot(
+        robot_type=RobotType.ACKERMANN,
+        geometry_type=RobotGeometry.Type.CYLINDER,
+        geometry_params=np.array([0.1, 0.4]),
+    )
+
+# Set the robot control limits
+robot_ctr_limits = RobotCtrlLimits(
+    vx_limits=LinearCtrlLimits(max_vel=1.0, max_acc=5.0, max_decel=10.0),
+    omega_limits=AngularCtrlLimits(
+        max_vel=2.0, max_acc=3.0, max_decel=3.0, max_steer=np.pi
+    ),
+)
+
+# Set the control time step (s)
+control_time_step = 0.1     # seconds
+
+# Initialize the controller
+dvz = DVZ(
+        robot=my_robot,
+        ctrl_limits=robot_ctr_limits,
+        control_time_step=control_time_step,
+    )
+
+# Set the reference path
+global_path : Path = Path()
+
+# Set the reference path for the motion control
+dvz.set_path(global_path)
+
+# Get the sensor data
+laser_scan = LaserScanData()
+
+# At each control step run
+dvz.loop_step(current_state=robot.state, laser_scan=laser_scan)
+```
+
+## Copyright
+
+The code in this distribution is Copyright (c) 2024 Automatika Robotics unless explicitly indicated otherwise.
+
+Kompass Core is made available under the MIT license. Details can be found in the [LICENSE](LICENSE) file.
+
+## Contributions
+
+Kompass Core has been developed in collaboration between [Automatika Robotics](https://automatikarobotics.com/) and [Inria](https://inria.fr/). Contributions from the community are most welcome.

kompass_core-0.2.2/README.md

@@ -0,0 +1,106 @@
+# Kompass Core
+
+Kompass Core is a motion planning and control package for robot navigation. The package contains C++ implementation for core algorithms along with Python wrappers. It also implements third party integrations with [OMPL](https://ompl.kavrakilab.org/) and [FCL](https://github.com/flexible-collision-library/fcl). This package is developed to be used with [Kompass](https://github.com/automatika-robotics/kompass) package for creating navigation stacks in [ROS2](https://docs.ros.org/en/rolling/index.html). For detailed usage documentation, check Kompass [docs](https://automatika-robotics.github.io/kompass/).
+
+## Installation
+
+### Install Dependencies
+
+- `sudo apt-get install libompl-dev libfcl-dev libpcl-dev`
+
+### Install with pip
+
+Wheels are available on Pypi for linux x86_64 and aarch64 architectures.
+
+- `pip install kompass-core`
+
+### Install from Source
+
+- `git clone https://github.com/automatika-robotics/kompass-core`
+- `cd kompass-core`
+- `pip install .`
+
+Installation with pip will install three Python package:
+
+- `kompass_core`: The main Python API containing all the wrappers and utilities for motion planning and control for navigation in 2D spaces.
+- `kompass_cpp`: Pybind11 python bindings for Kompass core C++ library containing the algorithms implementation for path tracking and motion control.
+- `ompl`: Bespoke Pybind11 python bindings for the Open Motion Planning Library (OMPL).
+
+## Testing
+
+### Run Planning Test
+
+- `cd tests`
+- `python3 test_ompl.py`
+
+To test path planning using OMPL bindings a reference planning problem is provided using Turtlebot3 Waffle map and fixed start and end position. The test will simulate the planning problem for all geometric planners for the number of desired repetitions to get average values.
+
+### Run Controllers Test
+
+- `cd tests`
+- `python3 test_controllers.py`
+
+The test will simulate path tracking using a reference global path. The results plot for each available controller will be generated in tests/resources/control
+
+## Usage Example
+
+```python
+from kompass_core.control import DVZ
+
+from kompass_core.models import (
+    AngularCtrlLimits,
+    LinearCtrlLimits,
+    Robot,
+    RobotCtrlLimits,
+    RobotGeometry,
+    RobotType,
+)
+from nav_msgs.msg import Path
+
+# Setup the robot
+my_robot = Robot(
+        robot_type=RobotType.ACKERMANN,
+        geometry_type=RobotGeometry.Type.CYLINDER,
+        geometry_params=np.array([0.1, 0.4]),
+    )
+
+# Set the robot control limits
+robot_ctr_limits = RobotCtrlLimits(
+    vx_limits=LinearCtrlLimits(max_vel=1.0, max_acc=5.0, max_decel=10.0),
+    omega_limits=AngularCtrlLimits(
+        max_vel=2.0, max_acc=3.0, max_decel=3.0, max_steer=np.pi
+    ),
+)
+
+# Set the control time step (s)
+control_time_step = 0.1     # seconds
+
+# Initialize the controller
+dvz = DVZ(
+        robot=my_robot,
+        ctrl_limits=robot_ctr_limits,
+        control_time_step=control_time_step,
+    )
+
+# Set the reference path
+global_path : Path = Path()
+
+# Set the reference path for the motion control
+dvz.set_path(global_path)
+
+# Get the sensor data
+laser_scan = LaserScanData()
+
+# At each control step run
+dvz.loop_step(current_state=robot.state, laser_scan=laser_scan)
+```
+
+## Copyright
+
+The code in this distribution is Copyright (c) 2024 Automatika Robotics unless explicitly indicated otherwise.
+
+Kompass Core is made available under the MIT license. Details can be found in the [LICENSE](LICENSE) file.
+
+## Contributions
+
+Kompass Core has been developed in collaboration between [Automatika Robotics](https://automatikarobotics.com/) and [Inria](https://inria.fr/). Contributions from the community are most welcome.

kompass_core-0.2.2/pyproject.toml

@@ -0,0 +1,113 @@
+[build-system]
+requires = ["setuptools>=61.0.0", "wheel", "pybind11>=2.10.0"]
+build-backend = "setuptools.build_meta"
+
+[project]
+name = "kompass_core"
+version = "0.2.2"
+description = "Navigation Algorithms for Kompass"
+authors = [{ name = "Automatika Robotics", email = "contact@automatikarobotics.com" }]
+readme = "README.md"
+classifiers = [
+    "License :: OSI Approved :: MIT License",
+    "Programming Language :: Python",
+    "Programming Language :: Python :: 3",
+]
+license = { file = "LICENSE" }
+
+keywords = ["robotics", "robots", "navigation", "control", "planning", "mapping"]
+dependencies = [
+    "numpy<=1.26.4",
+    "numpy-quaternion>=2023.0.3",
+    "python-fcl-fork",
+    "pybind11",
+    "omegaconf",
+    "attrs>=23.2.0",
+    "numba",
+    "pyyaml",
+    "scipy",
+    "matplotlib",
+    "pandas",
+]
+
+requires-python = ">=3.8.4,<3.13"
+
+[project.optional-dependencies]
+dev = ["pip-tools", "pytest==8.0.0", "pre-commit"]
+
+[tool.setuptools.packages.find]
+where = ["src"]
+
+[tool.pytest.ini_options]
+minversion = "6.0"
+addopts = "-ra -q"
+log_cli = true
+log_cli_level = "INFO"
+log_cli_format = "%(asctime)s [%(levelname)8s] %(message)s"
+log_cli_date_format = "%Y-%m-%d %H:%M:%S"
+testpaths = [
+    "tests"
+]
+python_files = 'test*.py'
+
+[tool.interrogate]
+ignore-init-method = true
+ignore-init-module = false
+ignore-magic = false
+ignore-semiprivate = false
+ignore-private = false
+ignore-property-decorators = false
+ignore-module = true
+ignore-nested-functions = false
+ignore-nested-classes = true
+ignore-setters = false
+exclude = ["setup.py", "docs", "build", "tests"]
+ignore-regex = ["^get$", "^mock_.*", ".*BaseClass.*"]
+quiet = false
+whitelist-regex = []
+color = true
+generate-badge = "."
+badge-format = "svg"
+
+[tool.ruff]
+extend-exclude = [".mypy_cache", ".tox", ".venv", "buck-out", "build", ".pytest_cache"]
+fix = true
+line-length = 88
+preview = true
+[tool.ruff.lint]
+ignore = ["E203", "E266", "E501", "F403", "F401"]
+select = ["B","C","E","F","W","B9"]
+[tool.ruff.lint.mccabe]
+max-complexity = 10
+
+[tool.bumpver]
+current_version = "0.2.2"
+version_pattern = "MAJOR.MINOR.PATCH"
+commit_message = "(chore) Bump version {old_version} -> {new_version}"
+tag_message = "{new_version}"
+tag_scope = "default"
+pre_commit_hook = ""
+post_commit_hook = ""
+commit = true
+tag = true
+push = true
+
+[tool.bumpver.file_patterns]
+"pyproject.toml" = [
+    'version = "{version}"',
+]
+"setup.py" = [
+    "{version}",
+]
+
+[tool.cibuildwheel]
+skip = ["pp*", "*musllinux*"]
+test-requires = "pytest"
+test-command = "pytest {package}/tests/test_fcl.py"
+manylinux-x86_64-image = "manylinux_2_28"
+manylinux-aarch64-image = "manylinux_2_28"
+
+[tool.cibuildwheel.linux]
+environment = {VCPKG_ROOT="/project/deps/vcpkg", LD_LIBRARY_PATH="$LD_LIBRARY_PATH:$VCPKG_ROOT/installed/x64-linux/lib:$VCPKG_ROOT/installed/arm64-linux/lib"}
+before-all = "bash build_dependencies/install_linux.sh"
+archs = ["x86_64"]

kompass_core-0.2.2/setup.cfg

@@ -0,0 +1,4 @@
+[egg_info]
+tag_build = 
+tag_date = 0
+

kompass_core-0.2.2/setup.py

@@ -0,0 +1,200 @@
+import glob
+import os
+import sys
+
+from pybind11.setup_helpers import (
+    Pybind11Extension,
+    build_ext,
+    ParallelCompile,
+    naive_recompile,
+)
+from setuptools import find_packages, setup
+import subprocess
+
+__version__ = "0.2.2"
+
+# DEFAULTS FOR UBUNTU 22.04
+OMPL_INCLUDE_DEFAULT_DIR = "/usr/include/ompl-1.5"
+EIGEN_INCLUDE_DEFAULT_DIR = "/usr/include/eigen3"
+PCL_INCLUDE_DEFAULT_DIR = "/usr/include/pcl-1.12"
+
+
+def get_libraries_dir():
+    """Get libraries dir"""
+    lib_dirs = ["/usr/lib", "/usr/local/lib", "/usr/lib/x86_64-linux-gnu"]
+
+    if "LD_LIBRARY_PATH" in os.environ:
+        lib_dirs += os.environ["LD_LIBRARY_PATH"].split(":")
+    return lib_dirs
+
+
+def check_pkg_config():
+    """Check if pkg-config is installed"""
+    try:
+        subprocess.check_output(["pkg-config", "--version"])
+    except subprocess.CalledProcessError as e:
+        raise RuntimeError(
+            "pkg-config is not installed, but it is required for building this package."
+        ) from e
+
+
+def find_pkg(package, versions=None):
+    """Find a given package using pkg-config"""
+    if not versions:
+        try:
+            subprocess.check_output(["pkg-config", "--exists", package])
+            print(f"FOUND {package}")
+            return package
+        except subprocess.CalledProcessError:
+            return
+    else:
+        found_version = None
+        for version in versions:
+            try:
+                subprocess.check_output([
+                    "pkg-config",
+                    "--exists",
+                    f"{package}-{version}",
+                ])
+            except subprocess.CalledProcessError:
+                continue
+            print(f"FOUND {package} version {version}")
+            found_version = version
+            break
+        if found_version:
+            return f"{package}-{found_version}"
+
+
+def pkg_config(packages, flag, versions=None):
+    """Run pkg-config with given flag"""
+    found_packages = []
+    # Check if the package exists
+    for pkg in packages:
+        if found_pkg := find_pkg(pkg, versions):
+            found_packages.append(found_pkg)
+    try:
+        output = (
+            subprocess.check_output(["pkg-config", flag] + found_packages)
+            .decode("utf-8")
+            .strip()
+        )
+        output_dirs = []
+        for output_dir in output.split():
+            output_dirs.append(output_dir.split("-I")[-1])
+        return output_dirs
+    except subprocess.CalledProcessError:
+        return []
+
+
+# Check that pkg-config is installed
+check_pkg_config()
+
+# get vcpkg paths when building wheels
+vcpkg_path = os.environ.get("VCPKG_ROOT")
+
+
+def get_vcpkg_includes():
+    """Get include dir for vcpkg"""
+    if vcpkg_path:
+        python_version = f"{sys.version_info.major}.{sys.version_info.minor}"
+        headers_dir = next(
+            (s for s in os.listdir("/opt/_internal") if python_version in s), None
+        )
+        if not headers_dir:
+            return [
+                f"{vcpkg_path}/installed/x64-linux/include",
+                f"{vcpkg_path}/installed/arm64-linux/include",
+            ]
+        return [
+            f"{vcpkg_path}/installed/x64-linux/include",
+            f"{vcpkg_path}/installed/arm64-linux/include",
+            f"/opt/_internal/{headers_dir}/include/python{python_version}",
+        ]
+    else:
+        return []
+
+
+def get_vcpkg_extra_libs():
+    """Get extra libs explicitly for linking because FCL port for vcpkg
+    builds as a static lib that does not link its dependencies
+    """
+    if vcpkg_path:
+        return ["octomap", "octomath", "ccd"]
+    else:
+        return []
+
+
+# OMPL dependencies
+ompl_include_dirs = pkg_config(["ompl", "eigen3"], flag="--cflags-only-I")
+
+# Accepted PCL versions
+pcl_versions = ["1.14", "1.13", "1.12", "1.11", "1.10", "1.9"]
+
+# Kompass CPP dependencies
+eigen_include_dir = pkg_config(["eigen3"], flag="--cflags-only-I")
+pcl_include_dir = pkg_config(
+    ["pcl_common"], flag="--cflags-only-I", versions=pcl_versions
+)
+
+# vcpkg paths when running in CI
+vcpkg_includes_dir = get_vcpkg_includes()
+vcpkg_extra_libs = get_vcpkg_extra_libs()
+
+
+## Check include dirs
+
+# for OMPL
+ompl_module_includes = ompl_include_dirs + vcpkg_includes_dir
+# try a static path when pkg_config does not return anything and not in CI
+if not ompl_module_includes:
+    ompl_module_includes = [OMPL_INCLUDE_DEFAULT_DIR, EIGEN_INCLUDE_DEFAULT_DIR]
+
+# for Kompass CPP
+kompass_cpp_dependency_includes = (
+    eigen_include_dir + pcl_include_dir + vcpkg_includes_dir
+)
+kompass_cpp_module_includes = [
+    "src/kompass_cpp/kompass_cpp/include"
+] + kompass_cpp_dependency_includes
+# try a static path when pkg_config does not return anything and not in CI
+kompass_cpp_module_includes = (
+    [
+        "src/kompass_cpp/kompass_cpp/include",
+        EIGEN_INCLUDE_DEFAULT_DIR,
+        PCL_INCLUDE_DEFAULT_DIR,
+    ]
+    if not kompass_cpp_dependency_includes
+    else ["src/kompass_cpp/kompass_cpp/include"] + kompass_cpp_dependency_includes
+)
+
+
+ext_modules = [
+    Pybind11Extension(
+        "ompl",
+        glob.glob(os.path.join("src/ompl/src", "*.cpp")),
+        include_dirs=ompl_module_includes,
+        libraries=["ompl"],
+        library_dirs=get_libraries_dir(),
+        define_macros=[("VERSION_INFO", __version__)],
+    ),
+    Pybind11Extension(
+        "kompass_cpp",
+        glob.glob(
+            os.path.join("src/kompass_cpp/kompass_cpp/src/**", "*.cpp"), recursive=True
+        ),
+        include_dirs=kompass_cpp_module_includes,
+        libraries=["fcl", "pcl_io_ply", "pcl_common"] + vcpkg_extra_libs,
+        library_dirs=get_libraries_dir(),
+        define_macros=[("VERSION_INFO", __version__)],
+    ),
+]
+
+# add parallel compilation if flag is set
+ParallelCompile("CMAKE_BUILD_PARALLEL_LEVEL", needs_recompile=naive_recompile).install()
+
+setup(
+    packages=find_packages(where="src"),
+    ext_modules=ext_modules,
+    cmdclass={"build_ext": build_ext},
+    zip_safe=False,
+)

kompass_core-0.2.2/src/kompass_core/algorithms/__init__.py

@@ -0,0 +1,3 @@
+from .dvz import DeformableVirtualZone, DeformableVirtualZoneParams
+
+__all__ = ["DeformableVirtualZone", "DeformableVirtualZoneParams"]