diff --git a/.github/workflows/build_and_deploy.yml b/.github/workflows/build_and_deploy.yml index ae6196b2..eb69c98a 100644 --- a/.github/workflows/build_and_deploy.yml +++ b/.github/workflows/build_and_deploy.yml @@ -16,7 +16,7 @@ jobs: matrix: os-arch: - { os: ubuntu-latest, arch: x86_64 } - - { os: ubuntu-24.04-arm, arch: aarch64 } + # - { os: ubuntu-24.04-arm, arch: aarch64 } name: Build wheels on ${{ matrix.os-arch.os }} ${{ matrix.os-arch.arch }} runs-on: ${{ matrix.os-arch.os }} @@ -27,6 +27,12 @@ jobs: - name: Set environment variables run: echo "CIBW_ARCHS=${{ matrix.os-arch.arch }}" >> $GITHUB_ENV + - name: Install Python development headers + run: | + PYTHON_VERSION=$(python3 -c "import sys; print(f'{sys.version_info.major}.{sys.version_info.minor}')") + sudo apt-get update + sudo apt-get install -y python${PYTHON_VERSION}-dev + - name: Build wheels uses: pypa/cibuildwheel@v2.21.3 diff --git a/README.md b/README.md index 4da4fe6d..229c71f2 100644 --- a/README.md +++ b/README.md @@ -1,9 +1,15 @@ # Kompass Core +[![中文版本][cn-badge]][cn-url] +[![ドキュメント-日本語][jp-badge]][jp-url] [![PyPI][pypi-badge]][pypi-url] [![MIT licensed][mit-badge]][mit-url] [![Python Version][python-badge]][python-url] +[cn-badge]: https://img.shields.io/badge/文档-中文-blue.svg +[cn-url]: docs/README.zh.md +[jp-badge]: https://img.shields.io/badge/ドキュメント-日本語-red.svg +[jp-url]: docs/README.ja.md [pypi-badge]: https://img.shields.io/pypi/v/kompass-core.svg [pypi-url]: https://pypi.org/project/kompass-core/ [mit-badge]: https://img.shields.io/pypi/l/kompass-core.svg @@ -11,95 +17,94 @@ [python-badge]: https://img.shields.io/pypi/pyversions/kompass-core.svg [python-url]: https://www.python.org/downloads/ -Kompass Core is a fast, GPU powered 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). The Kompass philosophy is to be blazzingly fast and highly reliable, by implementing GPGPU supported parallelized algorithms which are agnostic to underlying hardware. Thus Kompass Core can be run on CPUs, GPUs or FPGAs from a wide variety of vendors, making it easy for robot hardware manufacturers to switch underlying compute architecture. +Kompass Core is a high-performance, GPU-accelerated library for motion planning, mapping, and control in robot navigation systems. The core algorithms are implemented in C++ with seamless Python bindings. It also implements third party integrations with [OMPL](https://ompl.kavrakilab.org/) and [FCL](https://github.com/flexible-collision-library/fcl). The Kompass philosophy is to be blazzingly fast and highly reliable, by implementing GPGPU supported parallelized algorithms which are agnostic to underlying hardware. Thus Kompass Core can be run on CPUs or GPUs from a wide variety of vendors, making it easy for robot hardware manufacturers to switch underlying compute architecture without overhauling their software stack. This package is developed to be used with [Kompass](https://github.com/automatika-robotics/kompass) 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 with GPU Support (Recommended) +- [**Install**](#installation) Kompass Core 🛠️ +- Check the [**Package Overview**](#-package-overview) +- To use Kompass Core on your robot with ROS2, check the [**Kompass**](https://automatika-robotics.github.io/kompass) framework 🚀 -To install kompass-core with GPU support, on any Ubuntu 20+ (including Jetpack) based machine, you can simply run the following: -- `curl https://raw.githubusercontent.com/automatika-robotics/kompass-core/refs/heads/main/build_dependencies/install_gpu.sh | bash` +# Installation + +## Install with GPU Support (Recommended) + +- To install kompass-core with GPU support, on any Ubuntu 20+ (including Jetpack) based machine, you can simply run the following: + +```bash +curl https://raw.githubusercontent.com/automatika-robotics/kompass-core/refs/heads/main/build_dependencies/install_gpu.sh | bash +``` This script will install all relevant dependencies, including [AdaptiveCPP](https://github.com/AdaptiveCpp/AdaptiveCpp) and install the latest version of kompass-core from source. It is good practice to read the [script](https://github.com/automatika-robotics/kompass-core/blob/main/build_dependencies/install_gpu.sh) first. -### Installing with pip (CPU only) +## Installing with pip (CPU only) -On Ubuntu versions >= 22.04, install dependencies by running the following: +- On Ubuntu versions >= 22.04, install dependencies by running the following: -- `sudo apt-get install libompl-dev libfcl-dev libpcl-dev` +```bash +sudo apt-get install libompl-dev libfcl-dev libpcl-dev +``` -Then install kompass-core as follows: +- Then install kompass-core as follows: -- `pip install kompass-core` +```bash +pip install kompass-core +``` Wheels are available on Pypi for linux x86_64 and aarch64 architectures. Please note that the version available on Pypi does not support GPU acceleration yet. -### Installation Contents +## Installation Contents The following three packages will become available once kompass-core is installed. -- `kompass_core`: The main Python API containing all the wrappers and utilities for motion planning and control for navigation in 2D spaces. -- `kompass_cpp`: Python bindings for Kompass core C++ library containing the algorithms implementation for path tracking and motion control. +- `kompass_cpp`: Core C++ library for control, collision checking, and mapping algorithms. +- `kompass_core`: Python bindings for Kompass core C++ library with front-end classes for configuration and high-level logic. - `omplpy`: Bespoke python bindings for the Open Motion Planning Library (OMPL). -## Testing -### Run Planning Test +# 📦 Package Overview -- `cd tests` -- `python3 test_ompl.py` +The package includes modules for mapping, control, trajectory planning, and vision-based tracking algorithms, with **GPU acceleration** support and Python bindings via `nanobind`. -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 +### Control Module +- Includes a rich set of optimized C++ control strategies implementations and their python wrappers. +- Supports **GPU-accelerated** trajectory sampling and cost evaluation with customizable weights for sampling based controllers. +- Internally implements feature-based bounding box tracking and depth detection for enhanced vision-based tracking control. -- `cd tests` -- `python3 test_controllers.py` +| Algorithm | Description | +| ------------------------------------------- | -------------------------------------------------- | +| **Stanley** | Path tracking with robust convergence | +| **DWA (Dynamic Window Approach)** | Velocity-space sampling and optimization | +| **DVZ** | Reactive obstacle avoidance using deformable zones | +| **VisionRGBFollower** | Follow visual targets using RGB images | +| **VisionRGBDFollower** | Follow visual targets using RGBD (depth) images | -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 +### Mapping Module +- Implements efficient local mapping and occupancy grid generation algorithms, with configuration support for various laser models and grid resolution settings. +- Supports **GPU-accelerated** mapping for real-time performance. -## Usage Example -```python -from kompass_core.control import DVZ +### Utilities Module +- Provides collision checking utilities and critical zone detection to ensure safe navigation, including both CPU and GPU implementations. +- Logger utilities for runtime diagnostics. +- Linear state-space Kalman filter implementation for state estimation (C++). +- Spline interpolation utilities for path control. -from kompass_core.models import ( - AngularCtrlLimits, - LinearCtrlLimits, - Robot, - RobotCtrlLimits, - RobotGeometry, - RobotType, -) +### Data Types and Models Modules +- Rich set of data types to represent paths, trajectories, controls, velocities, bounding boxes and various sensor data. +- Strongly-typed parameters and configuration classes to enable flexible tuning. +- Robot models and motion kinematics, supporting differential, omni-directional, and Ackermann robots. Along with geometry definitions, control limits and simulation-ready state representations. -# Setup the robot -my_robot = Robot( - robot_type=RobotType.ACKERMANN, - geometry_type=RobotGeometry.Type.CYLINDER, - geometry_params=np.array([0.1, 0.4]), - ) +### Third Party Modules +Includes wrappers and integrations with external planning and collision libraries: -# 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 - ), -) +- FCL (Flexible Collision Library) -# Set the control time step (s) -control_time_step = 0.1 # seconds +- OMPL (Open Motion Planning Library) -# Initialize the controller -dvz = DVZ( - robot=my_robot, - ctrl_limits=robot_ctr_limits, - control_time_step=control_time_step, - ) -``` ## Copyright diff --git a/build_dependencies/install_gpu.sh b/build_dependencies/install_gpu.sh index 7fb12d73..76df7673 100644 --- a/build_dependencies/install_gpu.sh +++ b/build_dependencies/install_gpu.sh @@ -162,8 +162,8 @@ install_dependencies # Check for LLVM/Clang versions if [[ $LLVM_VERSION ]]; then # Check if given version is within range - if (( LLVM_VERSION < 14 || LLVM_VERSION > 17 )); then - log ERROR "LLVM Versions higher than 17 and lower than 14 are not compatible with kompass-core." + if (( LLVM_VERSION < 15 || LLVM_VERSION > 17 )); then + log ERROR "LLVM Versions higher than 20 and lower than 15 are not compatible with kompass-core." exit 1 fi if check_llvm_clang_version $LLVM_VERSION; then diff --git a/docs/README.ja.md b/docs/README.ja.md new file mode 100644 index 00000000..65330da3 --- /dev/null +++ b/docs/README.ja.md @@ -0,0 +1,156 @@ +# Kompass Core + +[![English Version][en-badge]][en-url] +[![中文版本][cn-badge]][cn-url] +[![PyPI][pypi-badge]][pypi-url] +[![MIT licensed][mit-badge]][mit-url] +[![Python Version][python-badge]][python-url] + +[en-badge]: https://img.shields.io/badge/Documentation-English-green.svg +[en-url]: ../README.md +[cn-badge]: https://img.shields.io/badge/文档-中文-blue.svg +[cn-url]: README.zh.md +[pypi-badge]: https://img.shields.io/pypi/v/kompass-core.svg +[pypi-url]: https://pypi.org/project/kompass-core/ +[mit-badge]: https://img.shields.io/pypi/l/kompass-core.svg +[mit-url]: https://github.com/automatika-robotics/kompass-core/LICENSE +[python-badge]: https://img.shields.io/pypi/pyversions/kompass-core.svg +[python-url]: https://www.python.org/downloads/ + +**Kompass Core** は、ロボットナビゲーションシステムにおける経路計画、マッピング、および制御のための高性能で GPU アクセラレーション対応のライブラリです。コアアルゴリズムは C++ で実装され、Python バインディングによってシームレスに利用できます。また、[OMPL](https://ompl.kavrakilab.org/) や [FCL](https://github.com/flexible-collision-library/fcl) との外部統合も備えています。 + +Kompass の理念は、「驚異的な高速性」と「高信頼性」を追求することです。GPGPU 対応の並列アルゴリズムを基盤とし、ハードウェアに依存しない設計を実現しているため、Kompass Core はさまざまなベンダーの CPU または GPU 上で実行可能です。これにより、ロボットハードウェアメーカーは、ソフトウェアスタックを大きく変更することなく、計算アーキテクチャを柔軟に切り替えることが可能です。 + +このパッケージは、[ROS2](https://docs.ros.org/en/rolling/index.html) 上でナビゲーションスタックを構築するための [Kompass](https://github.com/automatika-robotics/kompass) と共に使用するように設計されています。詳細な使用方法については、[Kompass ドキュメント](https://automatika-robotics.github.io/kompass/) をご覧ください。 + +- [**インストール**](#installation) Kompass Core 🛠️ +- [**パッケージ概要**](#-package-overview) を確認 +- ROS2 環境でロボットに Kompass Core を導入したい場合は、[**Kompass**](https://automatika-robotics.github.io/kompass) フレームワークをご参照ください 🚀 + +# インストール方法 + +## GPU サポート付きインストール（推奨） + +Ubuntu 20 以降（Jetpack を含む）の任意のマシンに GPU サポート付きで kompass-core をインストールするには、以下を実行します： + +```bash +curl https://raw.githubusercontent.com/automatika-robotics/kompass-core/refs/heads/main/build_dependencies/install_gpu.sh | bash +``` + +このスクリプトは、[AdaptiveCPP](https://github.com/AdaptiveCpp/AdaptiveCpp) を含むすべての関連依存関係をインストールし、`kompass-core` の最新版をソースから構築します。実行前に、[スクリプト](https://github.com/automatika-robotics/kompass-core/blob/main/build_dependencies/install_gpu.sh) の内容を確認することを推奨します。 + + +## pip によるインストール（CPU のみ） + +- Ubuntu 22.04 以降では、以下のコマンドで依存パッケージをインストールします： + +```bash + sudo apt-get install libompl-dev libfcl-dev libpcl-dev +``` +- その後、以下のように kompass-core をインストールします： + +```bash +pip install kompass-core +``` + +PyPI では、Linux x86_64 と aarch64 向けのホイールが提供されています。なお、現時点で PyPI にあるバージョンは GPU アクセラレーションには対応していません。 + + +## インストール内容 + +kompass-core をインストールすると、以下の 3 つのパッケージが使用可能になります。 + +- `kompass_core`：2D 空間でのナビゲーションのための運動計画と制御に関するラッパーやユーティリティを含む主要な Python API +- `kompass_cpp`：経路追跡および運動制御アルゴリズムを実装した Kompass コア C++ ライブラリの Python バインディング +- `omplpy`：Open Motion Planning Library（OMPL）向けに特化した Python バインディング + +# 📦 パッケージ概要 + +本リポジトリには以下のモジュールが含まれます： + +- `kompass_cpp/` — 計画、制御、衝突判定、マッピングアルゴリズムを実装したコア C++ モジュール + +- `kompass_core/` — 設定や高レベルロジックのための Python 実装およびフロントエンドクラス + +## `kompass_cpp` モジュール概要 + +`kompass_cpp/` はマッピング、制御、軌道計画、視覚ベースのトラッキングアルゴリズムを含む C++ パッケージであり、**GPU アクセラレーション** をサポートし、`nanobind` 経由で Python バインディングが提供されています。 + +### 1. マッピング +- 高速な局所マッピングアルゴリズムを実装 +- **GPU アクセラレーション** に対応しリアルタイム性能を実現 +- 主なクラス：`LocalMapper`, `LocalMapperGPU` + +### 2. 制御と軌道計画 +- PID、Stanley、動的ウィンドウ法（DWA）、ビジョンガイドコントローラなど複数の制御戦略を搭載 +- **GPU アクセラレーション** による軌道サンプリングとコスト評価、重みのカスタマイズが可能 +- 主なクラス：`Controller`, `PID`, `Stanley`, `DWA`, `VisionDWA`, `TrajectorySampler`, `CostEvaluator` + +### 3. 衝突判定とクリティカルゾーン検出 +- 安全なナビゲーションを実現する衝突判定とクリティカルゾーン検出機能を提供 +- CPU 実装と GPU 実装の両方に対応 +- 主なクラス：`CollisionChecker`, `CriticalZoneChecker`, `CriticalZoneCheckerGPU` + +### 4. ビジョンとトラッキング +- 特徴点ベースのバウンディングボックス追跡と深度検出により認識性能を強化 +- 頑健な視覚ベースのナビゲーションアルゴリズムをサポート +- 主なクラス：`FeatureBasedBboxTracker`, `DepthDetector` + +### 5. ユーティリティ +- 高効率なマルチスレッド処理を実現するスレッドプール +- 実行時診断用のロガー +- 線形状態空間カルマンフィルタによる状態推定 +- `tk` 名前空間で提供されるスプライン補間ユーティリティ + +### 6. データ型とパラメータ +- 経路、軌道、制御、速度、バウンディングボックスを表現する豊富なデータ型 +- 柔軟なパラメータ調整を可能にする強型の設定クラス + +### 7. Python バインディング +- `nanobind` によって構築された包括的な Python バインディングにより、Python ワークフローとシームレスに統合可能 +- マッピング、制御、ビジョン、ユーティリティの主要機能を広くカバー + +## `kompass_core` モジュール概要 + +- `kompass_core.calibration` - ロボット運動モデルのキャリブレーション、フィッティング、ロボットシミュレーション用モジュール + +- `kompass_core.control` - 多様な制御戦略と設定を含む。C++ 実装の Python ラッパークラスを提供： + +| アルゴリズム名 | 説明 | +|---------------------------------------|------------------------------------------| +| **Stanley** | 高い収束性能を持つ経路追従 | +| **DWA（動的ウィンドウ法）** | 速度空間のサンプリングと最適化 | +| **DVZ** | 可変ゾーンを用いたリアクティブな障害物回避 | +| **VisionRGBFollower** | RGB 画像を用いた視覚ターゲット追従 | +| **VisionRGBDFollower** | RGBD（深度付き）画像を用いた視覚ターゲット追従 | + +- `kompass_core.datatypes` - ロボットやセンサーデータ用の標準メッセージ・データ形式 + +- `kompass_core.mapping` - 局所マッピングおよび占有グリッド生成。さまざまなレーザーモデルとグリッド解像度設定に対応 + +- `kompass_core.models` - 差動型、全方向型、アッカーマン型ロボットの運動モデルおよび運動学をサポート。ジオメトリ定義、制御制限、シミュレーション用の状態表現も提供 + +- `kompass_core.motion_cost` - Python 上で使用可能な軌道評価用コストモデル（衝突確率、リファレンストラッキング、動的・静的障害物対応を含む） + +- `kompass_core.performance` - アルゴリズム性能評価用モジュール + +- `kompass_core.py_path_tools` - 経路補間と実行ツール + +- `kompass_core.simulation` - ロボット運動のシミュレーションと経路の実行可能性評価ツール + +- `kompass_core.third_party` - 外部計画ライブラリおよび衝突判定ライブラリとのラッパーと統合： + + - FCL（Flexible Collision Library） + + - OMPL（Open Motion Planning Library） + +- `kompass_core.utils` - 汎用ユーティリティ群 + +## 著作権 + +本配布物に含まれるコードは、特記がない限り 2024 年 Automatika Robotics に著作権があります。 +Kompass Core は MIT ライセンスのもとで公開されています。詳細は [LICENSE](LICENSE) ファイルをご覧ください。 + +## コントリビューション + +Kompass Core は [Automatika Robotics](https://automatikarobotics.com/) と [Inria](https://inria.fr/) の共同開発プロジェクトです。コミュニティからの貢献は大歓迎です。 diff --git a/docs/README.zh.md b/docs/README.zh.md new file mode 100644 index 00000000..d0d6f217 --- /dev/null +++ b/docs/README.zh.md @@ -0,0 +1,153 @@ +# Kompass Core（核心库） + +[![English Version][en-badge]][en-url] +[![ドキュメント-日本語][jp-badge]][jp-url] +[![PyPI][pypi-badge]][pypi-url] +[![MIT 许可][mit-badge]][mit-url] +[![Python 版本][python-badge]][python-url] + +[en-badge]: https://img.shields.io/badge/Documentation-English-green.svg +[en-url]: ../README.md +[jp-badge]: https://img.shields.io/badge/ドキュメント-日本語-red.svg +[jp-url]: README.ja.md +[pypi-badge]: https://img.shields.io/pypi/v/kompass-core.svg +[pypi-url]: https://pypi.org/project/kompass-core/ +[mit-badge]: https://img.shields.io/pypi/l/kompass-core.svg +[mit-url]: https://github.com/automatika-robotics/kompass-core/LICENSE +[python-badge]: https://img.shields.io/pypi/pyversions/kompass-core.svg +[python-url]: https://www.python.org/downloads/ + +Kompass Core 是一个高性能、支持 GPU 加速的库，用于机器人导航系统中的运动规划、建图与控制。核心算法采用 C++ 实现，并通过无缝 Python 绑定提供接口。它还集成了第三方库 [OMPL](https://ompl.kavrakilab.org/) 和 [FCL](https://github.com/flexible-collision-library/fcl)。Kompass 的设计哲学是极致高速与高度可靠，通过 GPGPU 支持的并行化算法实现与底层硬件无关。因此，Kompass Core 可运行于多种厂商的 CPU 或 GPU 上，便于机器人硬件厂商在不更换软件栈的情况下切换计算平台。 + +本软件包可与 [Kompass](https://github.com/automatika-robotics/kompass) 一起使用，用于在 [ROS2](https://docs.ros.org/en/rolling/index.html) 中构建导航栈。详细使用文档请见 [Kompass 文档](https://automatika-robotics.github.io/kompass/)。 + +- [**安装指南**](#安装指南) Kompass Core 🛠️ +- 查看 [**软件包概览**](#-软件包概览) +- 若需在 ROS2 中使用 Kompass Core，请访问 [**Kompass 框架**](https://automatika-robotics.github.io/kompass) 🚀 + +# 安装指南 + +## 安装支持 GPU 的版本（推荐） + +- 要在任何基于 Ubuntu 20+（包括 Jetpack）系统的机器上安装支持 GPU 的 kompass-core，只需运行以下命令： + +```bash +curl https://raw.githubusercontent.com/automatika-robotics/kompass-core/refs/heads/main/build_dependencies/install_gpu.sh | bash +``` + +该脚本将安装所有相关的依赖项，包括 [AdaptiveCPP](https://github.com/AdaptiveCpp/AdaptiveCpp)，并从源代码安装最新版的 kompass-core。建议您先阅读该[脚本](https://github.com/automatika-robotics/kompass-core/blob/main/build_dependencies/install_gpu.sh)。 + +## 使用 pip 安装（仅支持 CPU） + +- 在 Ubuntu 22.04 或更高版本上，请先运行以下命令安装依赖项： + +```bash +sudo apt-get install libompl-dev libfcl-dev libpcl-dev +``` + +- 然后按如下方式安装 kompass-core： +```bash +pip install kompass-core +``` + +适用于 linux x86_64 和 aarch64 架构的安装包已发布在 Pypi 上。请注意，Pypi 上提供的版本暂不支持 GPU 加速。 + +## 安装内容 + +安装 kompass-core 后，将提供以下三个软件包： + +- `kompass_core`：主 Python API，包含用于 2D 空间导航的运动规划与控制的所有封装器与工具。 +- `kompass_cpp`：Kompass 核心 C++ 库的 Python 绑定，包含路径跟踪与运动控制算法的实现。 +- `omplpy`：为 OMPL（开源运动规划库）量身定制的 Python 绑定。 + +# 📦 软件包概览 + +本仓库包含以下模块： + +- `kompass_cpp/` — 规划、控制、碰撞检测与建图算法的核心 C++ 实现。 + +- `kompass_core/` — Python 实现与前端配置类，支持高级逻辑开发。 + +## `kompass_cpp` 模块概览 + +`kompass_cpp/` 是一个包含建图、控制、轨迹规划与基于视觉的跟踪算法的 C++ 软件包，支持 **GPU 加速**，并通过 `nanobind` 提供 Python 绑定。 + +### 1. 建图 +- 实现高效的局部建图算法。 +- 支持 **GPU 加速**，实现实时性能。 +- 核心类：`LocalMapper`, `LocalMapperGPU` + +### 2. 控制与轨迹规划 +- 支持多种控制策略，包括 PID、Stanley、动态窗口法（DWA）与视觉引导控制器。 +- 支持 **GPU 加速** 的轨迹采样与代价评估，可自定义权重。 +- 核心类：`Controller`, `PID`, `Stanley`, `DWA`, `VisionDWA`, `TrajectorySampler`, `CostEvaluator` + +### 3. 碰撞检测与关键区域检测 +- 提供碰撞检测工具与关键区域识别功能，保障导航安全。 +- 提供 CPU 与 GPU 两种实现。 +- 核心类：`CollisionChecker`, `CriticalZoneChecker`, `CriticalZoneCheckerGPU` + +### 4. 视觉与跟踪 +- 基于特征的边框跟踪与深度检测，增强感知能力。 +- 支持鲁棒的视觉导航算法。 +- 核心类：`FeatureBasedBboxTracker`, `DepthDetector` + +### 5. 实用工具 +- 多线程池用于高效的并发处理。 +- 日志工具支持运行时诊断。 +- 基于线性状态空间的卡尔曼滤波器用于状态估计。 +- `tk` 命名空间下的样条插值工具。 + +### 6. 数据类型与参数 +- 提供丰富的数据结构，用于表示路径、轨迹、控制、速度与边框等。 +- 强类型参数与配置类，支持灵活调参。 + +### 7. Python 绑定 +- 使用 `nanobind` 构建的全面 Python 绑定，实现与 Python 工作流的无缝集成。 +- 绑定涵盖建图、控制、视觉与工具等核心功能。 + +## `kompass_core` 模块概览 + +- `kompass_core.calibration` - 机器人运动模型的校准、拟合与仿真模块。 + +- `kompass_core.control` - 多种控制策略及配置，包含 C++ 控制器的 Python 包装类： + +| 算法名称 | 描述 | +| ------------------------------------ | ------------------------------------------------- | +| **Stanley** | 具备鲁棒收敛性的路径跟踪 | +| **DWA（动态窗口法）** | 基于速度空间的采样与优化 | +| **DVZ** | 使用可变形区域的反应式避障 | +| **VisionRGBFollower** | 基于 RGB 图像跟踪视觉目标 | +| **VisionRGBDFollower** | 基于 RGBD 图像（含深度）跟踪视觉目标 | + +- `kompass_core.datatypes` - 标准化的机器人与传感器数据格式。 + +- `kompass_core.mapping` - 局部建图与占据栅格生成，支持多种激光模型与分辨率配置。 + +- `kompass_core.models` - 机器人模型与运动学，支持差动式、全向轮式与 Ackermann 结构；包含几何定义、控制限制与仿真状态表示。 + +- `kompass_core.motion_cost` - Python 中用于轨迹评估的代价模型，支持包括碰撞概率、参考轨迹跟踪、动态/静态障碍物等多种代价函数。 + +- `kompass_core.performance` - 用于算法性能评估的模块。 + +- `kompass_core.py_path_tools` - 路径插值与执行工具。 + +- `kompass_core.simulation` - 用于机器人运动仿真与路径可行性评估的工具。 + +- `kompass_core.third_party` - 与外部规划与碰撞库的封装与集成： + + - FCL（灵活碰撞库） + + - OMPL（开源运动规划库） + +- `kompass_core.utils` - 通用工具函数集。 + +## 版权信息 + +本发行版中的源代码（除非另有说明）版权归 Automatika Robotics 所有 © 2024。 + +Kompass Core 遵循 MIT 开源许可证。详细信息请见 [LICENSE](LICENSE) 文件。 + +## 贡献说明 + +Kompass Core 由 [Automatika Robotics](https://automatikarobotics.com/) 与 [Inria](https://inria.fr/) 合作开发，欢迎社区开发者贡献代码与文档。 diff --git a/pyproject.toml b/pyproject.toml index 78ef0369..b76f10f5 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -20,15 +20,10 @@ license = { text = "MIT" } keywords = ["robotics", "gpgpu", "navigation", "control", "planning", "mapping"] dependencies = [ - "numpy<=1.26.4", - "numpy-quaternion>=2023.0.3", - "python-fcl-fork", - "omegaconf", + "numpy", + "toml", "attrs>=23.2.0", "pyyaml", - "scipy", - "matplotlib", - "pandas", ] requires-python = ">=3.8.4,<3.13" @@ -137,4 +132,3 @@ select = "*-manylinux_aarch64" inherit.environment = "append" environment.VCPKG_TARGET_TRIPLET = "arm64-linux-release" environment.LD_LIBRARY_PATH="$LD_LIBRARY_PATH:$VCPKG_ROOT/installed/$VCPKG_TARGET_TRIPLET/lib" - diff --git a/src/kompass_core/__init__.py b/src/kompass_core/__init__.py index e69de29b..3b588519 100644 --- a/src/kompass_core/__init__.py +++ b/src/kompass_core/__init__.py @@ -0,0 +1,14 @@ +import kompass_cpp + + +def set_logging_level(level: str): + """Set the logging level to "debug", "warn", "error" or "info" + + :param level: Logging level + :type level: str + """ + level = level.upper() + kompass_cpp.set_log_level(getattr(kompass_cpp.LogLevel, level)) + + +__all__ = ["set_logging_level"] diff --git a/src/kompass_core/algorithms/dvz.py b/src/kompass_core/algorithms/dvz.py index 94fd0413..1d490142 100644 --- a/src/kompass_core/algorithms/dvz.py +++ b/src/kompass_core/algorithms/dvz.py @@ -3,10 +3,8 @@ from ..utils.common import base_validators, BaseAttrs from ..utils.geometry import convert_to_0_2pi, convert_to_plus_minus_pi -import matplotlib.pyplot as plt import numpy as np from attrs import define, field -from matplotlib.patches import Ellipse, Polygon from ..models import Robot, RobotState, RobotCtrlLimits @@ -121,12 +119,12 @@ def _init_constant_zone_parameters(self) -> None: self.zone_shift_y_diff: float = 0.0 def set_from_yaml(self, path_to_file: str) -> None: - """Setup the DVZ controller params from YAML. + """Setup the DVZ controller params from file. :param path_to_file: :type path_to_file: str """ - self.config.from_yaml(path_to_file, nested_root_name="DVZ") + self.config.from_file(path_to_file, nested_root_name="DVZ") self._set_control_regularization() def _set_control_regularization(self) -> None: @@ -512,6 +510,13 @@ def plt_robot_zone( :param display_now: Display the figure, defaults to False :type display_now: bool, optional """ + try: + import matplotlib.pyplot as plt + from matplotlib.patches import Ellipse, Polygon + except ImportError as e: + raise ImportError( + "Matplotlib is required for visualization. Please install it using 'pip install matplotlib'." + ) from e # Create new figure if none is given if not fig_ax: fig_ax = plt.gca() diff --git a/src/kompass_core/calibration.py b/src/kompass_core/calibration.py index 153d65fd..2bf50d3c 100644 --- a/src/kompass_core/calibration.py +++ b/src/kompass_core/calibration.py @@ -1,10 +1,8 @@ import logging from .utils.geometry import convert_to_plus_minus_pi -import matplotlib.pyplot as plt import numpy as np from .datatypes.path import MotionSample, PathSample -from scipy import optimize from .models import MotionModel2D, Robot, RobotState from .simulation import RobotSim @@ -63,6 +61,12 @@ def fit_data(self, log: bool = False) -> MotionModel2D: :return: Calibrated robot model :rtype: MotionModel2D """ + try: + from scipy import optimize + except ImportError as e: + raise ImportError( + "SciPy is required for optimization. Please install it using 'pip install scipy'." + ) from e motion_model = self.robot.state.model vx_opt, vx_cov = optimize.curve_fit( MotionModel2D.x_model, @@ -132,7 +136,7 @@ def simulate_calibrated_model_data( x=motion_sample.path_sample.x_points[0], y=motion_sample.path_sample.y_points[0], yaw=motion_sample.path_sample.heading_points[0], - motion_model=calibrated_model, + model=calibrated_model, ) robot = Robot( @@ -181,6 +185,13 @@ def vis_calibration(cls, robot_data: MotionSample, modeled_path: PathSample): :param modeled_path: Motion model output data :type modeled_path: PathSample """ + try: + import matplotlib.pyplot as plt + except ImportError: + logging.error( + "Matplotlib is not installed. Please install it to visualize the calibration results." + ) + return fig, [ax0, ax1, ax2, ax3] = plt.subplots(nrows=4, ncols=1, figsize=(8, 8)) fig.suptitle("Calibration Results") fig.tight_layout(pad=3.0) diff --git a/src/kompass_core/control/__init__.py b/src/kompass_core/control/__init__.py index b411408a..0b7f8c10 100644 --- a/src/kompass_core/control/__init__.py +++ b/src/kompass_core/control/__init__.py @@ -10,7 +10,8 @@ from .dvz import DVZ, DVZConfig from .dwa import DWA, DWAConfig from .stanley import StanleyConfig, Stanley -from .vision_follower import VisionFollower, VisionFollowerConfig +from .rgb_follower import VisionRGBFollower, VisionRGBFollowerConfig +from .rgbd_follower import VisionRGBDFollower, VisionRGBDFollowerConfig ControllerType = FollowerTemplate @@ -70,19 +71,24 @@ class ControllersID(StrEnum): STANLEY = "Stanley" DWA = "DWA" DVZ = "DVZ" - VISION = "VisionFollower" + VISION_IMG = "VisionRGBFollower" + VISION_DEPTH = "VisionRGBDFollower" ControlClasses = { ControllersID.STANLEY: Stanley, ControllersID.DVZ: DVZ, ControllersID.DWA: DWA, + ControllersID.VISION_IMG: VisionRGBFollower, + ControllersID.VISION_DEPTH: VisionRGBDFollower, } ControlConfigClasses = { ControllersID.STANLEY: StanleyConfig, ControllersID.DVZ: DVZConfig, ControllersID.DWA: DWAConfig, + ControllersID.VISION_IMG: VisionRGBFollowerConfig, + ControllersID.VISION_DEPTH: VisionRGBDFollowerConfig, } @@ -100,6 +106,8 @@ class ControllersID(StrEnum): "DWA", "DWAConfig", "TrajectoryCostsWeights", - "VisionFollower", - "VisionFollowerConfig", + "VisionRGBFollower", + "VisionRGBFollowerConfig", + "VisionRGBDFollower", + "VisionRGBDFollowerConfig", ] diff --git a/src/kompass_core/control/_base_.py b/src/kompass_core/control/_base_.py index 1f2ca6cf..429e9516 100644 --- a/src/kompass_core/control/_base_.py +++ b/src/kompass_core/control/_base_.py @@ -1,5 +1,5 @@ from abc import abstractmethod -from typing import Optional, List +from typing import Optional, List, Union import numpy as np import kompass_cpp from ..models import RobotState @@ -95,7 +95,7 @@ def logging_info(self) -> str: @property @abstractmethod - def linear_x_control(self): + def linear_x_control(self) -> Union[List[float], np.ndarray]: """ Getter of the last linear forward velocity control computed by the controller @@ -106,7 +106,7 @@ def linear_x_control(self): @property @abstractmethod - def linear_y_control(self): + def linear_y_control(self) -> Union[List[float], np.ndarray]: """ Getter the last linear velocity lateral control computed by the controller @@ -117,7 +117,7 @@ def linear_y_control(self): @property @abstractmethod - def angular_control(self): + def angular_control(self) -> Union[List[float], np.ndarray]: """ Getter of the last angular velocity control computed by the controller @@ -255,7 +255,7 @@ def tracked_state(self) -> Optional[RobotState]: @property @abstractmethod - def linear_x_control(self) -> List[float]: + def linear_x_control(self) -> Union[List[float], np.ndarray]: """ Getter of the last linear forward velocity control computed by the controller @@ -266,7 +266,7 @@ def linear_x_control(self) -> List[float]: @property @abstractmethod - def linear_y_control(self) -> List[float]: + def linear_y_control(self) -> Union[List[float], np.ndarray]: """ Getter the last linear velocity lateral control computed by the controller @@ -277,7 +277,7 @@ def linear_y_control(self) -> List[float]: @property @abstractmethod - def angular_control(self) -> List[float]: + def angular_control(self) -> Union[List[float], np.ndarray]: """ Getter of the last angular velocity control computed by the controller @@ -298,7 +298,7 @@ def distance_error(self) -> float: return target.crosstrack_error @property - def orientation_error(self) -> float: + def orientation_error(self) -> Union[float, np.ndarray]: """ Getter of the path tracking orientation error (rad) diff --git a/src/kompass_core/control/dvz.py b/src/kompass_core/control/dvz.py index 139179c2..6e647f2a 100644 --- a/src/kompass_core/control/dvz.py +++ b/src/kompass_core/control/dvz.py @@ -71,11 +71,11 @@ def __init__( :type ctrl_limits: RobotCtrlLimits :param control_time_step: Time step (s) :type control_time_step: float - :param config_file: Path to YAML config file, defaults to None + :param config_file: Path to config file, defaults to None :type config_file: Optional[str], optional :param config: DVZ configuration, defaults to None :type config: Optional[DVZConfig], optional - :param config_yaml_root_name: Root name for the config in the YAML file, defaults to None + :param config_yaml_root_name: Root name for the config in the config file, defaults to None :type config_yaml_root_name: Optional[str], optional """ # Init the controller @@ -130,7 +130,7 @@ def interpolated_path(self) -> kompass_cpp.types.Path: return self.__reference_cmd_generator.interpolated_path() @property - def tracked_state(self) -> RobotState: + def tracked_state(self) -> Optional[RobotState]: """ Tracked state on the path @@ -228,7 +228,7 @@ def logging_info(self) -> str: :rtype: str """ - return f"total deformation : {self._path_controller.dvz_controller.total_deformation}" + return f"Total DVZ deformation : {self._path_controller.total_deformation}" @property def linear_x_control(self) -> List[float]: diff --git a/src/kompass_core/control/dwa.py b/src/kompass_core/control/dwa.py index 553de937..e125df76 100644 --- a/src/kompass_core/control/dwa.py +++ b/src/kompass_core/control/dwa.py @@ -1,5 +1,5 @@ import logging -from typing import List, Optional +from typing import Optional, Union, List import numpy as np from attrs import Factory, define, field from ..datatypes.laserscan import LaserScanData @@ -96,13 +96,15 @@ class DWAConfig(FollowerConfig): default=0.1, validator=base_validators.in_range(min_value=1e-4, max_value=1e6) ) - prediction_horizon: float = field( - default=1.0, validator=base_validators.in_range(min_value=1e-4, max_value=1e6) + control_horizon: int = field( + default=2, validator=base_validators.in_range(min_value=1, max_value=1000) ) + # Number of steps for applying the control - control_horizon: float = field( - default=0.2, validator=base_validators.in_range(min_value=1e-4, max_value=1e6) + prediction_horizon: int = field( + default=10, validator=base_validators.in_range(min_value=1, max_value=1000) ) + # Number of steps for future prediction max_linear_samples: int = field( default=20, validator=base_validators.in_range(min_value=1, max_value=1e3) @@ -112,9 +114,13 @@ class DWAConfig(FollowerConfig): default=20, validator=base_validators.in_range(min_value=1, max_value=1e3) ) - sensor_position_to_robot: List[float] = field(default=[0.0, 0.0, 0.0]) + proximity_sensor_position_to_robot: np.ndarray = field( + default=np.array([0.0, 0.0, 0.0], dtype=np.float32) + ) - sensor_rotation_to_robot: List[float] = field(default=[0.0, 0.0, 0.0, 1.0]) + proximity_sensor_rotation_to_robot: np.ndarray = field( + default=np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32) + ) octree_resolution: float = field( default=0.1, validator=base_validators.in_range(min_value=1e-9, max_value=1e3) @@ -188,47 +194,48 @@ def __init__( ctrl_limits: RobotCtrlLimits, config: Optional[DWAConfig] = None, config_file: Optional[str] = None, - config_yaml_root_name: Optional[str] = None, + config_root_name: Optional[str] = None, control_time_step: Optional[float] = None, **_, ): - """ - Setup the controller + """Init DWA (Dynamic Window Approach) Controller - :param robot: Robot using the controller + :param robot: Robot object to be controlled :type robot: Robot - :param params_file: Yaml file containing the parameters of the controller under 'dvz_controller' - :type params_file: str + :param ctrl_limits: Robot control limits + :type ctrl_limits: RobotCtrlLimits + :param config: Controller configuration, defaults to None + :type config: Optional[VisionRGBDFollowerConfig], optional + :param config_file: Path to config file (yaml, json, toml), defaults to None + :type config_file: Optional[str], optional + :param config_root_name: Root name for the controller config in the file, defaults to None + :type config_root_name: Optional[str], optional + :param control_time_step: Control time step (sec), defaults to None + :type control_time_step: Optional[float], optional """ # Init and configure the planner - if not config: - # Default config - config = DWAConfig() + self._config = config or DWAConfig() if config_file: - config.from_yaml( - file_path=config_file, nested_root_name=config_yaml_root_name - ) + self._config.from_file(file_path=config_file, nested_root_name=config_root_name) if control_time_step: - config.control_time_step = control_time_step + self._config.control_time_step = control_time_step self._got_path = False - self._config = config - self._planner = kompass_cpp.control.DWA( control_limits=ctrl_limits.to_kompass_cpp_lib(), control_type=RobotType.to_kompass_cpp_lib(robot.robot_type), time_step=config.control_time_step, - prediction_horizon=config.prediction_horizon, - control_horizon=config.control_horizon, + prediction_horizon=config.prediction_horizon * config.control_time_step, + control_horizon=config.control_horizon * config.control_time_step, max_linear_samples=config.max_linear_samples, max_angular_samples=config.max_angular_samples, robot_shape_type=RobotGeometry.Type.to_kompass_cpp_lib(robot.geometry_type), robot_dimensions=robot.geometry_params, - sensor_position_robot=config.sensor_position_to_robot, - sensor_rotation_robot=config.sensor_rotation_to_robot, + sensor_position_robot=config.proximity_sensor_position_to_robot, + sensor_rotation_robot=config.proximity_sensor_rotation_to_robot, octree_resolution=config.octree_resolution, cost_weights=config.costs_weights.to_kompass_cpp(), max_num_threads=config.max_num_threads, @@ -236,9 +243,7 @@ def __init__( # Init the following result self._result = kompass_cpp.control.SamplingControlResult() - self._end_of_ctrl_horizon: int = max( - int(self._config.control_horizon / self._config.control_time_step), 1 - ) + self._end_of_ctrl_horizon: int = max(self._config.control_horizon, 1) logging.info("DWA PATH CONTROLLER IS READY") @property @@ -351,7 +356,7 @@ def logging_info(self) -> str: @property def control_till_horizon( self, - ) -> Optional[List[kompass_cpp.types.TrajectoryVelocities2D]]: + ) -> Optional[kompass_cpp.types.TrajectoryVelocities2D]: """ Getter of the planner control result until the control horizon @@ -381,7 +386,7 @@ def result_cost(self) -> Optional[float]: return None @property - def linear_x_control(self) -> np.ndarray: + def linear_x_control(self) -> Union[List[float], np.ndarray]: """ Getter of the last linear forward velocity control computed by the controller @@ -393,7 +398,7 @@ def linear_x_control(self) -> np.ndarray: return [0.0] @property - def linear_y_control(self) -> np.ndarray: + def linear_y_control(self) -> Union[List[float], np.ndarray]: """ Getter the last linear velocity lateral control computed by the controller @@ -405,7 +410,7 @@ def linear_y_control(self) -> np.ndarray: return [0.0] @property - def angular_control(self) -> np.ndarray: + def angular_control(self) -> Union[List[float], np.ndarray]: """ Getter of the last angular velocity control computed by the controller diff --git a/src/kompass_core/control/rgb_follower.py b/src/kompass_core/control/rgb_follower.py new file mode 100644 index 00000000..b2b13795 --- /dev/null +++ b/src/kompass_core/control/rgb_follower.py @@ -0,0 +1,283 @@ +from ._base_ import ControllerTemplate +import logging +from typing import Optional, List +from attrs import define, field +from ..utils.common import BaseAttrs, base_validators +from ..models import Robot, RobotCtrlLimits, RobotType +from kompass_cpp.control import RGBFollower, RGBFollowerParameters +from kompass_cpp.types import Bbox2D +import numpy as np + + +@define +class VisionRGBFollowerConfig(BaseAttrs): + """ + Configuration class for an RGB-based vision target follower. + + This class defines configuration parameters for controlling a robot that follows a target using RGB vision. + It provides settings for control behavior, target tracking, search strategies, velocity and tolerance tuning, + and camera-to-robot coordinate transformations. + + Attributes: + control_time_step (float): Time interval between control updates (s). + control_horizon (int): Number of time steps in the control planning horizon. + buffer_size (int): Number of buffered detections to maintain. + tolerance (float): Acceptable error when tracking the target. + target_distance (Optional[float]): Desired distance to maintain from the target (m). + target_wait_timeout (float): Maximum time to wait for a target to reappear if lost (s). + target_search_timeout (float): Maximum duration to perform a search when target is lost (s). + target_search_pause (float): Delay between successive search attempts (s). + target_search_radius (float): Radius used for searching the target (m). + rotation_gain (float): Proportional gain for angular control. + speed_gain (float): Proportional gain for linear speed control. + min_vel (float): Minimum linear velocity allowed during target following (m/s). + enable_search (bool): Whether to activate search behavior when the target is lost. + """ + + control_time_step: float = field( + default=0.1, validator=base_validators.in_range(min_value=1e-4, max_value=1e6) + ) + control_horizon: int = field( + default=2, validator=base_validators.in_range(min_value=1, max_value=1000) + ) + buffer_size: int = field( + default=1, validator=base_validators.in_range(min_value=1, max_value=10) + ) + tolerance: float = field( + default=0.1, validator=base_validators.in_range(min_value=1e-6, max_value=1.0) + ) + target_distance: Optional[float] = field(default=None) + target_wait_timeout: float = field( + default=30.0, validator=base_validators.in_range(min_value=0.0, max_value=1e3) + ) # wait for target to appear again timeout (seconds), used if search is disabled + target_search_timeout: float = field( + default=30.0, validator=base_validators.in_range(min_value=0.0, max_value=1e3) + ) # search timeout in seconds + target_search_pause: float = field( + default=2.0, validator=base_validators.in_range(min_value=0.0, max_value=1e3) + ) # pause between search actions to find target (seconds) + target_search_radius: float = field( + default=0.5, validator=base_validators.in_range(min_value=1e-4, max_value=1e4) + ) + rotation_gain: float = field( + default=1.0, validator=base_validators.in_range(min_value=1e-9, max_value=1.0) + ) + speed_gain: float = field( + default=0.7, validator=base_validators.in_range(min_value=1e-9, max_value=10.0) + ) + min_vel: float = field( + default=0.1, validator=base_validators.in_range(min_value=1e-9, max_value=1e9) + ) + enable_search: bool = field(default=True) + + camera_position_to_robot: np.ndarray = field( + default=np.array([0.0, 0.0, 0.0], dtype=np.float32) + ) + + camera_rotation_to_robot: np.ndarray = field( + default=np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32) + ) + + def to_kompass_cpp(self) -> RGBFollowerParameters: + """ + Convert to kompass_cpp lib config format + + :return: _description_ + :rtype: kompass_cpp.control.VisionFollowerParameters + """ + vision_config = RGBFollowerParameters() + vision_config.from_dict(self.asdict()) + return vision_config + + +class VisionRGBFollower(ControllerTemplate): + """ + VisionRGBFollower is a controller for vision-based target following using RGB image data. + + This controller processes 2D object detections (e.g., bounding boxes) and generates velocity commands + to follow a visual target using a proportional control law. It supports configuration via Python or external + configuration files and allows integration into Kompass-style robotic systems. + + - Usage Example: + + ```python + import numpy as np + from kompass_core.control import VisionRGBFollower, VisionRGBFollowerConfig + from kompass_core.models import ( + Robot, + RobotType, + RobotCtrlLimits, + LinearCtrlLimits, + AngularCtrlLimits, + RobotGeometry, + ) + from kompass_core.datatypes import Bbox2D + + # Define robot + my_robot = Robot( + robot_type=RobotType.ACKERMANN, + geometry_type=RobotGeometry.Type.CYLINDER, + geometry_params=np.array([0.2, 0.5]) + ) + + # Define control limits + ctrl_limits = RobotCtrlLimits( + vx_limits=LinearCtrlLimits(max_vel=1.0, max_acc=2.0, max_decel=4.0), + omega_limits=AngularCtrlLimits( + max_vel=2.0, max_acc=3.0, max_decel=3.0, max_steer=np.pi + ) + ) + + # Create the controller + config = VisionRGBFollowerConfig( + target_search_timeout=20.0, + speed_gain=0.8, + rotation_gain=0.9, + enable_search=True, + ) + controller = VisionRGBFollower(robot=my_robot, ctrl_limits=ctrl_limits, config=config) + + # 2D detection + detection = Bbox2D( + top_left_corner=np.array( + [30, 40], dtype=np.int32 + ), + size=np.array( + [ + 100, + 200, + ], + dtype=np.int32, + ), + timestamp=0.0, + label="person", + ) + detection.set_img_size(np.array([640, 480], dtype=np.int32)) + + # Set initial target + controller.set_initial_tracking_2d_target(detection) + + # Perform a control loop step + success = controller.loop_step(detections_2d=[detection]) + + # Access control outputs + vx = controller.linear_x_control + omega = controller.angular_control + ``` + """ + + def __init__( + self, + robot: Robot, + ctrl_limits: RobotCtrlLimits, + config: Optional[VisionRGBFollowerConfig] = None, + config_file: Optional[str] = None, + config_root_name: Optional[str] = None, + **_, + ): + """Init Vision RGB (Image) Follower Controller + + :param robot: Robot object to be controlled + :type robot: Robot + :param ctrl_limits: Robot control limits + :type ctrl_limits: RobotCtrlLimits + :param config: Controller configuration, defaults to None + :type config: Optional[VisionRGBDFollowerConfig], optional + :param config_file: Path to config file (yaml, json, toml), defaults to None + :type config_file: Optional[str], optional + :param config_root_name: Root name for the controller config in the file, defaults to None + :type config_root_name: Optional[str], optional + """ + self._config = config or VisionRGBFollowerConfig() + + if config_file: + self._config.from_file(config_file, config_root_name, get_common=False) + self.__controller = RGBFollower( + control_type=RobotType.to_kompass_cpp_lib(robot.robot_type), + control_limits=ctrl_limits.to_kompass_cpp_lib(), + config=self._config.to_kompass_cpp(), + ) + + self._found_ctrl = False + logging.info("VISION TARGET FOLLOWING CONTROLLER IS READY") + + def set_initial_tracking_2d_target(self, target_box: Bbox2D, **_) -> bool: + """Sets the initial target for the controller to track + + :param target_box: 2D bounding box of the target + :type target_box: Bbox2D + :return: True if the target was set successfully, False otherwise + :rtype: bool + """ + self.__controller.reset_target(target_box) + return True + + @property + def dist_error(self) -> float: + """Getter of the last distance error computed by the controller + + :return: Last distance error (m) + :rtype: float + """ + return self.__controller.get_errors()[0] + + @property + def orientation_error(self) -> float: + """Getter of the last orientation error computed by the controller (radians) + + :return: Last orientation error (radians) + :rtype: float + """ + return self.__controller.get_errors()[1] + + def loop_step( + self, + *, + detections_2d: Optional[List[Bbox2D]], + **_, + ) -> bool: + self._found_ctrl = self.__controller.run( + detections_2d[0] if detections_2d else None + ) + if self._found_ctrl: + self._ctrl = self.__controller.get_ctrl() + return self._found_ctrl + + def logging_info(self) -> str: + """ + Returns controller progress info for the Node to log + + :return: Controller Info + :rtype: str + """ + return f"Vision Object Follower found control: {self.linear_x_control}, {self.angular_control}" + + @property + def linear_x_control(self): + """ + Getter of the last linear forward velocity control computed by the controller + + :return: Linear Velocity Control (m/s) + :rtype: List[float] + """ + return self._ctrl.vx if self._found_ctrl else None + + @property + def linear_y_control(self): + """ + Getter the last linear velocity lateral control computed by the controller + + :return: Linear Velocity Control (m/s) + :rtype: List[float] + """ + return self._ctrl.vy if self._found_ctrl else None + + @property + def angular_control(self): + """ + Getter of the last angular velocity control computed by the controller + + :return: Angular Velocity Control (rad/s) + :rtype: List[float] + """ + return self._ctrl.omega if self._found_ctrl else None diff --git a/src/kompass_core/control/rgbd_follower.py b/src/kompass_core/control/rgbd_follower.py new file mode 100644 index 00000000..aac15d34 --- /dev/null +++ b/src/kompass_core/control/rgbd_follower.py @@ -0,0 +1,571 @@ +from attrs import define, field +from ..utils.common import base_validators +from kompass_cpp.control import ( + VisionDWA as VisionDWACpp, + VisionDWAParameters, + SamplingControlResult, +) +from kompass_cpp.types import ( + Bbox2D, + ControlCmd, + LaserScan, + TrajectoryVelocities2D, + TrajectoryPath, +) +from typing import Optional, List, Union +import numpy as np +import logging +from ._base_ import ControllerTemplate +from ..models import Robot, RobotState, RobotCtrlLimits, RobotGeometry, RobotType +from ..datatypes.laserscan import LaserScanData +from ..datatypes.pointcloud import PointCloudData +from .dwa import DWAConfig + + +@define +class VisionRGBDFollowerConfig(DWAConfig): + """ + Configuration class for a vision-based RGB-D target follower using Dynamic Window Approach (DWA) planning. + + Attributes: + control_time_step (float): Time interval between control updates (s). + control_horizon (int): Number of steps in the control horizon. + prediction_horizon (int): Number of steps in the prediction horizon. + buffer_size (int): Size of the trajectory buffer. + target_distance (Optional[float]): Desired distance to maintain from the target (m). + target_wait_timeout (float): Time to wait for the target to reappear before giving up (s). + target_search_timeout (float): Time limit for the search process if the target is lost (s). + target_search_pause (float): Pause between successive search attempts (s). + target_search_radius (float): Radius within which to search for the lost target (m). + rotation_gain (float): Gain applied to rotational control (unitless). + speed_gain (float): Gain applied to speed control (unitless). + enable_search (bool): Enables or disables the target search behavior. + distance_tolerance (float): Acceptable deviation in target distance (m). + angle_tolerance (float): Acceptable deviation in target bearing (rad). + target_orientation (float): Desired orientation relative to the target (rad). + use_local_coordinates (bool): Whether to use robot-local coordinates for tracking. + error_pose (float): Estimated error in pose measurements (m). + error_vel (float): Estimated error in velocity measurements (m/s). + error_acc (float): Estimated error in acceleration measurements (m/s²). + depth_conversion_factor (float): Factor to convert raw depth image values to meters. + min_depth (float): Minimum depth value considered valid (m). + max_depth (float): Maximum depth value considered valid (m). + camera_position_to_robot (np.ndarray): 3D translation vector from the camera frame to the robot base (m). + camera_rotation_to_robot (np.ndarray): Quaternion representing camera-to-robot rotation. + """ + control_time_step: float = field( + default=0.1, validator=base_validators.in_range(min_value=1e-4, max_value=1e6) + ) + control_horizon: int = field( + default=2, validator=base_validators.in_range(min_value=1, max_value=1000) + ) + prediction_horizon: int = field( + default=10, validator=base_validators.in_range(min_value=1, max_value=1000) + ) + buffer_size: int = field( + default=1, validator=base_validators.in_range(min_value=1, max_value=10) + ) + target_distance: Optional[float] = field(default=None) + target_wait_timeout: float = field( + default=30.0, validator=base_validators.in_range(min_value=0.0, max_value=1e3) + ) # wait for target to appear again timeout (seconds), used if search is disabled + target_search_timeout: float = field( + default=30.0, validator=base_validators.in_range(min_value=0.0, max_value=1e3) + ) # search timeout in seconds + target_search_pause: float = field( + default=2.0, validator=base_validators.in_range(min_value=0.0, max_value=1e3) + ) # pause between search actions to find target (seconds) + target_search_radius: float = field( + default=0.5, validator=base_validators.in_range(min_value=1e-4, max_value=1e4) + ) + rotation_gain: float = field( + default=1.0, validator=base_validators.in_range(min_value=1e-9, max_value=1.0) + ) + enable_search: bool = field(default=True) + distance_tolerance: float = field( + default=0.05, validator=base_validators.in_range(min_value=1e-6, max_value=1e3) + ) + angle_tolerance: float = field( + default=0.1, validator=base_validators.in_range(min_value=1e-6, max_value=1e3) + ) + # Tolerance value for distance and angle following errors + + target_orientation: float = field( + default=0.0, + validator=base_validators.in_range(min_value=-np.pi, max_value=np.pi), + ) # Bearing angle to maintain with the target (rad) + + rotation_gain: float = field( + default=0.5, validator=base_validators.in_range(min_value=1e-2, max_value=10.0) + ) # Gain for the rotation control law + + speed_gain: float = field( + default=1.0, validator=base_validators.in_range(min_value=1e-2, max_value=10.0) + ) # Gain for the speed control law + + use_local_coordinates: bool = field( + default=True + ) # Track the target using robot local coordinates (no need for robot location at lop step) + + error_pose: float = field( + default=0.05, validator=base_validators.in_range(min_value=1e-9, max_value=1e9) + ) # Error in pose estimation (m) + + error_vel: float = field( + default=0.05, validator=base_validators.in_range(min_value=1e-9, max_value=1e9) + ) # Error in velocity estimation (m/s) + + error_acc: float = field( + default=0.05, validator=base_validators.in_range(min_value=1e-9, max_value=1e9) + ) # Error in acceleration estimation (m/s^2) + + depth_conversion_factor: float = field( + default=1e-3, validator=base_validators.in_range(min_value=1e-9, max_value=1e9) + ) # Factor to convert depth image values to meters + + min_depth: float = field( + default=0.0, validator=base_validators.in_range(min_value=0.0, max_value=1e3) + ) # Range of interest minimum depth value (m) + + max_depth: float = field( + default=1e3, validator=base_validators.in_range(min_value=1e-3, max_value=1e9) + ) # Range of interest maximum depth value (m) + + camera_position_to_robot: np.ndarray = field( + default=np.array([0.0, 0.0, 0.0], dtype=np.float32) + ) + + camera_rotation_to_robot: np.ndarray = field( + default=np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32) + ) + + def to_kompass_cpp(self) -> VisionDWAParameters: + """ + Convert to kompass_cpp lib config format + + :return: _description_ + :rtype: kompass_cpp.control.VisionDWAParameters + """ + vision_dwa_params = VisionDWAParameters() + + # Special handling for None values that are represented by -1 in C++ + params_dict = self.asdict() + + if params_dict["target_distance"] is None: + params_dict["target_distance"] = -1.0 + + vision_dwa_params.from_dict(params_dict) + return vision_dwa_params + + +class VisionRGBDFollower(ControllerTemplate): + """ + VisionRGBDFollower is a controller for vision-based target tracking using RGB-D (color + depth) image data. + + This controller combines image-based detections (2D bounding boxes) and depth data to estimate 3D positions + of visual targets and uses a sampling-based planner (similar to DWA) to compute optimal local motion commands. + It integrates camera intrinsics, robot geometry, and multiple sensor modalities (e.g., point clouds, laser scans, + local maps) to generate robust and feasible trajectories for following dynamic or static targets in the environment. + + - Usage Example: + + ```python + import numpy as np + from kompass_core.control import VisionRGBDFollower, VisionRGBDFollowerConfig + from kompass_core.models import ( + Robot, + RobotType, + RobotCtrlLimits, + LinearCtrlLimits, + AngularCtrlLimits, + RobotGeometry, + RobotState, + ) + from kompass_core.datatypes import Bbox2D, LaserScanData + + # Define robot + my_robot = Robot( + robot_type=RobotType.ACKERMANN, + geometry_type=RobotGeometry.Type.CYLINDER, + geometry_params=np.array([0.3, 0.6]) + ) + + # Define control limits + ctrl_limits = RobotCtrlLimits( + vx_limits=LinearCtrlLimits(max_vel=1.5, max_acc=3.0, max_decel=3.0), + omega_limits=AngularCtrlLimits( + max_vel=2.5, max_acc=2.5, max_decel=2.5, max_steer=np.pi / 2 + ) + ) + + # Configure controller + config = VisionRGBDFollowerConfig( + max_linear_samples=15, + max_angular_samples=15, + control_horizon=10, + enable_obstacle_avoidance=True, + ) + controller = VisionRGBDFollower( + robot=my_robot, + ctrl_limits=ctrl_limits, + config=config, + camera_focal_length=[525.0, 525.0], + camera_principal_point=[319.5, 239.5], + ) + + # Prepare sensor inputs + bbox = Bbox2D(top_left_corner=np.array([200, 150]), size=np.array([50, 100])) + bbox.set_img_size(np.array([640, 480])) + + aligned_depth_image = np.random.rand(480, 640).astype(np.int32) # Fake depth + robot_state = RobotState(x=0.0, y=0.0, yaw=0.0, speed=0.0) + + # Initialize target tracking + controller.set_initial_tracking_2d_target( + current_state=robot_state, + target_box=bbox, + aligned_depth_image=aligned_depth_image, + ) + + # Run control loop step + success = controller.loop_step( + current_state=robot_state, + detections_2d=[bbox], + depth_image=aligned_depth_image, + local_map=np.random.rand(100, 100), # Fake local map + local_map_resolution=0.05 + ) + + # Access control outputs + vx_cmd = controller.linear_x_control + omega_cmd = controller.angular_control + ``` + """ + def __init__( + self, + robot: Robot, + ctrl_limits: RobotCtrlLimits, + config: Optional[VisionRGBDFollowerConfig] = None, + config_file: Optional[str] = None, + config_root_name: Optional[str] = None, + control_time_step: Optional[float] = None, + camera_focal_length: Optional[List[float]] = None, + camera_principal_point: Optional[List[float]] = None, + **_, + ): + """Init Vision RGBD (Depth) Follower Controller + + :param robot: Robot object to be controlled + :type robot: Robot + :param ctrl_limits: Robot control limits + :type ctrl_limits: RobotCtrlLimits + :param config: Controller configuration, defaults to None + :type config: Optional[VisionRGBDFollowerConfig], optional + :param config_file: Path to config file (yaml, json, toml), defaults to None + :type config_file: Optional[str], optional + :param config_root_name: Root name for the controller config in the file, defaults to None + :type config_root_name: Optional[str], optional + :param control_time_step: Control time step (sec), defaults to None + :type control_time_step: Optional[float], optional + :param camera_focal_length: Depth camera focal length, defaults to None + :type camera_focal_length: Optional[List[float]], optional + :param camera_principal_point: Depth camera principal point, defaults to None + :type camera_principal_point: Optional[List[float]], optional + """ + self._config = config or VisionRGBDFollowerConfig() + + if config_file: + self._config.from_file( + file_path=config_file, nested_root_name=config_root_name + ) + + if control_time_step: + self._config.control_time_step = control_time_step + + self._planner = VisionDWACpp( + control_type=RobotType.to_kompass_cpp_lib(robot.robot_type), + control_limits=ctrl_limits.to_kompass_cpp_lib(), + max_linear_samples=self._config.max_linear_samples, + max_angular_samples=self._config.max_angular_samples, + robot_shape_type=RobotGeometry.Type.to_kompass_cpp_lib(robot.geometry_type), + robot_dimensions=robot.geometry_params, + proximity_sensor_position_wrt_body=self._config.proximity_sensor_position_to_robot, + proximity_sensor_rotation_wrt_body=self._config.proximity_sensor_rotation_to_robot, + vision_sensor_position_wrt_body=self._config.camera_position_to_robot, + vision_sensor_rotation_wrt_body=self._config.camera_rotation_to_robot, + octree_res=self._config.octree_resolution, + cost_weights=self._config.costs_weights.to_kompass_cpp(), + max_num_threads=self._config.max_num_threads, + config=self._config.to_kompass_cpp(), + ) + if camera_focal_length is not None and camera_principal_point is not None: + self._planner.set_camera_intrinsics( + camera_focal_length[0], + camera_focal_length[1], + camera_principal_point[0], + camera_principal_point[1], + ) + + # Init the following result + self._result = SamplingControlResult() + self._end_of_ctrl_horizon: int = max(self._config.control_horizon, 1) + logging.info("VisionDWA CONTROLLER IS READY") + + def set_camera_intrinsics(self, fx: float, fy: float, cx: float, cy: float) -> None: + """Set depth camera intrinsics for the planner + + :param fx: Focal length in x direction + :type fx: float + :param fy: Focal length in y direction + :type fy: float + :param cx: Principal point x coordinate + :type cx: float + :param cy: Principal point y coordinate + :type cy: float + """ + self._planner.set_camera_intrinsics(fx, fy, cx, cy) + + def set_initial_tracking_2d_target( + self, + current_state: RobotState, + target_box: Bbox2D, + aligned_depth_image: np.ndarray, + ) -> bool: + """ + Set initial tracking state + + :param detected_boxes: Detected boxes + :type detected_boxes: List[Bbox3D] + """ + try: + if current_state: + self._planner.set_current_state( + current_state.x, + current_state.y, + current_state.yaw, + current_state.speed, + ) + return self._planner.set_initial_tracking( + aligned_depth_image, + target_box, + current_state.yaw, + ) + + except Exception as e: + logging.error(f"Could not set initial tracking state: {e}") + return False + + @property + def dist_error(self) -> float: + """Getter of the last distance error computed by the controller + + :return: Last distance error (m) + :rtype: float + """ + return self._planner.get_errors()[0] + + @property + def orientation_error(self) -> float: + """Getter of the last orientation error computed by the controller (radians) + + :return: Last orientation error (radians) + :rtype: float + """ + return self._planner.get_errors()[1] + + def set_initial_tracking_image( + self, + current_state: RobotState, + pose_x_img: int, + pose_y_img: int, + detected_boxes: List[Bbox2D], + aligned_depth_image: np.ndarray, + ) -> bool: + """ + Set initial tracking state + + :param detected_boxes: Detected boxes + :type detected_boxes: List[Bbox3D] + """ + try: + if self._config.use_local_coordinates: + self._planner.set_current_state( + current_state.x, + current_state.y, + current_state.yaw, + current_state.speed, + ) + if any(detected_boxes): + return self._planner.set_initial_tracking( + pose_x_img, + pose_y_img, + aligned_depth_image, + detected_boxes, + current_state.yaw, + ) + logging.error( + "Could not set initial tracking state: No detections are provided" + ) + return False + + except Exception as e: + logging.error(f"Could not set initial tracking state: {e}") + return False + + def loop_step( + self, + *, + current_state: Optional[RobotState] = None, + detections_2d: Optional[List[Bbox2D]] = None, + depth_image: Optional[np.ndarray] = None, + laser_scan: Optional[LaserScanData] = None, + point_cloud: Optional[List[np.ndarray]] = None, + local_map: Optional[np.ndarray] = None, + local_map_resolution: Optional[float] = None, + **_, + ) -> bool: + """ + One iteration of the DWA planner + + :param current_state: Current robot state (position and velocity) + :type current_state: RobotState + :param laser_scan: Current laser scan value + :type laser_scan: LaserScanData + + :return: If planner found a valid solution + :rtype: bool + """ + robot_cmd = None + if self._config.use_local_coordinates and current_state is not None: + self._planner.set_current_state( + current_state.x, current_state.y, current_state.yaw, current_state.speed + ) + robot_cmd = ControlCmd( + vx=current_state.vx, vy=current_state.vy, omega=current_state.omega + ) + + if local_map_resolution: + self._planner.set_resolution(local_map_resolution) + + if local_map is not None: + sensor_data = PointCloudData.numpy_to_kompass_cpp(local_map) + elif laser_scan: + sensor_data = LaserScan(ranges=laser_scan.ranges, angles=laser_scan.angles) + elif point_cloud is not None: + sensor_data = point_cloud + else: + logging.error( + "Cannot compute control without sensor data. Provide 'laser_scan' or 'point_cloud' input" + ) + return False + + try: + self._result = self._planner.get_tracking_ctrl( + depth_image, detections_2d, robot_cmd or self._last_cmd, sensor_data + ) + + except Exception as e: + logging.error(f"Could not find velocity command: {e}") + return False + + return self._result.is_found + + def has_result(self) -> None: + """ + Set global path to be tracked by the planner + + :param global_path: Global reference path + :type global_path: Path + """ + return self._result.is_found + + def logging_info(self) -> str: + """logging_info.""" + if self._result.is_found: + return ( + f"VisionDWA Controller found trajectory with cost: {self._result.cost}" + ) + else: + return "VisionDWA Controller Failed to find a valid trajectory" + + @property + def control_till_horizon( + self, + ) -> Optional[TrajectoryVelocities2D]: + """ + Getter of the planner control result until the control horizon + + :return: Velocity commands of the minimal cost path + :rtype: List[kompass_cpp.types.TrajectoryVelocities2D] + """ + if self._result.is_found: + return self._result.trajectory.velocities + return None + + def optimal_path(self) -> Optional[TrajectoryPath]: + """Get optimal (local) plan.""" + if not self._result.is_found: + return None + return self._result.trajectory.path + + @property + def result_cost(self) -> Optional[float]: + """ + Getter of the planner optimal path + + :return: Path found with the least cost + :rtype: Optional[float] + """ + if self._result.is_found: + return self._result.cost + return None + + @property + def linear_x_control(self) -> Union[List[float], np.ndarray]: + """ + Getter of the last linear forward velocity control computed by the controller + + :return: Linear Velocity Control (m/s) + :rtype: List[float] + """ + if self._result.is_found: + return self.control_till_horizon.vx[: self._end_of_ctrl_horizon] + return [0.0] + + @property + def linear_y_control(self) -> Union[List[float], np.ndarray]: + """ + Getter the last linear velocity lateral control computed by the controller + + :return: Linear Velocity Control (m/s) + :rtype: List[float] + """ + if self._result.is_found: + return self.control_till_horizon.vy[: self._end_of_ctrl_horizon] + return [0.0] + + @property + def angular_control(self) -> Union[List[float], np.ndarray]: + """ + Getter of the last angular velocity control computed by the controller + + :return: Angular Velocity Control (rad/s) + :rtype: List[float] + """ + if self._result.is_found: + return self.control_till_horizon.omega[: self._end_of_ctrl_horizon] + return [0.0] + + @property + def _last_cmd(self) -> ControlCmd: + """ + Getter of the last command sent to the controller + + :return: Last command sent to the controller + :rtype: ControlCmd + """ + return ControlCmd( + vx=self.linear_x_control[-1], + vy=self.linear_y_control[-1], + omega=self.angular_control[-1], + ) diff --git a/src/kompass_core/control/stanley.py b/src/kompass_core/control/stanley.py index 451d375b..3dd752ad 100644 --- a/src/kompass_core/control/stanley.py +++ b/src/kompass_core/control/stanley.py @@ -110,17 +110,24 @@ def __init__( ctrl_limits: RobotCtrlLimits, config: Optional[StanleyConfig] = None, config_file: Optional[str] = None, - config_yaml_root_name: Optional[str] = None, + config_root_name: Optional[str] = None, generate_reference: bool = False, **_, ): - """ - Setup the controller + """_summary_ - :param robot: Robot using the controller + :param robot: Robot object to be controlled :type robot: Robot - :param params_file: Yaml file containing the parameters of the controller under 'dvz_controller' - :type params_file: str + :param ctrl_limits: Robot control limits + :type ctrl_limits: RobotCtrlLimits + :param config: Controller configuration, defaults to None + :type config: Optional[VisionRGBDFollowerConfig], optional + :param config_file: Path to config file (yaml, json, toml), defaults to None + :type config_file: Optional[str], optional + :param config_root_name: Root name for the controller config in the file, defaults to None + :type config_root_name: Optional[str], optional + :param generate_reference: Use to generate reference commands, defaults to False + :type generate_reference: bool, optional """ self.__generate_reference = generate_reference self._robot = robot @@ -130,9 +137,7 @@ def __init__( config = StanleyConfig(wheel_base=robot.wheelbase) if config_file: - config.from_yaml( - file_path=config_file, nested_root_name=config_yaml_root_name - ) + config.from_file(file_path=config_file, nested_root_name=config_root_name) self._config = config self._control_time_step = config.control_time_step diff --git a/src/kompass_core/control/vision_follower.py b/src/kompass_core/control/vision_follower.py deleted file mode 100644 index c32cd755..00000000 --- a/src/kompass_core/control/vision_follower.py +++ /dev/null @@ -1,136 +0,0 @@ -from ._base_ import ControllerTemplate -import logging -from typing import Optional -from attrs import define, field -from ..utils.common import BaseAttrs, base_validators -from ..models import Robot, RobotCtrlLimits, RobotType -from ..datatypes import TrackingData -import kompass_cpp - - -@define -class VisionFollowerConfig(BaseAttrs): - control_time_step: float = field( - default=0.1, validator=base_validators.in_range(min_value=1e-4, max_value=1e6) - ) - control_horizon: int = field( - default=2, validator=base_validators.in_range(min_value=1, max_value=1000) - ) - buffer_size: int = field( - default=3, validator=base_validators.in_range(min_value=1, max_value=10) - ) - tolerance: float = field( - default=0.1, validator=base_validators.in_range(min_value=1e-6, max_value=1e3) - ) - target_distance: Optional[float] = field(default=None) - target_wait_timeout: float = field( - default=30.0, validator=base_validators.in_range(min_value=0.0, max_value=1e3) - ) # wait for target to appear again timeout (seconds), used if search is disabled - target_search_timeout: float = field( - default=30.0, validator=base_validators.in_range(min_value=0.0, max_value=1e3) - ) # search timeout in seconds - target_search_pause: float = field( - default=2.0, validator=base_validators.in_range(min_value=0.0, max_value=1e3) - ) # pause between search actions to find target (seconds) - target_search_radius: float = field( - default=0.5, validator=base_validators.in_range(min_value=1e-4, max_value=1e4) - ) - rotation_multiple: float = field( - default=1.0, validator=base_validators.in_range(min_value=1e-9, max_value=1.0) - ) - speed_depth_multiple: float = field( - default=0.7, validator=base_validators.in_range(min_value=1e-9, max_value=10.0) - ) - min_vel: float = field( - default=0.1, validator=base_validators.in_range(min_value=1e-9, max_value=1e9) - ) - enable_search: bool = field(default=True) - - def to_kompass_cpp(self) -> kompass_cpp.control.VisionFollowerParameters: - """ - Convert to kompass_cpp lib config format - - :return: _description_ - :rtype: kompass_cpp.control.VisionFollowerParameters - """ - vision_config = kompass_cpp.control.VisionFollowerParameters() - vision_config.from_dict(self.asdict()) - return vision_config - - -class VisionFollower(ControllerTemplate): - def __init__( - self, - robot: Robot, - ctrl_limits: RobotCtrlLimits, - config: Optional[VisionFollowerConfig] = None, - config_file: Optional[str] = None, - config_yaml_root_name: Optional[str] = None, - **_, - ): - config = config or VisionFollowerConfig() - - if config_file: - config.from_yaml(config_file, config_yaml_root_name, get_common=False) - self.__controller = kompass_cpp.control.VisionFollower( - control_type=RobotType.to_kompass_cpp_lib(robot.robot_type), - control_limits=ctrl_limits.to_kompass_cpp_lib(), - config=config.to_kompass_cpp(), - ) - - self._found_ctrl = False - logging.info("VISION TARGET FOLLOWING CONTROLLER IS READY") - - def reset_target(self, tracking: TrackingData): - self.__controller.reset_target(tracking.to_kompass_cpp()) - - def loop_step( - self, - *, - tracking: TrackingData, - **_, - ) -> bool: - tracking_cpp = tracking.to_kompass_cpp() if tracking else None - self._found_ctrl = self.__controller.run(tracking_cpp) - if self._found_ctrl: - self._ctrl = self.__controller.get_ctrl() - return self._found_ctrl - - def logging_info(self) -> str: - """ - Returns controller progress info for the Node to log - - :return: Controller Info - :rtype: str - """ - return f"Vision Object Follower found control: {self.linear_x_control}, {self.angular_control}" - - @property - def linear_x_control(self): - """ - Getter of the last linear forward velocity control computed by the controller - - :return: Linear Velocity Control (m/s) - :rtype: List[float] - """ - return self._ctrl.vx if self._found_ctrl else None - - @property - def linear_y_control(self): - """ - Getter the last linear velocity lateral control computed by the controller - - :return: Linear Velocity Control (m/s) - :rtype: List[float] - """ - return self._ctrl.vy if self._found_ctrl else None - - @property - def angular_control(self): - """ - Getter of the last angular velocity control computed by the controller - - :return: Angular Velocity Control (rad/s) - :rtype: List[float] - """ - return self._ctrl.omega if self._found_ctrl else None diff --git a/src/kompass_core/datatypes/__init__.py b/src/kompass_core/datatypes/__init__.py index 717457ba..1ae6f16f 100644 --- a/src/kompass_core/datatypes/__init__.py +++ b/src/kompass_core/datatypes/__init__.py @@ -14,7 +14,7 @@ ) from .pointcloud import PointCloudData from .pose import PoseData -from .vision import TrackingData, ImageMetaData +from kompass_cpp.types import Bbox3D, Bbox2D __all__ = [ "LaserScanData", @@ -31,6 +31,6 @@ "Odom2D", "PointCloudData", "PoseData", - "TrackingData", - "ImageMetaData", + "Bbox3D", + "Bbox2D", ] diff --git a/src/kompass_core/datatypes/path.py b/src/kompass_core/datatypes/path.py index 93952098..56e1aa1a 100644 --- a/src/kompass_core/datatypes/path.py +++ b/src/kompass_core/datatypes/path.py @@ -3,7 +3,6 @@ from typing import List, Union import numpy as np -import pandas as pd from attrs import define, field @@ -428,7 +427,12 @@ def save_to_csv(self, file_location: str, file_name: str) -> bool: """ try: csv_mapping = self._csv_mapping() - + try: + import pandas as pd + except ImportError as e: + raise ImportError( + "Pandas is required for data handling. Please install it using 'pip install pandas'." + ) from e # Create a DataFrame using pandas motion_df = pd.DataFrame(csv_mapping) # Check if the directory exists, if not, create it @@ -467,6 +471,12 @@ def get_from_csv(self, file_location: str, file_name: str) -> bool: :return: Motion sample loaded from file :rtype: bool """ + try: + import pandas as pd + except ImportError as e: + raise ImportError( + "Pandas is required for data handling. Please install it using 'pip install pandas'." + ) from e if os.path.exists(file_location): _, extension = os.path.splitext(file_name) diff --git a/src/kompass_core/datatypes/pose.py b/src/kompass_core/datatypes/pose.py index 7f44fe5c..c4e3a03b 100644 --- a/src/kompass_core/datatypes/pose.py +++ b/src/kompass_core/datatypes/pose.py @@ -1,35 +1,30 @@ from typing import Union import numpy as np -import quaternion -from quaternion import quaternion as quat def _equal_approx( - v1, v2, is_quaternion=False, absolute_tolerance=0.01 + v1: np.ndarray, + v2: np.ndarray, + is_quaternion: bool = False, + absolute_tolerance: float = 0.01, ) -> Union[bool, np.bool_]: """ - check if two vector/quaternion arrays are approximately equals - to within absolute_tolerance - - :param v1: vector 1 - :type v1: numpy/ numpy-quaternion - :param v2: vector 2 - :type v2: vector numpy / numpy-quaternion - :param is_quaternion: is it quaternion check?, defaults to False - :type is_quaternion: bool, optional - :param absolute_tolerance: tolerate some difference if exist, defaults to 0.01 - :type absolute_tolerance: float, optional - - :return: is both vectors are equal? - :rtype: bool + Check if two vectors or quaternions are approximately equal within a tolerance. + + :param v1: First vector or quaternion [w, x, y, z] + :param v2: Second vector or quaternion + :param is_quaternion: Set to True if comparing quaternions + :param absolute_tolerance: Absolute tolerance for comparison + :return: True if approximately equal """ if is_quaternion: - equal = quaternion.allclose(a=v1, b=v2, rtol=0.0, atol=absolute_tolerance) + # q and -q represent the same rotation + return np.allclose(v1, v2, rtol=0.0, atol=absolute_tolerance) or np.allclose( + v1, -v2, rtol=0.0, atol=absolute_tolerance + ) else: - equal = np.allclose(a=v1, b=v2, rtol=0.0, atol=absolute_tolerance) - - return equal + return np.allclose(v1, v2, rtol=0.0, atol=absolute_tolerance) class PoseData: @@ -134,14 +129,14 @@ def get_position(self) -> np.ndarray: """ return np.array([self.x, self.y, self.z], dtype=np.float32) - def get_orientation(self) -> quaternion.quaternion: + def get_orientation(self) -> np.ndarray: """ Get the orientation represented as quaternion :return: orientation represented as quaternion - :rtype: quaternion.quaternion + :rtype: np.ndarray """ - return quat(self.qw, self.qx, self.qy, self.qz) + return np.array([self.qw, self.qx, self.qy, self.qz]) def get_yaw(self) -> np.float64: """ diff --git a/src/kompass_core/datatypes/vision.py b/src/kompass_core/datatypes/vision.py deleted file mode 100644 index ed97601d..00000000 --- a/src/kompass_core/datatypes/vision.py +++ /dev/null @@ -1,46 +0,0 @@ -from ..utils.common import BaseAttrs -from attrs import define, field -from typing import Optional, List, Union -import kompass_cpp - - -def _xy_validator(_, attribute, value): - if len(value) != 2: - raise ValueError(f"Attribute {attribute} should be a list of length 2: [x, y]") # noqa: F821 - return - - -def _xy_optional_validator(_, attribute, value): - if value and len(value) != 2: - raise ValueError( - f"Attribute {attribute} should be None or a list of length 2: [x, y]" - ) # noqa: F821 - return - - -@define -class ImageMetaData(BaseAttrs): - frame_id: str = field() - width: int = field() - height: int = field() - encoding: str = field(default="rgb8") - - -@define -class TrackingData(BaseAttrs): - label: str = field() - center_xy: List = field(validator=_xy_validator) - size_xy: List = field(validator=_xy_validator) - id: int = field() - img_meta: Union[ImageMetaData, None] = field() - velocity_xy: Optional[List] = field(default=None, validator=_xy_optional_validator) - depth: Optional[float] = field(default=None) - - def to_kompass_cpp(self): - return kompass_cpp.types.TrackingData( - size_xy=self.size_xy, - center_xy=self.center_xy, - img_width=self.img_meta.width, - img_height=self.img_meta.height, - depth=self.depth or -1.0, - ) diff --git a/src/kompass_core/mapping/local_mapper.py b/src/kompass_core/mapping/local_mapper.py index 938e04b8..adc83eee 100644 --- a/src/kompass_core/mapping/local_mapper.py +++ b/src/kompass_core/mapping/local_mapper.py @@ -9,7 +9,7 @@ OCCUPANCY_TYPE, ) -from ..utils.geometry import from_frame1_to_frame2, get_pose_target_in_reference_frame +from ..utils.geometry import transform_point_from_local_to_global, get_relative_pose from .laserscan_model import LaserScanModelConfig from ..utils.common import BaseAttrs, base_validators @@ -221,8 +221,8 @@ def _calculate_grid_shift(self, current_robot_pose: PoseData): # i.e. we have a t+1 state # get current shift in translation and orientation of the new center # with respect to the previous old center - pose_current_robot_in_previous_robot = get_pose_target_in_reference_frame( - reference_pose=self._pose_robot_in_world, target_pose=current_robot_pose + pose_current_robot_in_previous_robot = get_relative_pose( + pose_1_in_ref=self._pose_robot_in_world, pose_2_in_ref=current_robot_pose ) # new position and orientation with respect to the previous pose _position_in_previous_pose = pose_current_robot_in_previous_robot.get_position() @@ -264,16 +264,18 @@ def update_from_scan( # Calculate new grid pose self._pose_robot_in_world = robot_pose - self.lower_right_corner_pose = from_frame1_to_frame2( - robot_pose, self._local_lower_right_corner_point + self.lower_right_corner_pose = transform_point_from_local_to_global( + self._local_lower_right_corner_point, robot_pose ) if self.config.baysian_update: if self.processed: self._calculate_grid_shift(robot_pose) - scan_occupancy, scan_occupancy_prob = self.local_mapper.scan_to_grid_baysian( - angles=laser_scan.angles, - ranges=filtered_ranges, + scan_occupancy, scan_occupancy_prob = ( + self.local_mapper.scan_to_grid_baysian( + angles=laser_scan.angles, + ranges=filtered_ranges, + ) ) # Update grid diff --git a/src/kompass_core/models.py b/src/kompass_core/models.py index 9fcd5bd6..86205baf 100644 --- a/src/kompass_core/models.py +++ b/src/kompass_core/models.py @@ -1,7 +1,6 @@ from enum import Enum from typing import List, Optional, Union -from .utils import common as CommonUtils from .utils.common import BaseAttrs, base_validators, set_params_from_yaml from .utils import geometry as GeometryUtils @@ -9,9 +8,6 @@ from attrs import Factory, define, field, validators from .datatypes.path import Point2D -import matplotlib.pyplot as plt -from matplotlib.patches import Circle, Rectangle - import kompass_cpp @@ -175,14 +171,14 @@ def apply( ) return output_state - def set_params_from_yaml(self, path_to_file: str) -> None: + def set_params_from_file(self, path_to_file: str) -> None: """ - Sets the robot testing parameters values from a given yaml file under 'robot' + Sets the robot testing parameters values from a given config file under 'robot' - :param path_to_file: Path to YAML file + :param path_to_file: Path to file (yaml, json, toml) :type path_to_file: str """ - self.params.from_yaml(path_to_file) + self.params.from_file(path_to_file) def set_linear_x_params(self, params: List[float]) -> None: """ @@ -490,6 +486,14 @@ def plt_robot( :param ax: Plot figure axis, defaults to None :type ax: _type_, optional """ + try: + import matplotlib.pyplot as plt + from matplotlib.patches import Circle + except ImportError as e: + raise ImportError( + "Matplotlib is required for plotting robot footprints. " + "Please install it using 'pip install matplotlib'." + ) from e if not ax: ax = plt.gca() @@ -538,7 +542,7 @@ class RectangleFootprint: def __init__(self, width=1.0, length=2.0): """ - Inits a regtangular robot footprint + Inits a rectangular robot footprint :param width: Robot width (m), defaults to 2.0 :type width: float, optional @@ -582,6 +586,14 @@ def plt_robot( :param ax: Plot figure axis, defaults to None :type ax: _type_, optional """ + try: + import matplotlib.pyplot as plt + from matplotlib.patches import Circle, Rectangle + except ImportError as e: + raise ImportError( + "Matplotlib is required for plotting robot footprints. " + "Please install it using 'pip install matplotlib'." + ) from e if not ax: ax = plt.gca() @@ -788,7 +800,7 @@ def get_radius(cls, geometry_type: Type, parameters: np.ndarray) -> float: cls.Type.SPHERE, cls.Type.CAPSULE, ]: - # First parameter is the radius -> equivilant to wheelbase + # First parameter is the radius -> equivalent to wheelbase return parameters[0] else: return np.sqrt(parameters[1] + parameters[0]) / 2 @@ -831,7 +843,7 @@ def get_footprint( cls.Type.SPHERE, cls.Type.CAPSULE, ]: - # First parameter is the radius -> equivilant to wheelbase + # First parameter is the radius -> equivalent to wheelbase return CircularFootprint(rad=parameters[0]) else: # Wheelbase is the distance on robot lateral axis (y-axis) @@ -1096,9 +1108,9 @@ def init_zero(cls, **kwargs): class RobotType(Enum): """RobotType.""" - ACKERMANN = "ACKERMANN_ROBOT" - DIFFERENTIAL_DRIVE = "DIFFERENTIAL_DRIVE_ROBOT" - OMNI = "OMNI_ROBOT" + ACKERMANN = "ACKERMANN" + DIFFERENTIAL_DRIVE = "DIFFERENTIAL_DRIVE" + OMNI = "OMNI" @classmethod def values(cls) -> List[str]: @@ -1136,17 +1148,17 @@ def to_kompass_cpp_lib(cls, value: str) -> kompass_cpp.control.ControlType: :return: Robot Type :rtype: kompass_cpp.control_types """ - if value == "ACKERMANN_ROBOT": + if value == "ACKERMANN": return kompass_cpp.control.ControlType.ACKERMANN - if value == "DIFFERENTIAL_DRIVE_ROBOT": + if value == "DIFFERENTIAL_DRIVE": return kompass_cpp.control.ControlType.DIFFERENTIAL_DRIVE return kompass_cpp.control.ControlType.OMNI control_types = { - "ACKERMANN_ROBOT": AckermannControl, - "DIFFERENTIAL_DRIVE_ROBOT": DifferentialDriveControl, - "OMNI_ROBOT": OmniDirectionalControl, + "ACKERMANN": AckermannControl, + "DIFFERENTIAL_DRIVE": DifferentialDriveControl, + "OMNI": OmniDirectionalControl, } @@ -1163,6 +1175,7 @@ class LinearCtrlLimits(BaseAttrs): max_vel: float = field(validator=validators.ge(0.0)) # m/s max_acc: float = field(validator=validators.ge(0.0)) # m/s^2 max_decel: float = field(validator=validators.ge(0.0)) # m/s^2 + min_absolute_val: float = field(default=0.01, validator=validators.ge(0.0)) # m/s @define(kw_only=True) @@ -1173,6 +1186,7 @@ class AngularCtrlLimits(BaseAttrs): max_steer: float = field(validator=validators.ge(0.0)) max_acc: float = field(validator=validators.ge(0.0)) max_decel: float = field(validator=validators.ge(0.0)) + min_absolute_val: float = field(default=0.01, validator=validators.ge(0.0)) # m/s @define(kw_only=True) @@ -1189,7 +1203,7 @@ class RobotCtrlLimits(BaseAttrs): def to_kompass_cpp_lib(self) -> kompass_cpp.control.ControlLimitsParams: """ - Get the control limits parameters transferred to Robotctrl library format + Get the control limits parameters transferred to Kompass_cpp library format :return: 2D control limits :rtype: kompass_cpp.control.ctr_limits_params @@ -1204,7 +1218,7 @@ def linear_to_kompass_cpp_lib( self, linear_limits: LinearCtrlLimits ) -> kompass_cpp.control.LinearVelocityControlParams: """ - Get linear velocity control limits parameters transfered to Robotctrl library format + Get linear velocity control limits parameters transferred to Kompass_cpp library format :return: Linear forward velocity Vx parameters :rtype: kompass_cpp.control.linear_vel_x_params @@ -1219,7 +1233,7 @@ def angular_to_kompass_cpp_lib( self, ) -> kompass_cpp.control.AngularVelocityControlParams: """ - Get Omega control limits parameters transfered to Robotctrl library format + Get Omega control limits parameters transferred to Kompass_cpp library format :return: Angular velocity Omega parameters :rtype: kompass_cpp.control.angular_vel_params diff --git a/src/kompass_core/performance.py b/src/kompass_core/performance.py index 06b4b6f0..d30b9df1 100644 --- a/src/kompass_core/performance.py +++ b/src/kompass_core/performance.py @@ -1,8 +1,6 @@ -from typing import Any, List +from typing import Any, List, Optional -import matplotlib.pyplot as plt import numpy as np -import pandas as pd from .datatypes.obstacles import ObstaclesData from .datatypes.path import ( PathPoint, @@ -20,7 +18,7 @@ class MotionResult: def __init__(self) -> None: self.time = [] self.steer_cmds: List[float] = [] - self.robot_path: PathSample = None + self.robot_path: Optional[PathSample] = None self.speed_cmd: List[float] = [] self.ori_error: List[float] = [] self.lat_error: List[float] = [] @@ -44,6 +42,12 @@ def vis_result( :param figure_title: Title of the generated figure, defaults to 'Figure 0' :type figure_title: str, optional """ + try: + import matplotlib.pyplot as plt + except ImportError as e: + raise ImportError( + "Matplotlib is required for visualization. Please install it using 'pip install matplotlib'." + ) from e _fig_margin = 0.5 fig, [ax0, ax1, ax2, ax3] = plt.subplots(nrows=4, ncols=1, figsize=(8, 8)) @@ -89,7 +93,7 @@ def vis_result( ) # Plot test path - visualization.plt_path_points_List(test, color="red", ax=ax3) + visualization.plt_path_points_list(test, color="red", ax=ax3) # Plot robot initial state robot_footprint.plt_robot( @@ -130,6 +134,12 @@ def reset(self): class TestAvgResults: def __init__(self) -> None: + try: + import pandas as pd + except ImportError as e: + raise ImportError( + "Pandas is required for data handling. Please install it using 'pip install pandas'." + ) from e columns = [ "test_id", "test_type", diff --git a/src/kompass_core/py_path_tools/executor.py b/src/kompass_core/py_path_tools/executor.py index 7321d5d2..9403002a 100644 --- a/src/kompass_core/py_path_tools/executor.py +++ b/src/kompass_core/py_path_tools/executor.py @@ -103,14 +103,14 @@ def __init__(self, params: Optional[PathExecutorParams] = None): def configure(self, config_file: str, nested_root_name: Optional[str] = None): """ - Configure the executor using a yaml file + Configure the executor using a configuration file (yaml, json, toml) - :param config_file: _description_ + :param config_file: Path to file with configuration parameters :type config_file: str :param nested_root_name: _description_, defaults to None :type nested_root_name: Optional[str], optional """ - self.params.from_yaml(config_file, nested_root_name) + self.params.from_file(config_file, nested_root_name) def record_path_point(self, x: float, y: float, heading: float, vel: float) -> bool: """ diff --git a/src/kompass_core/py_path_tools/interpolation.py b/src/kompass_core/py_path_tools/interpolation.py index 50c585a4..c332a31e 100644 --- a/src/kompass_core/py_path_tools/interpolation.py +++ b/src/kompass_core/py_path_tools/interpolation.py @@ -5,11 +5,17 @@ from ..utils import geometry as GeometryUtils import numpy as np from ..datatypes.path import InterpolationPoint, Point2D, Range2D, TrackedPoint -from scipy.interpolate import CubicSpline class Spline: def __init__(self): + try: + from scipy.interpolate import CubicSpline + except ImportError as e: + raise ImportError( + "CubicSpline from scipy is required for spline interpolation. " + "Please install it using 'pip install scipy'." + ) from e self.x_points: np.ndarray = np.array([]) self.y_points: np.ndarray = np.array([]) self.func: Optional[CubicSpline] = None @@ -23,6 +29,13 @@ def set_points(self, x_points: np.ndarray, y_points: np.ndarray) -> None: :param y_points: Y coordinates of the spline points :type y_points: np.ndarray """ + try: + from scipy.interpolate import CubicSpline + except ImportError as e: + raise ImportError( + "CubicSpline from scipy is required for spline interpolation. " + "Please install it using 'pip install scipy'." + ) from e # arrange x points in increasing order sorted_indices = np.argsort(x_points) self.x_points = np.array(x_points[sorted_indices]) diff --git a/src/kompass_core/third_party/fcl/__init__.py b/src/kompass_core/third_party/fcl/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/src/kompass_core/third_party/fcl/collisions.py b/src/kompass_core/third_party/fcl/collisions.py deleted file mode 100644 index b6ef0593..00000000 --- a/src/kompass_core/third_party/fcl/collisions.py +++ /dev/null @@ -1,115 +0,0 @@ -from typing import Optional - -import fcl -import numpy as np - -from ...models import RobotState - -from .config import FCLConfig, fcl_object_geometry - - -class FCL: - def __init__( - self, config: Optional[FCLConfig] = None, map_3d: Optional[np.ndarray] = None - ) -> None: - """ - Setup a handler using the Flexible Collision Library (FCL) - - :param config: FCL configuration parameters - :type config: FCLConfig - - :param map_3d: 3D array for map PointCloud data, defaults to None - :type map_3d: np.ndarray | None, optional - - :raises Exception: FCL exceptions - """ - try: - if config: - self._config = config - else: - self._config = FCLConfig() - - self._setup_from_config() - - if map_3d is not None: - self.set_map(map_3d) - except Exception as e: - raise Exception(f"FCL geometry setup error: {e}") from e - - def _setup_from_config(self): - """ - Setup fcl from provided config - """ - self._robot_geometry = fcl_object_geometry[self._config.robot_geometry_type]( - *tuple(self._config.robot_geometry_params) - ) - self._map_resolution = self._config.map_resolution - - self._collision_manager = fcl.DynamicAABBTreeCollisionManager() - - self.got_map = False - - def configure(self, yaml_file: str, root_name: Optional[str] = None): - """ - Load configuration from yaml - - :param yaml_file: Path to config file (.yaml) - :type yaml_file: str - :param root_name: Parent root name of the config in the file 'Parent.fcl' - config must be under 'fcl', defaults to None - :type root_name: str | None, optional - """ - if root_name: - nested_root_name = root_name + ".fcl" - else: - nested_root_name = "fcl" - self._config.from_yaml(yaml_file, nested_root_name) - self._setup_from_config() - - def update_state(self, robot_state: RobotState): - """ - Update robot collision object from new robot state - - :param robot_state: Robot pose (x, y, yaw) - :type robot_state: RobotState - """ - self._robot_object = fcl.CollisionObject(self._robot_geometry) - - # Get translation and rotation from robot state - translation = np.array([robot_state.x, robot_state.y, 0.0]) - c_t = np.cos(robot_state.yaw) - s_t = np.sin(robot_state.yaw) - R = np.array([[c_t, -s_t, 0.0], [s_t, c_t, 0.0], [0.0, 0.0, 1.0]]) - transform = fcl.Transform(R, translation) - - # Set transform to robot object - self._robot_object.setTransform(transform) - - def set_map(self, map_3d: np.ndarray): - """ - Set new map to the collision manager - - :param map_3d: 3D array (PointCloud data) - :type map_3d: np.ndarray - """ - map_octree = fcl.OcTree(self._map_resolution, points=map_3d) - self._map_object = fcl.CollisionObject(map_octree) - self.got_map = True - - def check_collision(self) -> bool: - """ - Check collision between the robot and the map obstacles - - :return: If any collisions are found - :rtype: bool - """ - # Clear olf robot pose and old map data from the collision manager - self._collision_manager.clear() - - # Register up to dat data to manager - self._collision_manager.registerObjects([self._map_object, self._robot_object]) - self._collision_manager.setup() - - # Get collision between objects registered in the manager - collision_data = fcl.CollisionData() - self._collision_manager.collide(collision_data, fcl.defaultCollisionCallback) - return collision_data.result.is_collision diff --git a/src/kompass_core/third_party/fcl/config.py b/src/kompass_core/third_party/fcl/config.py deleted file mode 100644 index cbab71b0..00000000 --- a/src/kompass_core/third_party/fcl/config.py +++ /dev/null @@ -1,34 +0,0 @@ -from typing import Union, Tuple - -import fcl -from attrs import define, field -from ...utils.common import BaseAttrs, base_validators - -from ...models import RobotGeometry - -# Mapping from RobotGeometry to FCL Geometry -fcl_object_geometry = { - RobotGeometry.Type.BOX: fcl.Box, # (x, y, z) Axis-aligned box with given side lengths - RobotGeometry.Type.SPHERE: fcl.Sphere, # (rad) Sphere with given radius - RobotGeometry.Type.ELLIPSOID: fcl.Ellipsoid, # (x, y, z) Axis-aligned ellipsoid with given radis - RobotGeometry.Type.CAPSULE: fcl.Capsule, # (rad, lz) Capsule with given radius and height along z-axis - RobotGeometry.Type.CONE: fcl.Cone, # (rad, lz) Cone with given radius and cylinder height along z-axis - RobotGeometry.Type.CYLINDER: fcl.Cylinder, # (rad, lz) Cylinder with given radius and height along z-axis -} - - -@define -class FCLConfig(BaseAttrs): - """ - FCL parameters - """ - - map_resolution: float = field( - default=0.01, validator=base_validators.in_range(min_value=1e-9, max_value=1e9) - ) - - robot_geometry_type: Union[str, RobotGeometry.Type] = field( - default=RobotGeometry.Type.BOX, - converter=lambda value: RobotGeometry.Type.from_str(value), - ) - robot_geometry_params: Tuple[float] = field(default=(1.0, 1.0, 1.0)) diff --git a/src/kompass_core/third_party/ompl/config.py b/src/kompass_core/third_party/ompl/config.py index 5eea954f..a6027c9b 100644 --- a/src/kompass_core/third_party/ompl/config.py +++ b/src/kompass_core/third_party/ompl/config.py @@ -1,5 +1,5 @@ import inspect -from typing import Any +from typing import Any, Union from attrs import field, make_class from ...utils.common import BaseAttrs, base_validators @@ -87,14 +87,34 @@ def getPlanners(self): return self.available_planners -def initializePlanners(): +def _convert_log_level(log_level: Union[ompl.util.LogLevel, str]) -> ompl.util.LogLevel: + """Convert log level to ompl.util.LogLevel.""" + if isinstance(log_level, ompl.util.LogLevel): + return log_level + elif isinstance(log_level, str): + upper_log_level = log_level.upper() + if not upper_log_level.startswith("LOG_"): + upper_log_level = "LOG_" + upper_log_level + if upper_log_level == "LOG_WARNING": + # Handle the special case for LOG_WARNING + # as it is not a valid enum in ompl.util.LogLevel + upper_log_level = "LOG_WARN" + try: + return getattr(ompl.util.LogLevel, upper_log_level) + except AttributeError as e: + raise ValueError(f"Invalid OMPL log level string: {log_level}") from e + raise ValueError(f"Invalid OMPL log level: {log_level}") + + +def initializePlanners( + log_level: Union[ompl.util.LogLevel, str] = ompl.util.LogLevel.LOG_ERROR, +): """Initialize planner map, similar to ompl python bindings.""" - logLevel = ompl.util.getLogLevel() - # TODO: make log_level an input to initializePlanners to set it when using ompl - ompl.util.setLogLevel(ompl.util.LogLevel.LOG_ERROR) + log_level = _convert_log_level(log_level) + ompl.util.setLogLevel(log_level) if not hasattr(ompl.geometric, "planners"): ompl.geometric.planners = PlanningAlgorithms(ompl.geometric) - ompl.util.setLogLevel(logLevel) + ompl.util.setLogLevel(log_level) optimization_objectives = { diff --git a/src/kompass_core/third_party/ompl/planner.py b/src/kompass_core/third_party/ompl/planner.py index 6b6da3e8..34b3d8a8 100644 --- a/src/kompass_core/third_party/ompl/planner.py +++ b/src/kompass_core/third_party/ompl/planner.py @@ -3,14 +3,12 @@ import numpy as np from attrs import asdict, define, field from ...utils.common import BaseAttrs, base_validators -from ...utils.geometry import convert_to_plus_minus_pi import omplpy as ompl -from ...models import Robot, RobotState +from ...models import Robot, RobotGeometry from omplpy import base, geometric +from kompass_cpp.planning import OMPL2DGeometricPlanner -from ..fcl.collisions import FCL -from ..fcl.config import FCLConfig from .config import create_config_class, initializePlanners, optimization_objectives @@ -39,11 +37,6 @@ class OMPLGeometricConfig(BaseAttrs): default=1.0, validator=base_validators.in_range(min_value=1e-9, max_value=1e3) ) - log_level: str = field( - default="WARN", - validator=base_validators.in_(["DEBUG", "INFO", "WARN", "ERROR"]), - ) - map_resolution: float = field( default=0.01, validator=base_validators.in_range(min_value=1e-9, max_value=1e9) ) @@ -55,10 +48,10 @@ class OMPLGeometric: def __init__( self, robot: Robot, + log_level: str = "ERROR", use_fcl: bool = True, config: Optional[OMPLGeometricConfig] = None, config_file: Optional[str] = None, - map_3d: Optional[np.ndarray] = None, ): """ Init Open Motion Planning Lib geometric handler @@ -73,7 +66,7 @@ def __init__( :type map_3d: np.ndarray | None, optional """ # Initialize available planners lists in ompl.geometric and ompl.control - initializePlanners() + initializePlanners(log_level) self.solution = None @@ -85,11 +78,6 @@ def __init__( else: self._config = OMPLGeometricConfig() - # Set log level - ompl.util.setLogLevel( - getattr(ompl.util.LogLevel, f"LOG_{self._config.log_level}") - ) - self._use_fcl = use_fcl # Create SE2 state space for 2D planning @@ -102,57 +90,41 @@ def __init__( self.available_planners = ompl.geometric.planners.getPlanners() # create a simple setup object - self.setup = geometric.SimpleSetup(self.state_space) - - # Configure FCL - if use_fcl: - self.__init_fcl(map_3d) - is_state_valid = self._validity_checker_with_fcl + self._ompl_setup = geometric.SimpleSetup(self.state_space) - else: - is_state_valid = self._validity_checker + # Set the selected planner + self._set_planner() - # Add validity method to ompl setup without custom validity checkers - self.setup.setStateValidityChecker(is_state_valid) + # Setup Path Optimization Objective + optimization_objective = getattr(base, self._config.optimization_objective)( + self._ompl_setup.getSpaceInformation() + ) - # Set the selected planner - self.set_planner() + self._ompl_setup.setOptimizationObjective(optimization_objective) if config_file: self.configure(config_file) - def __init_fcl(self, map_3d: Optional[np.ndarray] = None): - """ - Setup FCL - - :param map_3d: Map data - :type map_3d: np.ndarray - """ - fcl_config = FCLConfig( + self._cpp_planner: Optional[OMPL2DGeometricPlanner] = OMPL2DGeometricPlanner( + robot_shape=RobotGeometry.Type.to_kompass_cpp_lib( + self._robot.geometry_type + ), + robot_dimensions=self._robot.geometry_params, + ompl_setup=self._ompl_setup, map_resolution=self._config.map_resolution, - robot_geometry_type=self._robot.geometry_type, - robot_geometry_params=self._robot.geometry_params.tolist(), ) - if not hasattr(self, "fcl"): - # setup collision check - if map_3d is not None: - self.fcl = FCL(fcl_config, map_3d) - else: - self.fcl = FCL(fcl_config) - else: - self.fcl._setup_from_config() def configure( self, - yaml_file: str, + config_file: str, root_name: Optional[str] = None, planner_id: Optional[str] = None, ): """ - Load config from a yaml file + Load config from a configuration file - :param yaml_file: Path to .yaml fila - :type yaml_file: str + :param config_file: Path to file (yaml, json, toml) + :type config_file: str :param root_name: Parent root name of the config in the file 'Parent.ompl' - config must be under 'ompl', defaults to None :type root_name: str | None, optional """ @@ -160,12 +132,7 @@ def configure( nested_root_name = root_name + ".ompl" else: nested_root_name = "ompl" - self._config.from_yaml(yaml_file, nested_root_name=nested_root_name) - - # Set LOG level - ompl.util.setLogLevel( - getattr(ompl.util.LogLevel, f"LOG_{self._config.log_level}") - ) + self._config.from_file(config_file, nested_root_name=nested_root_name) if not planner_id: planner_id = self._config.planner_id @@ -182,61 +149,21 @@ def configure( planner_config = self.available_planners[planner_id] planner_params = create_config_class(name=planner_name, conf=planner_config)() - planner_params.from_yaml(yaml_file, nested_root_name + "." + planner_name) + planner_params.from_file(config_file, nested_root_name + "." + planner_name) - self.set_planner(planner_params, planner_id) + self._set_planner(planner_params, planner_id) self.start = False - # configure FCL if it is used - if self._use_fcl: - self.__init_fcl() - - def clear(self): - """ - Clears planning setup - """ - self.setup.clear() - @property - def path_cost(self) -> Optional[float]: + def path_cost(self) -> float: """ Getter of solution path cost using the configured optimization objective :return: Path cost :rtype: float | None """ - if self.solution: - optimization_objective = getattr(base, self._config.optimization_objective)( - self.setup.getSpaceInformation() - ) - cost = self.solution.cost(optimization_objective) - return cost.value() - return None - - def get_cost_using_objective(self, objective_key: str) -> Optional[float]: - """ - Get solution cost using a specific objective - This is used to get the cost using an objective other than the configured objective - To get the cost using the default objective use self.path_cost directly - - :param objective_key: _description_ - :type objective_key: str - :raises KeyError: Unknown objective name - :return: _description_ - :rtype: _type_ - """ - if not self.solution: - return None - if objective_key in optimization_objectives: - optimization_objective = getattr(base, objective_key)( - self.setup.getSpaceInformation() - ) - cost = self.solution.cost(optimization_objective) - return cost.value() - raise KeyError( - f"Unknown optimization objective. Available optimization objectives are: {optimization_objectives.keys()}" - ) + return self._cpp_planner.get_cost() def setup_problem( self, @@ -250,7 +177,7 @@ def setup_problem( map_3d: Optional[np.ndarray] = None, ): """ - Setup a new planning problem with a map, start and goal infromation + Setup a new planning problem with a map, start and goal information :param map_meta_data: Global map meta data as a dictionary :type map_meta_data: Dict @@ -269,132 +196,16 @@ def setup_problem( :param map_3d: 3D array for map PointCloud data, defaults to None :type map_3d: np.ndarray | None, optional """ - # Clear previous setup - self.setup.clear() - - self.set_space_bounds(map_meta_data) - - scoped_start_state = base.ScopedState(self.state_space) - start_state = scoped_start_state.get() - start_state.setX(start_x) - start_state.setY(start_y) - - # SE2 takes angle in [-pi,+pi] - yaw = convert_to_plus_minus_pi(start_yaw) - start_state.setYaw(yaw) - - scoped_goal_state = base.ScopedState(self.state_space) - goal_state = scoped_goal_state.get() - goal_state.setX(goal_x) - goal_state.setY(goal_y) - - # SE2 takes angle in [-pi,+pi] - yaw = convert_to_plus_minus_pi(goal_yaw) - goal_state.setYaw(yaw) - - self.setup.setStartAndGoalStates( - start=scoped_start_state, goal=scoped_goal_state - ) - - # Setup Path Optimization Objective - optimization_objective = getattr(base, self._config.optimization_objective)( - self.setup.getSpaceInformation() - ) - - self.setup.setOptimizationObjective(optimization_objective) - - if self._use_fcl and map_3d is None: - raise ValueError( - "OMPL is started with collision check -> Map should be provided" - ) - elif self._use_fcl: - # TODO: Add an option to update the map periodically or just at the start, or after a time interval - if not self.start: - self.fcl.set_map(map_3d) - self.start = True - self.fcl.update_state( - robot_state=RobotState(x=start_x, y=start_y, yaw=start_yaw) - ) - - def add_validity_check(self, name: str, validity_function: Callable) -> None: - """ - Add method for state validity check during planning - - :param name: Method key name - :type name: str - :param validity_function: Validity check method - :type validity_function: Callable - - :raises TypeError: If validity check is not callable - :raises TypeError: If validity check method does not return a boolean - """ - # Check that validity function is callable - if callable(validity_function) and callable(validity_function): - # Check if the function returns a boolean value - args = (None,) * validity_function.__code__.co_argcount - if isinstance(validity_function(*args), bool): - self._custom_validity_check[name] = validity_function - else: - raise TypeError("Validity check function needs to return a boolean") - else: - raise TypeError("Validity check function must be callable") - - def remove_validity_check(self, name: str) -> bool: - """ - Removes an added validity chec!= Nonek - - :param name: Validity check name key - :type name: str - - :raises ValueError: If given key does not correspond to an added validity check - - :return: If validity check is removed - :rtype: bool - """ - deleted_method = self._custom_validity_check.pop(name, None) - if deleted_method is not None: - return True - else: - raise ValueError( - f"Cannot remove validity check titled {name} as it does not exist" - ) - - def _validity_checker(self, state, **_) -> bool: - """ - State validity checker method - - :param state: Robot state - :type state: SE2State - - :return: If state is valid - :rtype: bool - """ - # Run bounds and state check - return self.setup.getSpaceInformation().satisfiesBounds(state) - - def _validity_checker_with_fcl(self, state, **_) -> bool: - """ - State validity checker method - - :param state: Robot state - :type state: SE2State - - :return: If state is valid - :rtype: bool - """ - # Run bounds and state check - state_space_valid: bool = self.setup.getSpaceInformation().satisfiesBounds( - state + self._set_space_bounds(map_meta_data) + self._cpp_planner.setup_problem( + start_x=start_x, + start_y=start_y, + start_yaw=start_yaw, + goal_x=goal_x, + goal_y=goal_y, + goal_yaw=goal_yaw, + map_3d=map_3d, ) - if self.fcl.got_map: - self.fcl.update_state( - RobotState(x=state.getX(), y=state.getY(), yaw=state.getYaw()) - ) - is_collision = self.fcl.check_collision() - else: - is_collision = False - - return state_space_valid and not is_collision @property def planner_params(self) -> Optional[ompl.base.ParamSet]: @@ -434,7 +245,7 @@ def planner_id(self) -> str: """ return self._config.planner_id - def set_planner( + def _set_planner( self, planner_config: Optional[BaseAttrs] = None, planner_id: Optional[str] = None, @@ -455,40 +266,32 @@ def set_planner( else: self._config.planner_id = planner_id - self.planner = eval("%s(self.setup.getSpaceInformation())" % planner_id) + self.planner = eval( + "%s(self._ompl_setup.getSpaceInformation())" % planner_id + ) if planner_config: self.planner_params = planner_config - self.setup.setPlanner(self.planner) + self._ompl_setup.setPlanner(self.planner) except Exception: raise - def set_space_bounds(self, map_meta_data: Dict): + def _set_space_bounds(self, map_meta_data: Dict): """ Set planning space bounds from map data :param map_meta_data: Map meta data :type map_meta_data: Dict """ - # X-axis bounds - x_lower = map_meta_data["origin_x"] - x_upper = x_lower + map_meta_data["resolution"] * map_meta_data["width"] - - # Y-axis bounds - y_lower = map_meta_data["origin_y"] - y_upper = y_lower + map_meta_data["resolution"] * map_meta_data["height"] - - # set lower and upper bounds - bounds = base.RealVectorBounds(2) - - bounds.setLow(index=0, value=x_lower) - bounds.setLow(index=1, value=y_lower) - bounds.setHigh(index=0, value=x_upper) - bounds.setHigh(index=1, value=y_upper) - - self.state_space.setBounds(bounds) + self._cpp_planner.set_space_bounds_from_map( + origin_x=map_meta_data["origin_x"], + origin_y=map_meta_data["origin_y"], + width=map_meta_data["width"], + height=map_meta_data["height"], + resolution=map_meta_data["resolution"], + ) def solve(self) -> ompl.geometric.PathGeometric: """ @@ -497,23 +300,9 @@ def solve(self) -> ompl.geometric.PathGeometric: :return: Solution (Path points) or None if no solution is found :rtype: ompl.geometric.PathGeometric """ - solved = self.setup.solve(self._config.planning_timeout) + solved = self._cpp_planner.solve(self._config.planning_timeout) if solved: - self.solution = self.setup.getSolutionPath() - - return self.solution - return None - - def simplify_solution(self) -> ompl.geometric.PathGeometric: - """ - Simplify the path - - :return: Simplified path - :rtype: ompl.geometric.PathGeometric - """ - if self.solution: - self.setup.simplifySolution(self._config.simplification_timeout) - self.solution = self.setup.getSolutionPath() + self.solution = self._cpp_planner.get_solution() return self.solution return None diff --git a/src/kompass_core/utils/base_attrs.py b/src/kompass_core/utils/base_attrs.py index 086522bd..5bdcadf3 100644 --- a/src/kompass_core/utils/base_attrs.py +++ b/src/kompass_core/utils/base_attrs.py @@ -1,5 +1,7 @@ import json -# from types import NoneType, GenericAlias +import os +import yaml +import toml from typing import ( Any, Callable, @@ -14,8 +16,7 @@ from copy import deepcopy import numpy as np from attrs import asdict, define, fields_dict -from attrs import Attribute, has as attrs_has -from omegaconf import OmegaConf +from attrs import Attribute def skip_no_init(a: Attribute, _) -> bool: @@ -197,61 +198,56 @@ def from_dict(self, dict_obj: Dict) -> None: ) setattr(self, key, value) - def from_yaml( + def _select_nested_config( + cls, config: Dict[str, Any], key_path: Optional[str] + ) -> Dict[str, Any]: + if not key_path: + return config + keys = key_path.split(".") + for key in keys: + config = config.get(key, {}) + return config + + def from_file( self, file_path: str, nested_root_name: Union[str, None] = None, get_common: bool = False, - ) -> None: + ) -> bool: """ - Update class attributes from yaml + Update class attributes from yaml, json, or toml - :param file_path: Path to config file (.yaml) - :type file_path: str + :param file_path: Path to config file (.yaml, .json, .toml) :param nested_root_name: Nested root name for the config, defaults to None - :type nested_root_name: str | None, optional + :param get_common: Whether to get extra config root (for merging), defaults to False """ - # Load the YAML file - raw_config = OmegaConf.load(file_path) - - # check for root name if given - if nested_root_name: - config = OmegaConf.select(raw_config, nested_root_name) - if get_common: - extra_config = OmegaConf.select(raw_config, "/**") + ext: str = os.path.splitext(file_path)[1].lower() + + with open(file_path, "r", encoding="utf-8") as f: + if ext in [".yaml", ".yml"]: + raw_config: Dict[str, Any] = yaml.safe_load(f) + elif ext == ".json": + raw_config = json.load(f) + elif ext == ".toml": + raw_config = toml.load(f) else: - extra_config = None - else: - config = raw_config - extra_config = None - - for attr in self.__attrs_attrs__: - # Check in config - if hasattr(config, attr.name): - attr_value = getattr(self, attr.name) - # Check to handle nested config - if attrs_has(attr.type): - root_name = f"{nested_root_name}.{attr.name}" - - attr_value.from_yaml(file_path, root_name) - - setattr(self, attr.name, attr_value) - else: - setattr(self, attr.name, getattr(config, attr.name)) - - # Check in the common config if present - elif extra_config: - if hasattr(extra_config, attr.name): - attr_value = getattr(self, attr.name) - # Check to handle nested config - if attrs_has(attr.type): - root_name = f"/**.{attr.name}" - - attr_value.from_yaml(file_path, root_name) - - setattr(self, attr.name, attr_value) - else: - setattr(self, attr.name, getattr(extra_config, attr.name)) + raise ValueError(f"Unsupported config format: {ext}") + + # Extract specific and common config sections + + config = self._select_nested_config(raw_config, nested_root_name) + extra_config = ( + self._select_nested_config(raw_config, "/**") if get_common else {} + ) + + if not config and not extra_config: + return False + + # Merge common config with selected config (selected config takes precedence) + merged_config = {**extra_config, **config} + # Set attributes from final merged config + self.from_dict(merged_config) + return True def to_json(self) -> Union[str, bytes, bytearray]: """ diff --git a/src/kompass_core/utils/emergency_stop.py b/src/kompass_core/utils/emergency_stop.py index ba2b0e41..6b873b5c 100644 --- a/src/kompass_core/utils/emergency_stop.py +++ b/src/kompass_core/utils/emergency_stop.py @@ -1,9 +1,10 @@ -from typing import Optional, List +from typing import Optional from logging import Logger from ..models import ( Robot, RobotGeometry, ) +import numpy as np from ..datatypes import LaserScanData @@ -14,15 +15,25 @@ def __init__( self, robot: Robot, emergency_distance: float, + slowdown_distance: float, emergency_angle: float, - sensor_position_robot: Optional[List[float]] = None, - sensor_rotation_robot: Optional[List[float]] = None, + sensor_position_robot: Optional[np.ndarray] = None, + sensor_rotation_robot: Optional[np.ndarray] = None, use_gpu: bool = False, ) -> None: self.__emergency_distance = emergency_distance + self.__slowdown_distance = slowdown_distance self.__emergency_angle = emergency_angle - self.__sensor_position_robot = sensor_position_robot or [0.0, 0.0, 0.0] - self.__sensor_rotation_robot = sensor_rotation_robot or [0.0, 0.0, 0.0, 1.0] + self.__sensor_position_robot = ( + sensor_position_robot + if sensor_position_robot is not None + else np.array([0.0, 0.0, 0.0], dtype=np.float32) + ) + self.__sensor_rotation_robot = ( + sensor_rotation_robot + if sensor_rotation_robot is not None + else np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float32) + ) self.__robot_shape = RobotGeometry.Type.to_kompass_cpp_lib(robot.geometry_type) self.__robot_dimensions = robot.geometry_params self.__use_gpu = use_gpu @@ -32,15 +43,15 @@ def _init_checker(self, scan: LaserScanData) -> None: if self.__use_gpu: try: from kompass_cpp.utils import CriticalZoneCheckerGPU + self._critical_zone_checker = CriticalZoneCheckerGPU( robot_shape=self.__robot_shape, robot_dimensions=self.__robot_dimensions, - sensor_position_body=self.__sensor_position_robot - or [0.0, 0.0, 0.0], - sensor_rotation_body=self.__sensor_rotation_robot - or [0.0, 0.0, 0.0, 1.0], + sensor_position_body=self.__sensor_position_robot, + sensor_rotation_body=self.__sensor_rotation_robot, critical_angle=self.__emergency_angle, critical_distance=self.__emergency_distance, + slowdown_distance=self.__slowdown_distance, scan_angles=scan.angles, ) except (ImportError, ModuleNotFoundError): @@ -55,22 +66,22 @@ def _init_checker(self, scan: LaserScanData) -> None: self._critical_zone_checker = CriticalZoneChecker( robot_shape=self.__robot_shape, robot_dimensions=self.__robot_dimensions, - sensor_position_body=self.__sensor_position_robot or [0.0, 0.0, 0.0], - sensor_rotation_body=self.__sensor_rotation_robot - or [0.0, 0.0, 0.0, 1.0], + sensor_position_body=self.__sensor_position_robot, + sensor_rotation_body=self.__sensor_rotation_robot, critical_angle=self.__emergency_angle, critical_distance=self.__emergency_distance, + slowdown_distance=self.__slowdown_distance, ) - def run(self, *_, scan: LaserScanData, forward: bool = True) -> bool: + def run(self, *_, scan: LaserScanData, forward: bool = True) -> float: """Runs emergency checking on new incoming laser scan data :param scan: 2D Laserscan data (ranges/angles) :type scan: LaserScanData :param forward: If the robot is moving forward or not, defaults to True :type forward: bool, optional - :return: If an obstacle is within the safety zone - :rtype: bool + :return: Slowdown factor if an obstacle is within the safety zone + :rtype: float """ if not self.__initialized: self._init_checker(scan) diff --git a/src/kompass_core/utils/geometry.py b/src/kompass_core/utils/geometry.py index 40b40349..5de47bbe 100644 --- a/src/kompass_core/utils/geometry.py +++ b/src/kompass_core/utils/geometry.py @@ -2,8 +2,6 @@ from typing import List, Union, Tuple import numpy as np -import quaternion -from quaternion import quaternion as quat from ..datatypes.pose import PoseData from ..datatypes.laserscan import LaserScanData @@ -81,88 +79,109 @@ def probability_of_collision( return prop_col -def _rotate_vector_by_quaternion(q: quaternion.quaternion, v: List) -> List: +def _np_quaternion_multiply(q1: np.ndarray, q2: np.ndarray) -> np.ndarray: """ - rotate a vector v by a rotation quaternion q + Multiplies two quaternions q1 * q2 + Each quaternion is an array [w, x, y, z] + """ + w0, x0, y0, z0 = q1 + w1, x1, y1, z1 = q2 + return np.array([ + w0 * w1 - x0 * x1 - y0 * y1 - z0 * z1, + w0 * x1 + x0 * w1 + y0 * z1 - z0 * y1, + w0 * y1 - x0 * z1 + y0 * w1 + z0 * x1, + w0 * z1 + x0 * y1 - y0 * x1 + z0 * w1, + ]) - :param q: the rotation to perform - :type q: quaternion.quaternion - :param v: the vector to be rotated - :type v: List - :return: the rotated position of the vector - :rtype: List +def _np_quaternion_conjugate(q: np.ndarray) -> np.ndarray: + """ + Returns the conjugate of a quaternion """ - vq = quat(0, 0, 0, 0) - vq.imag = v - return (q * vq * q.inverse()).imag + w, x, y, z = q + return np.array([w, -x, -y, -z]) -def get_pose_target_in_reference_frame( - reference_pose: PoseData, target_pose: PoseData -) -> PoseData: +def _rotate_vector_by_quaternion(q: np.ndarray, v: List[float]) -> List[float]: """ - Computes a target pose with respect to the frame (coordinate system) defined by a reference. - target and reference should be given first in a common frame - - :param reference_pose: Pose of reference frame in a common frame - :type reference_pose: PoseData - :param target_pose: Pose of target frame in a common frame - :type target_pose: PoseData + Rotate a 3D vector v by a quaternion q. - :return: The pose of target in the reference frame - :rtype: PoseData + :param q: quaternion [w, x, y, z] + :param v: vector [x, y, z] + :return: rotated vector """ - position_target = target_pose.get_position() - orientation_target = target_pose.get_orientation() + vq = np.array([0.0, *v]) + q_conj = _np_quaternion_conjugate(q) + rotated_vq = _np_quaternion_multiply(_np_quaternion_multiply(q, vq), q_conj) + return rotated_vq[1:].tolist() - reference_position = reference_pose.get_position() - reference_rotation = reference_pose.get_orientation() - orientation_target_in_ref = reference_rotation.inverse() * orientation_target +def _transform_pose(pose: PoseData, reference_pose: PoseData) -> PoseData: + """ + Transforms a pose from a local frame to a global frame using a reference pose. + Equivalent to: global_pose = reference_pose * local_pose + + :param pose: PoseData in local frame + :type pose: PoseData + :param reference_pose: PoseData of local frame in global frame + :type reference_pose: PoseData + :return: PoseData in global frame + :rtype: PoseData + """ + rotated_position = _rotate_vector_by_quaternion(reference_pose.get_orientation(), pose.get_position().tolist()) + translated_position = reference_pose.get_position() + np.array(rotated_position) - position_target_in_ref = _rotate_vector_by_quaternion( - reference_rotation.inverse(), (position_target - reference_position).tolist() - ) + combined_orientation = _np_quaternion_multiply(reference_pose.get_orientation(), pose.get_orientation()) - target_pose_in_ref = PoseData() - target_pose_in_ref.set_pose( - x=position_target_in_ref[0], - y=position_target_in_ref[1], - z=position_target_in_ref[2], - qw=orientation_target_in_ref.w, - qx=orientation_target_in_ref.x, - qy=orientation_target_in_ref.y, - qz=orientation_target_in_ref.z, + result = PoseData() + result.set_pose( + *translated_position, + *combined_orientation ) + return result - return target_pose_in_ref +def _inverse_pose(pose: PoseData) -> PoseData: + """ + Computes the inverse of a pose (converts pose_in_frame to frame_in_pose) -def from_frame1_to_frame2( - pose_1_in_2: PoseData, pose_target_in_1: PoseData -) -> PoseData: + :param pose: PoseData to invert + :type pose: PoseData + :return: Inverse pose + :rtype: PoseData """ - get the pose of a target in frame 2 instead of frame 1 + inv_orientation = _np_quaternion_conjugate(pose.get_orientation()) + inv_position = _rotate_vector_by_quaternion(inv_orientation, (-pose.get_position()).tolist()) + + result = PoseData() + result.set_pose( + *inv_position, + *inv_orientation + ) + return result - :param pose_1_in_2: pose of frame 1 in frame 2 - :type pose_1_in_2: PoseData - :param pose_target_in_1: pose of target in frame 1 - :type pose_target_in_1: PoseData +def transform_point_from_local_to_global(point_pose_in_local: PoseData, robot_pose_in_global: PoseData) -> PoseData: + """Transforms a point from the robot frame to the global frame. - :return: pose of target in frame 2 - :rtype: PoseData + :param point_pose_in_local: Target point in robot frame + :type point_pose_in_local: PoseData + :param robot_pose_in_global: Robot pose in global frame + :type robot_pose_in_global: PoseData + :return: Target point in global frame + :rtype: PoseData """ - pose_2_origin = PoseData() - pose_2_in_1 = get_pose_target_in_reference_frame( - reference_pose=pose_1_in_2, target_pose=pose_2_origin - ) + return _transform_pose(point_pose_in_local, robot_pose_in_global) - pose_target_in_2 = get_pose_target_in_reference_frame( - reference_pose=pose_2_in_1, target_pose=pose_target_in_1 - ) +def get_relative_pose(pose_1_in_ref: PoseData, pose_2_in_ref: PoseData) -> PoseData: + """ + Computes the pose of pose_2 in the coordinate frame of pose_1. - return pose_target_in_2 + :param pose_1_in_ref: PoseData of frame 1 in reference frame + :param pose_2_in_ref: PoseData of frame 2 in reference frame + :return: PoseData of pose_2 in frame 1 + """ + pose_ref_in_1 = _inverse_pose(pose_1_in_ref) + return _transform_pose(pose_2_in_ref, pose_ref_in_1) def from_euler_to_quaternion(yaw: float, pitch: float, roll: float) -> np.ndarray: @@ -209,7 +228,7 @@ def from_2d_to_PoseData(x: float, y: float, heading: float) -> PoseData: :rtype: PoseData """ pose_data = PoseData() - quat_data: quat = from_euler_to_quaternion(heading, 0.0, 0.0) + quat_data: np.ndarray = from_euler_to_quaternion(heading, 0.0, 0.0) pose_data.set_position(x, y, 0.0) pose_data.set_orientation( qw=quat_data[0], qx=quat_data[1], qy=quat_data[2], qz=quat_data[3] @@ -254,7 +273,7 @@ def from_frame1_to_frame2_2d( ) # Get transformd pose - pose_target_in_2: PoseData = from_frame1_to_frame2(pose_1_in_2, pose_target_in_1) + pose_target_in_2: PoseData = get_relative_pose(pose_1_in_2, pose_target_in_1) return pose_target_in_2 diff --git a/src/kompass_core/utils/visualization.py b/src/kompass_core/utils/visualization.py index 11ee327c..46a8fba9 100644 --- a/src/kompass_core/utils/visualization.py +++ b/src/kompass_core/utils/visualization.py @@ -1,16 +1,11 @@ from typing import List, Optional import numpy as np -import cv2 -import matplotlib.colors as PltColors -import matplotlib.markers as PltMarkers -import matplotlib.pyplot as plt -from matplotlib.axes import Axes from ..datatypes.obstacles import ObstaclesData, OCCUPANCY_TYPE from ..datatypes.path import PathPoint, PathSample -def plt_map_obstacles(map: ObstaclesData, ax: Axes = None): +def plt_map_obstacles(map: ObstaclesData, ax=None): """ Plot a given map's obstacles as circles @@ -19,6 +14,12 @@ def plt_map_obstacles(map: ObstaclesData, ax: Axes = None): :param ax: Plotting Axes, defaults to None :type ax: Axes, optional """ + try: + import matplotlib.pyplot as plt + except ImportError as e: + raise ImportError( + "Matplotlib is required for visualization. Please install it using 'pip install matplotlib'." + ) from e if not ax: ax = plt.gca() @@ -28,9 +29,7 @@ def plt_map_obstacles(map: ObstaclesData, ax: Axes = None): ax.add_patch(circle) -def plt_path_sample( - ref_path: PathSample, label="", color="blue", marker="", ax: Axes = None -): +def plt_path_sample(ref_path: PathSample, label="", color="blue", marker="", ax=None): """ Plot a given path sample as a line @@ -45,6 +44,14 @@ def plt_path_sample( :param ax: Plotting Axes, defaults to None :type ax: Axes, optional """ + try: + import matplotlib.colors as PltColors + import matplotlib.markers as PltMarkers + import matplotlib.pyplot as plt + except ImportError as e: + raise ImportError( + "Matplotlib is required for visualization. Please install it using 'pip install matplotlib'." + ) from e if color not in PltColors.cnames.keys(): color = "blue" @@ -64,7 +71,7 @@ def plt_path_sample( def plt_path_points_list( - ref_path: List[PathPoint], label="", color="blue", marker="", ax: Axes = None + ref_path: List[PathPoint], label="", color="blue", marker="", ax=None ): """ Plot a given list of path points as a line @@ -80,6 +87,14 @@ def plt_path_points_list( :param ax: Plotting Axes, defaults to None :type ax: Axes, optional """ + try: + import matplotlib.colors as PltColors + import matplotlib.markers as PltMarkers + import matplotlib.pyplot as plt + except ImportError as e: + raise ImportError( + "Matplotlib is required for visualization. Please install it using 'pip install matplotlib'." + ) from e if color not in PltColors.cnames.keys(): color = "blue" @@ -110,7 +125,12 @@ def _resize_image(image: np.ndarray, scale: float) -> np.ndarray: :return: _description_ :rtype: np.ndarray """ - + try: + import cv2 + except ImportError as e: + raise ImportError( + "OpenCV is required for image processing. Please install it using 'pip install opencv-python'." + ) from e width = int(image.shape[1] * scale) height = int(image.shape[0] * scale) dim = (width, height) @@ -197,6 +217,12 @@ def visualize_grid( :return: grid image :rtype: np.ndarray """ + try: + import cv2 + except ImportError as e: + raise ImportError( + "OpenCV is required for image processing. Please install it using 'pip install opencv-python'." + ) from e grid_image = MAPPING_GRID_TO_COLOR[ grid_data ] # map HxWx1 grid to HxWx3 RGB colored grid as image diff --git a/src/kompass_cpp/CMakeLists.txt b/src/kompass_cpp/CMakeLists.txt index 23e73f2d..d6075a06 100644 --- a/src/kompass_cpp/CMakeLists.txt +++ b/src/kompass_cpp/CMakeLists.txt @@ -2,6 +2,7 @@ find_package(Eigen3 3.4 REQUIRED NO_MODULE) find_package(PCL COMPONENTS common io REQUIRED) find_package(fcl REQUIRED) +find_package(ompl REQUIRED) # find sycl find_package(AdaptiveCpp) diff --git a/src/kompass_cpp/bindings/bindings.cpp b/src/kompass_cpp/bindings/bindings.cpp index d1cc3b3d..08f3f99f 100644 --- a/src/kompass_cpp/bindings/bindings.cpp +++ b/src/kompass_cpp/bindings/bindings.cpp @@ -1,8 +1,10 @@ #include #include #include +#include #include +#include "utils/gpu_check.h" #include "utils/logger.h" namespace py = nanobind; @@ -12,6 +14,7 @@ void bindings_config(py::module_ &); void bindings_control(py::module_ &); void bindings_mapping(py::module_ &); void bindings_utils(py::module_ &); +void bindings_planning(py::module_ &); using namespace Kompass; @@ -26,14 +29,19 @@ NB_MODULE(kompass_cpp, m) { bindings_config(m); bindings_control(m); bindings_mapping(m); + bindings_planning(m); // Utils bindings submodule py::enum_(m, "LogLevel") .value("DEBUG", LogLevel::DEBUG) .value("INFO", LogLevel::INFO) .value("WARNING", LogLevel::WARNING) + .value("WARN", LogLevel::WARNING) .value("ERROR", LogLevel::ERROR) .export_values(); + m.def("set_log_level", &setLogLevel, "Set the log level"); m.def("set_log_file", &setLogFile, "Set the log file"); + m.def("get_available_accelerators", &getAvailableAccelerators, + "Get available accelerators"); } diff --git a/src/kompass_cpp/bindings/bindings_control.cpp b/src/kompass_cpp/bindings/bindings_control.cpp index 95f67c17..dbce2055 100644 --- a/src/kompass_cpp/bindings/bindings_control.cpp +++ b/src/kompass_cpp/bindings/bindings_control.cpp @@ -1,15 +1,16 @@ #include #include -#include #include +#include #include #include #include "controllers/dwa.h" #include "controllers/follower.h" #include "controllers/pid.h" +#include "controllers/rgb_follower.h" #include "controllers/stanley.h" -#include "controllers/vision_follower.h" +#include "controllers/vision_dwa.h" #include "datatypes/control.h" #include "datatypes/trajectory.h" @@ -28,6 +29,7 @@ void bindings_control(py::module_ &m) { // Limits setup py::class_( m_control, "LinearVelocityControlParams") + .def(py::init()) .def(py::init(), py::arg("max_vel") = 0.0, py::arg("max_acc") = 0.0, py::arg("max_decel") = 0.0) .def_rw("max_vel", &Control::LinearVelocityControlParams::maxVel) @@ -37,6 +39,7 @@ void bindings_control(py::module_ &m) { py::class_( m_control, "AngularVelocityControlParams") + .def(py::init()) .def(py::init(), py::arg("max_ang") = M_PI, py::arg("max_omega") = 0.0, py::arg("max_acc") = 0.0, py::arg("max_decel") = 0.0) @@ -48,9 +51,10 @@ void bindings_control(py::module_ &m) { &Control::AngularVelocityControlParams::maxDeceleration); py::class_(m_control, "ControlLimitsParams") - .def(py::init(), + .def(py::init<>()) + .def(py::init(), py::arg("vel_x_ctr_params") = Control::LinearVelocityControlParams(), py::arg("vel_y_ctr_params") = Control::LinearVelocityControlParams(), py::arg("omega_ctr_params") = @@ -88,7 +92,8 @@ void bindings_control(py::module_ &m) { .def(py::init<>()) .def(py::init()) .def("set_interpolation_type", &Control::Follower::setInterpolationType) - .def("set_current_path", &Control::Follower::setCurrentPath) + .def("set_current_path", &Control::Follower::setCurrentPath, + py::arg("path"), py::arg("interpolate") = true) .def("clear_current_path", &Control::Follower::clearCurrentPath) .def("is_goal_reached", &Control::Follower::isGoalReached) .def("get_vx_cmd", &Control::Follower::getLinearVelocityCmdX) @@ -165,8 +170,8 @@ void bindings_control(py::module_ &m) { py::class_(m_control, "DWA") .def(py::init, const std::array &, - const std::array &, double, + std::vector, const Eigen::Vector3f &, + const Eigen::Vector4f &, double, Control::CostEvaluator::TrajectoryCostsWeights, int>(), py::arg("control_limits"), py::arg("control_type"), py::arg("time_step"), py::arg("prediction_horizon"), @@ -179,7 +184,7 @@ void bindings_control(py::module_ &m) { .def(py::init, - const std::array &, const std::array &, + const Eigen::Vector3f &, const Eigen::Vector4f &, Control::CostEvaluator::TrajectoryCostsWeights, int>(), py::arg("config"), py::arg("control_limits"), py::arg("control_type"), py::arg("robot_shape_type"), @@ -189,12 +194,13 @@ void bindings_control(py::module_ &m) { .def("compute_velocity_commands", py::overload_cast( - &Control::DWA::computeVelocityCommandsSet), + &Control::DWA::computeVelocityCommandsSet), py::rv_policy::reference_internal) .def("compute_velocity_commands", py::overload_cast &>( - &Control::DWA::computeVelocityCommandsSet), + &Control::DWA::computeVelocityCommandsSet< + std::vector>), py::rv_policy::reference_internal) .def("add_custom_cost", &Control::DWA::addCustomCost) // Custom cost function for DWA planner @@ -216,19 +222,85 @@ void bindings_control(py::module_ &m) { .def("set_resolution", &Control::DWA::resetOctreeResolution); // Vision Follower - py::class_( - m_control, "VisionFollowerParameters") + py::class_( + m_control, "RGBFollowerParameters") .def(py::init<>()); - py::class_(m_control, - "VisionFollower") + py::class_(m_control, "RGBFollower") .def(py::init(), + const Control::RGBFollower::RGBFollowerConfig>(), py::arg("control_type"), py::arg("control_limits"), py::arg("config")) - .def("reset_target", &Control::VisionFollower::resetTarget) - .def("get_ctrl", &Control::VisionFollower::getCtrl) - .def("run", &Control::VisionFollower::run); + .def("reset_target", &Control::RGBFollower::resetTarget) + .def("get_ctrl", &Control::RGBFollower::getCtrl) + .def("get_errors", &Control::RGBFollower::getErrors) + .def("run", &Control::RGBFollower::run, + py::arg("detection") = py::none()); + + // Vision DWA + py::class_(m_control, + "VisionDWAParameters") + .def(py::init<>()); + + py::class_(m_control, "VisionDWA") + .def(py::init &, const Eigen::Vector3f &, + const Eigen::Vector4f &, const Eigen::Vector3f &, + const Eigen::Vector4f &, const double, + const Control::CostEvaluator::TrajectoryCostsWeights &, + const int, const Control::VisionDWA::VisionDWAConfig &>(), + py::arg("control_type"), py::arg("control_limits"), + py::arg("max_linear_samples"), py::arg("max_angular_samples"), + py::arg("robot_shape_type"), py::arg("robot_dimensions"), + py::arg("proximity_sensor_position_wrt_body"), + py::arg("proximity_sensor_rotation_wrt_body"), + py::arg("vision_sensor_position_wrt_body"), + py::arg("vision_sensor_rotation_wrt_body"), py::arg("octree_res"), + py::arg("cost_weights"), py::arg("max_num_threads") = 1, + py::arg("config") = Control::VisionDWA::VisionDWAConfig()) + .def("set_camera_intrinsics", &Control::VisionDWA::setCameraIntrinsics, + py::arg("focal_length_x"), py::arg("focal_length_y"), + py::arg("principal_point_x"), py::arg("principal_point_y")) + .def("set_initial_tracking", + py::overload_cast &, + const float>( + &Control::VisionDWA::setInitialTracking), + py::arg("pixel_x"), py::arg("pixel_y"), py::arg("detected_boxes_3d"), + py::arg("robot_orientation") = 0.0) + .def("set_initial_tracking", + py::overload_cast &, + const std::vector &, const float>( + &Control::VisionDWA::setInitialTracking), + py::arg("pixel_x"), py::arg("pixel_y"), + py::arg("aligned_depth_image"), py::arg("detected_boxes_2d"), + py::arg("robot_orientation") = 0.0) + .def("set_initial_tracking", + py::overload_cast &, + const Bbox2D &, const float>( + &Control::VisionDWA::setInitialTracking), + py::arg("aligned_depth_image"), py::arg("target_box_2d"), + py::arg("robot_orientation") = 0.0) + .def("get_errors", &Control::VisionDWA::getErrors) + .def("get_tracking_ctrl", + py::overload_cast &, + const std::vector &, + const Control::Velocity2D &, + const std::vector &>( + &Control::VisionDWA::getTrackingCtrl< + std::vector>), + py::arg("aligned_depth_image"), py::arg("detected_boxes"), + py::arg("robot_velocity"), py::arg("sensor_data")) + .def("get_tracking_ctrl", + py::overload_cast &, + const std::vector &, + const Control::Velocity2D &, + const Control::LaserScan &>( + &Control::VisionDWA::getTrackingCtrl), + py::arg("aligned_depth_image"), py::arg("detected_boxes"), + py::arg("robot_velocity"), py::arg("sensor_data")); } diff --git a/src/kompass_cpp/bindings/bindings_gpu.cpp b/src/kompass_cpp/bindings/bindings_gpu.cpp index 0e1657ed..15f0db37 100644 --- a/src/kompass_cpp/bindings/bindings_gpu.cpp +++ b/src/kompass_cpp/bindings/bindings_gpu.cpp @@ -3,7 +3,6 @@ #include #include #include -#include namespace py = nanobind; using namespace Kompass; @@ -27,12 +26,12 @@ void bindings_utils_gpu(py::module_ &m) { py::class_(m, "CriticalZoneCheckerGPU") .def(py::init &, - const std::array &, const std::array &, - float, float, const std::vector &>(), + const Eigen::Vector3f &, const Eigen::Vector4f &, const float, + const float, const float, const std::vector &>(), py::arg("robot_shape"), py::arg("robot_dimensions"), py::arg("sensor_position_body"), py::arg("sensor_rotation_body"), py::arg("critical_angle"), py::arg("critical_distance"), - py::arg("scan_angles")) - .def("check", &CriticalZoneChecker::check, py::arg("ranges"), + py::arg("slowdown_distance"), py::arg("scan_angles")) + .def("check", &CriticalZoneCheckerGPU::check, py::arg("ranges"), py::arg("angles"), py::arg("forward")); } diff --git a/src/kompass_cpp/bindings/bindings_planning.cpp b/src/kompass_cpp/bindings/bindings_planning.cpp new file mode 100644 index 00000000..4d033249 --- /dev/null +++ b/src/kompass_cpp/bindings/bindings_planning.cpp @@ -0,0 +1,33 @@ +#include "planning/ompl.h" +#include +#include +#include +#include + +namespace py = nanobind; +using namespace Kompass; + +// Utils submodule +void bindings_planning(py::module_ &m) { + auto m_planning = m.def_submodule("planning", "KOMPASS CPP Planning Module"); + + py::class_(m_planning, + "OMPL2DGeometricPlanner") + .def(py::init &, + const ompl::geometric::SimpleSetup &, const float>(), + py::arg("robot_shape"), py::arg("robot_dimensions"), + py::arg("ompl_setup"), py::arg("map_resolution") = 0.01) + .def("setup_problem", &Planning::OMPL2DGeometricPlanner::setupProblem, + py::arg("start_x"), py::arg("start_y"), py::arg("start_yaw"), + py::arg("goal_x"), py::arg("goal_y"), py::arg("goal_yaw"), + py::arg("map_3d")) + .def("solve", &Planning::OMPL2DGeometricPlanner::solve, + py::arg("planning_timeout") = 1.0) + .def("get_solution", &Planning::OMPL2DGeometricPlanner::getPath) + .def("set_space_bounds_from_map", + &Planning::OMPL2DGeometricPlanner::setSpaceBoundsFromMap, + py::arg("origin_x"), py::arg("origin_y"), py::arg("width"), + py::arg("height"), py::arg("resolution")) + .def("get_cost", &Planning::OMPL2DGeometricPlanner::getCost); +} diff --git a/src/kompass_cpp/bindings/bindings_types.cpp b/src/kompass_cpp/bindings/bindings_types.cpp index 80eb3dbf..2c61dd2b 100644 --- a/src/kompass_cpp/bindings/bindings_types.cpp +++ b/src/kompass_cpp/bindings/bindings_types.cpp @@ -2,12 +2,13 @@ #include #include #include -#include #include +#include +#include -#include "controllers/vision_follower.h" #include "datatypes/control.h" #include "datatypes/path.h" +#include "datatypes/tracking.h" #include "datatypes/trajectory.h" #include "utils/collision_check.h" @@ -24,6 +25,7 @@ std::string printControlCmd(const Control::Velocity2D &velocity_command) { void bindings_types(py::module_ &m) { auto m_types = m.def_submodule("types", "KOMPASS CPP data types module"); + // Path types py::enum_(m_types, "PathInterpolationType") .value("LINEAR", Path::InterpolationType::LINEAR) .value("CUBIC_SPLINE", Path::InterpolationType::CUBIC_SPLINE) @@ -37,92 +39,119 @@ void bindings_types(py::module_ &m) { .def_rw("yaw", &Path::State::yaw) .def_rw("speed", &Path::State::speed); -py::class_(m_types, "PathPosition") - .def(py::init<>()) - .def_rw("segment_index", &Path::PathPosition::segment_index) - .def_rw("segment_length", &Path::PathPosition::segment_length) - .def_rw("parallel_distance", &Path::PathPosition::parallel_distance) - .def_rw("normal_distance", &Path::PathPosition::normal_distance); - -py::class_(m_types, "Path") - .def(py::init &>(), - py::arg("points") = std::vector()) - .def("reached_end", &Path::Path::endReached) - .def("get_total_length", &Path::Path::totalPathLength) - .def_rw("points", &Path::Path::points); - -// Velocity control command -py::class_(m_types, "ControlCmd") - .def(py::init(), py::arg("vx") = 0.0, - py::arg("vy") = 0.0, py::arg("omega") = 0.0, - py::arg("steer_ang") = 0.0) - .def_prop_rw("vx", &Control::Velocity2D::vx, &Control::Velocity2D::setVx) - .def_prop_rw("vy", &Control::Velocity2D::vy, &Control::Velocity2D::setVy) - .def_prop_rw("omega", &Control::Velocity2D::omega, - &Control::Velocity2D::setOmega) - .def_prop_rw("steer_ang", &Control::Velocity2D::steer_ang, - &Control::Velocity2D::setSteerAng) - .def("__str__", &printControlCmd); - -py::class_(m_types, "ControlCmdList") - .def(py::init<>(), "Default constructor") - .def(py::init(), "Constructor with length", py::arg("length")) - .def_rw("vx", &Control::Velocities::vx, "Speed on x-axis (m/s)") - .def_rw("vy", &Control::Velocities::vy, "Speed on y-axis (m/s)") - .def_rw("omega", &Control::Velocities::omega, "Angular velocity (rad/s)") - .def_rw("length", &Control::Velocities::_length, "Length of the vectors"); - -py::class_(m_types, "TrajectoryPath") - .def(py::init<>()) - .def_ro("x", &Control::TrajectoryPath::x, py::rv_policy::reference_internal) - .def_ro("y", &Control::TrajectoryPath::y, py::rv_policy::reference_internal) - .def_ro("z", &Control::TrajectoryPath::z, - py::rv_policy::reference_internal); - -py::class_(m_types, "TrajectoryVelocities2D") - .def(py::init<>()) - .def_ro("vx", &Control::TrajectoryVelocities2D::vx, - py::rv_policy::reference_internal) - .def_ro("vy", &Control::TrajectoryVelocities2D::vy, - py::rv_policy::reference_internal) - .def_ro("omega", &Control::TrajectoryVelocities2D::omega, - py::rv_policy::reference_internal); - -py::class_(m_types, "Trajectory") - .def(py::init<>()) - .def_ro("velocities", &Control::Trajectory2D::velocities) - .def_ro("path", &Control::Trajectory2D::path); - -py::class_(m_types, "LaserScan") - .def(py::init, std::vector>(), - py::arg("ranges"), py::arg("angles")) - .def_ro("ranges", &Control::LaserScan::ranges) - .def_ro("angles", &Control::LaserScan::angles); - -// For collisions detection -py::enum_(m_types, "RobotGeometry") - .value("CYLINDER", CollisionChecker::ShapeType::CYLINDER) - .value("BOX", CollisionChecker::ShapeType::BOX) - .value("SPHERE", CollisionChecker::ShapeType::SPHERE) - .def("get", [](const std::string &key) { - if (key == "CYLINDER") - return CollisionChecker::ShapeType::CYLINDER; - if (key == "BOX") - return CollisionChecker::ShapeType::BOX; - if (key == "SPHERE") - return CollisionChecker::ShapeType::SPHERE; - throw std::runtime_error("Invalid key"); - }); - -// Vision types -py::class_(m_types, "TrackingData") - .def(py::init, int, int, std::array, - double>(), - py::arg("size_xy"), py::arg("img_width"), py::arg("img_height"), - py::arg("center_xy"), py::arg("depth") = -1.0) - .def_rw("size_xy", &Control::VisionFollower::TrackingData::size_xy) - .def_rw("img_width", &Control::VisionFollower::TrackingData::img_width) - .def_rw("img_height", &Control::VisionFollower::TrackingData::img_height) - .def_rw("center_xy", &Control::VisionFollower::TrackingData::center_xy) - .def_rw("depth", &Control::VisionFollower::TrackingData::depth); + py::class_(m_types, "PathPosition") + .def(py::init<>()) + .def_rw("segment_index", &Path::PathPosition::segment_index) + .def_rw("segment_length", &Path::PathPosition::segment_length) + .def_rw("parallel_distance", &Path::PathPosition::parallel_distance) + .def_rw("normal_distance", &Path::PathPosition::normal_distance); + + py::class_(m_types, "Path") + .def(py::init &>(), + py::arg("points") = std::vector()) + .def("reached_end", &Path::Path::endReached) + .def("get_total_length", &Path::Path::totalPathLength) + .def("size", &Path::Path::getSize) + .def("getIndex", &Path::Path::getIndex, py::arg("index")) + .def("x", &Path::Path::getX) + .def("y", &Path::Path::getY); + + // Velocity control command + py::class_(m_types, "ControlCmd") + .def(py::init(), py::arg("vx") = 0.0, + py::arg("vy") = 0.0, py::arg("omega") = 0.0, + py::arg("steer_ang") = 0.0) + .def_prop_rw("vx", &Control::Velocity2D::vx, &Control::Velocity2D::setVx) + .def_prop_rw("vy", &Control::Velocity2D::vy, &Control::Velocity2D::setVy) + .def_prop_rw("omega", &Control::Velocity2D::omega, + &Control::Velocity2D::setOmega) + .def_prop_rw("steer_ang", &Control::Velocity2D::steer_ang, + &Control::Velocity2D::setSteerAng) + .def("__str__", &printControlCmd); + + py::class_(m_types, "ControlCmdList") + .def(py::init<>(), "Default constructor") + .def(py::init(), "Constructor with length", py::arg("length")) + .def_rw("vx", &Control::Velocities::vx, "Speed on x-axis (m/s)") + .def_rw("vy", &Control::Velocities::vy, "Speed on y-axis (m/s)") + .def_rw("omega", &Control::Velocities::omega, "Angular velocity (rad/s)") + .def_rw("length", &Control::Velocities::_length, "Length of the vectors"); + + py::class_(m_types, "TrajectoryPath") + .def(py::init<>()) + .def_ro("x", &Control::TrajectoryPath::x, + py::rv_policy::reference_internal) + .def_ro("y", &Control::TrajectoryPath::y, + py::rv_policy::reference_internal) + .def_ro("z", &Control::TrajectoryPath::z, + py::rv_policy::reference_internal); + + py::class_(m_types, "TrajectoryVelocities2D") + .def(py::init<>()) + .def_ro("vx", &Control::TrajectoryVelocities2D::vx, + py::rv_policy::reference_internal) + .def_ro("vy", &Control::TrajectoryVelocities2D::vy, + py::rv_policy::reference_internal) + .def_ro("omega", &Control::TrajectoryVelocities2D::omega, + py::rv_policy::reference_internal); + + py::class_(m_types, "Trajectory") + .def(py::init<>()) + .def_ro("velocities", &Control::Trajectory2D::velocities) + .def_ro("path", &Control::Trajectory2D::path); + + py::class_(m_types, "LaserScan") + .def(py::init, std::vector>(), + py::arg("ranges"), py::arg("angles")) + .def_ro("ranges", &Control::LaserScan::ranges) + .def_ro("angles", &Control::LaserScan::angles); + + // For collisions detection + py::enum_(m_types, "RobotGeometry") + .value("CYLINDER", CollisionChecker::ShapeType::CYLINDER) + .value("BOX", CollisionChecker::ShapeType::BOX) + .value("SPHERE", CollisionChecker::ShapeType::SPHERE) + .def_static("get", [](const std::string &key) { + if (key == "CYLINDER") + return CollisionChecker::ShapeType::CYLINDER; + if (key == "BOX") + return CollisionChecker::ShapeType::BOX; + if (key == "SPHERE") + return CollisionChecker::ShapeType::SPHERE; + throw std::runtime_error("Invalid key"); + }); + + // Vision types + py::class_(m_types, "Bbox2D") + .def(py::init<>()) + .def(py::init()) + .def(py::init(), + py::arg("top_left_corner"), py::arg("size"), + py::arg("timestamp") = 0.0, py::arg("label") = "") + .def_rw("top_left_corner", &Bbox2D::top_corner) + .def_rw("size", &Bbox2D::size) + .def_rw("timestamp", &Bbox2D::timestamp) + .def_rw("label", &Bbox2D::label) + .def_rw("img_size", &Bbox2D::img_size) + .def("set_vel", &Bbox2D::setVel) + .def("set_img_size", &Bbox2D::setImgSize); + + py::class_(m_types, "Bbox3D") + .def(py::init<>()) + .def(py::init()) + .def(py::init &>(), + py::arg("center"), py::arg("size"), py::arg("center_img_frame"), + py::arg("size_img_frame"), py::arg("timestamp") = 0.0, + py::arg("label") = "", py ::arg("pc_points") = py::list()) + .def_rw("center", &Bbox3D::center) + .def_rw("size", &Bbox3D::size) + .def_rw("center_img_frame", &Bbox3D::center_img_frame) + .def_rw("size_img_frame", &Bbox3D::size_img_frame) + .def_rw("pc_points", &Bbox3D::pc_points) + .def_rw("timestamp", &Bbox3D::timestamp) + .def_rw("label", &Bbox3D::label); } diff --git a/src/kompass_cpp/bindings/bindings_utils.cpp b/src/kompass_cpp/bindings/bindings_utils.cpp index 23c3398d..c66d405b 100644 --- a/src/kompass_cpp/bindings/bindings_utils.cpp +++ b/src/kompass_cpp/bindings/bindings_utils.cpp @@ -1,7 +1,7 @@ #include "utils/critical_zone_check.h" +#include #include #include -#include namespace py = nanobind; using namespace Kompass; @@ -16,11 +16,12 @@ void bindings_utils(py::module_ &m) { py::class_(m_utils, "CriticalZoneChecker") .def(py::init &, - const std::array &, const std::array &, - float, float>(), + const Eigen::Vector3f &, const Eigen::Vector4f &, const float, + const float, const float>(), py::arg("robot_shape"), py::arg("robot_dimensions"), py::arg("sensor_position_body"), py::arg("sensor_rotation_body"), - py::arg("critical_angle"), py::arg("critical_distance")) + py::arg("critical_angle"), py::arg("critical_distance"), + py::arg("slowdown_distance")) .def("check", &CriticalZoneChecker::check, py::arg("ranges"), py::arg("angles"), py::arg("forward")); diff --git a/src/kompass_cpp/kompass_cpp/CMakeLists.txt b/src/kompass_cpp/kompass_cpp/CMakeLists.txt index 82d8b43c..c8f69cd4 100644 --- a/src/kompass_cpp/kompass_cpp/CMakeLists.txt +++ b/src/kompass_cpp/kompass_cpp/CMakeLists.txt @@ -32,10 +32,10 @@ endif() # add project as library add_library(${MODULE_NAME} STATIC ${SOURCES}) target_include_directories(${MODULE_NAME} PUBLIC include) -target_include_directories(${MODULE_NAME} PUBLIC ${EIGEN3_INCLUDE_DIR} ${PCL_INCLUDE_DIRS}) - +target_include_directories(${MODULE_NAME} PUBLIC ${OMPL_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR} ${PCL_INCLUDE_DIRS}) +message(STATUS "OMPL_INCLUDE_DIR ${OMPL_INCLUDE_DIR}") # Link fcl and PCL -target_link_libraries(${MODULE_NAME} PRIVATE fcl ${PCL_LIBRARIES}) +target_link_libraries(${MODULE_NAME} PRIVATE fcl ${OMPL_LIBRARIES} ${PCL_LIBRARIES}) # add sycl if(AdaptiveCpp_FOUND) diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h index 7285d12e..852596ad 100644 --- a/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h +++ b/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h @@ -6,7 +6,10 @@ #include "datatypes/trajectory.h" #include "follower.h" #include "utils/cost_evaluator.h" +#include "utils/logger.h" #include "utils/trajectory_sampler.h" +#include +#include #include namespace Kompass { @@ -22,8 +25,8 @@ class DWA : public Follower { int maxLinearSamples, int maxAngularSamples, const CollisionChecker::ShapeType robotShapeType, const std::vector robotDimensions, - const std::array &sensor_position_body, - const std::array &sensor_rotation_body, const double octreeRes, + const Eigen::Vector3f &sensor_position_body, + const Eigen::Vector4f &sensor_rotation_body, const double octreeRes, CostEvaluator::TrajectoryCostsWeights costWeights, const int maxNumThreads = 1); @@ -31,8 +34,8 @@ class DWA : public Follower { ControlLimitsParams controlLimits, ControlType controlType, const CollisionChecker::ShapeType robotShapeType, const std::vector robotDimensions, - const std::array &sensor_position_body, - const std::array &sensor_rotation_body, + const Eigen::Vector3f &sensor_position_body, + const Eigen::Vector4f &sensor_rotation_body, CostEvaluator::TrajectoryCostsWeights costWeights, const int maxNumThreads = 1); @@ -48,30 +51,48 @@ class DWA : public Follower { */ // void reconfigure(DWAConfig &cfg); void configure(ControlLimitsParams controlLimits, ControlType controlType, - double timeStep, double predictionHorizon, - double controlHorizon, int maxLinearSamples, - int maxAngularSamples, - const CollisionChecker::ShapeType robotShapeType, - const std::vector robotDimensions, - const std::array &sensor_position_body, - const std::array &sensor_rotation_body, - const double octreeRes, - CostEvaluator::TrajectoryCostsWeights costWeights, - const int maxNumThreads = 1); + double timeStep, double predictionHorizon, + double controlHorizon, int maxLinearSamples, + int maxAngularSamples, + const CollisionChecker::ShapeType robotShapeType, + const std::vector robotDimensions, + const Eigen::Vector3f &sensor_position_body, + const Eigen::Vector4f &sensor_rotation_body, + const double octreeRes, + CostEvaluator::TrajectoryCostsWeights costWeights, + const int maxNumThreads = 1); void configure(TrajectorySampler::TrajectorySamplerParameters config, - ControlLimitsParams controlLimits, ControlType controlType, - const CollisionChecker::ShapeType robotShapeType, - const std::vector robotDimensions, - const std::array &sensor_position_body, - const std::array &sensor_rotation_body, - CostEvaluator::TrajectoryCostsWeights costWeights, - const int maxNumThreads = 1); + ControlLimitsParams controlLimits, ControlType controlType, + const CollisionChecker::ShapeType robotShapeType, + const std::vector robotDimensions, + const Eigen::Vector3f &sensor_position_body, + const Eigen::Vector4f &sensor_rotation_body, + CostEvaluator::TrajectoryCostsWeights costWeights, + const int maxNumThreads = 1); + /** + * @brief Reset the resolution of the robot's local map, this is equivalent to + * the box size representing each obstacle after conversion + * + * @param octreeRes + */ void resetOctreeResolution(const double octreeRes); + /** + * @brief Set the robot's sensor or local map max range (meters) + * + * @param max_range + */ void setSensorMaxRange(const float max_range); + /** + * @brief Set the Current State of the robot + * + * @param position + */ + void setCurrentState(const Path::State &position); + /** * @brief Adds a new custom cost to be used in the trajectory evaluation * @@ -91,13 +112,30 @@ class DWA : public Follower { template Controller::Result computeVelocityCommand(const Velocity2D &global_vel, - const T &scan_points); + const T &scan_points) { + TrajSearchResult searchRes = findBestPath(global_vel, scan_points); + Controller::Result finalResult; + if (searchRes.isTrajFound) { + finalResult.status = Controller::Result::Status::COMMAND_FOUND; + // Get the first command to be applied + finalResult.velocity_command = searchRes.trajectory.velocities.getFront(); + latest_velocity_command_ = finalResult.velocity_command; + } else { + finalResult.status = Controller::Result::Status::NO_COMMAND_POSSIBLE; + } + return finalResult; + }; + template TrajSearchResult computeVelocityCommandsSet(const Velocity2D &global_vel, - const LaserScan &scan); - TrajSearchResult - computeVelocityCommandsSet(const Velocity2D &global_vel, - const std::vector &cloud); + const T &scan_points) { + TrajSearchResult searchRes = findBestPath(global_vel, scan_points); + // Update latest velocity command + if (searchRes.isTrajFound) { + latest_velocity_command_ = searchRes.trajectory.velocities.getFront(); + } + return searchRes; + }; std::tuple getDebuggingSamples() const; @@ -123,13 +161,9 @@ class DWA : public Follower { global_vel, currentState, scan_points); }; -private: +protected: std::unique_ptr trajSampler; std::unique_ptr trajCostEvaluator; - double max_forward_distance_ = 0.0; - int maxNumThreads; - float maxLocalRange_ = 10.0; // Max range of the laserscan or the robot local map, default to 10 meters. Used to calculate the cost of coming close to obstacles - std::unique_ptr debuggingSamples_; /** * @brief Given the current position and velocity of the robot, find the @@ -142,9 +176,62 @@ class DWA : public Follower { */ template TrajSearchResult findBestPath(const Velocity2D &global_vel, - const T &scan_points); + const T &scan_points) { + // Throw an error if the global path is not set + if (!currentPath) { + throw std::invalid_argument("Pointer to global path is NULL. Cannot use " + "DWA local planner without " + "setting a global path"); + } + // find closest segment to use in cost computation + determineTarget(); + + if (rotate_in_place and currentTrackedTarget_->heading_error > + goal_orientation_tolerance * 10.0) { + // If the robot is rotating in place and the heading error is large, we + // do not need to sample trajectories + LOG_DEBUG("Rotating In Place ..."); + auto trajectory = trajSampler->generateSingleSampleFromVel( + Velocity2D(0.0, 0.0, + -currentTrackedTarget_->heading_error * + ctrlimitsParams.omegaParams.maxOmega / M_PI)); + return TrajSearchResult{trajectory, true, 0.0}; + } + + // Generate set of valid trajectories in the DW + std::unique_ptr samples_ = + trajSampler->generateTrajectories(global_vel, currentState, + scan_points); + if (samples_->size() == 0) { + return TrajSearchResult{Trajectory2D(), false, 0.0}; + } + + trajCostEvaluator->setPointScan(scan_points, currentState, maxLocalRange_); + + Path::Path trackedRefPathSegment = findTrackedPathSegment(); + + // Evaluate the samples and get the sample with the minimum cost + return trajCostEvaluator->getMinTrajectoryCost(samples_, currentPath.get(), + trackedRefPathSegment); + }; + +private: + double max_forward_distance_ = 0.0; + int maxNumThreads; + std::unique_ptr debuggingSamples_ = nullptr; + float maxLocalRange_ = + 10.0; // Max range of the robot sensor or local map in meters. Used to + // calculate the cost of coming close to obstacles + + /** + * @brief get maximum reference path length + */ + // size_t getMaxPathLength(); + size_t getMaxPathLength(); Path::Path findTrackedPathSegment(); + + void initJitCompile(); }; }; // namespace Control diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/follower.h b/src/kompass_cpp/kompass_cpp/include/controllers/follower.h index ea2421a8..e8298da9 100644 --- a/src/kompass_cpp/kompass_cpp/include/controllers/follower.h +++ b/src/kompass_cpp/kompass_cpp/include/controllers/follower.h @@ -48,7 +48,7 @@ class Follower : public Controller { // robot reached the goal point addParameter( "loosing_goal_distance", - Parameter(0.1, 0.001, 1000.0)); // [m] If driving past the goal + Parameter(0.5, 0.001, 1000.0)); // [m] If driving past the goal // we stop after this distance } }; @@ -82,7 +82,7 @@ class Follower : public Controller { * * @param path Global path to be followed */ - void setCurrentPath(const Path::Path &path); + void setCurrentPath(const Path::Path &path, const bool interpolate = true); void clearCurrentPath(); diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/rgb_follower.h b/src/kompass_cpp/kompass_cpp/include/controllers/rgb_follower.h new file mode 100644 index 00000000..6eb14930 --- /dev/null +++ b/src/kompass_cpp/kompass_cpp/include/controllers/rgb_follower.h @@ -0,0 +1,107 @@ +#pragma once + +#include "datatypes/control.h" +#include "datatypes/parameter.h" +#include "datatypes/tracking.h" +#include +#include +#include + +namespace Kompass { +namespace Control { + +class RGBFollower { +public: + class RGBFollowerConfig : public Parameters { + public: + RGBFollowerConfig() { + addParameter("control_time_step", + Parameter(0.1, 1e-4, 1e6, "Control time step (s)")); + addParameter("tolerance", Parameter(0.1, 0.0, 1.0, "Tolerance value")); + addParameter( + "target_distance", + Parameter( + 0.1, -1.0, 1e9, + "Target distance to maintain with the target (m)")); // Use -1 for + // None + // Search Parameters + addParameter("target_wait_timeout", Parameter(30.0, 0.0, 1e3)); + addParameter("target_search_timeout", Parameter(30.0, 0.0, 1e3)); + addParameter("target_search_radius", Parameter(0.5, 1e-4, 1e4)); + addParameter("target_search_pause", Parameter(1.0, 0.0, 1e3)); + // Pure tracking control law parameters + addParameter("rotation_gain", Parameter(1.0, 1e-2, 10.0)); + addParameter("speed_gain", Parameter(1.0, 1e-2, 10.0)); + addParameter("min_vel", Parameter(0.01, 1e-9, 1e9)); + addParameter("enable_search", Parameter(false)); + } + bool enable_search() const { return getParameter("enable_search"); } + double control_time_step() const { + return getParameter("control_time_step"); + } + double target_search_timeout() const { + return getParameter("target_search_timeout"); + } + double target_wait_timeout() const { + return getParameter("target_wait_timeout"); + } + double target_search_radius() const { + return getParameter("target_search_radius"); + } + double search_pause() const { + return getParameter("target_search_pause"); + } + double tolerance() const { return getParameter("tolerance"); } + double target_distance() const { + double val = getParameter("target_distance"); + return val < 0 ? -1.0 : val; // Return -1 for None + } + void set_target_distance(double value) { + setParameter("target_distance", value); + } + double K_omega() const { return getParameter("rotation_gain"); } + double K_v() const { return getParameter("speed_gain"); } + double min_vel() const { return getParameter("min_vel"); } + }; + + RGBFollower(const ControlType robotCtrlType, + const ControlLimitsParams ctrl_limits, + const RGBFollowerConfig config = RGBFollowerConfig()); + + // Default Destructor + ~RGBFollower() = default; + + void resetTarget(const Bbox2D &tracking); + + bool run(const std::optional tracking); + + const Velocities getCtrl() const; + + Eigen::Vector2f getErrors() const { + return Eigen::Vector2f(dist_error_, orientation_error_); + } + +protected: + bool is_diff_drive_; + ControlLimitsParams ctrl_limits_; + double recorded_search_time_ = 0.0, recorded_wait_time_ = 0.0; + std::queue> search_commands_queue_; + std::array search_command_; + std::unique_ptr last_tracking_ = nullptr; + float dist_error_ = 0.0f, orientation_error_ = 0.0f; + + void generateSearchCommands(float total_rotation, float search_radius, + float max_rotation_time, + bool enable_pause = false); + void getFindTargetCmds(const int last_direction = 1); + +private: + RGBFollowerConfig config_; + Velocities out_vel_; + + void trackTarget(const Bbox2D &tracking); + // +}; + +} // namespace Control +} // namespace Kompass diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h new file mode 100644 index 00000000..27b67a27 --- /dev/null +++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h @@ -0,0 +1,380 @@ +#pragma once + +#include "controllers/rgb_follower.h" +#include "datatypes/control.h" +#include "datatypes/parameter.h" +#include "datatypes/path.h" +#include "datatypes/tracking.h" +#include "datatypes/trajectory.h" +#include "dwa.h" +#include "utils/logger.h" +#include "vision/depth_detector.h" +#include "vision/tracker.h" +#include +#include +#include +#include +#include +#include + +namespace Kompass { +namespace Control { + +class VisionDWA : public DWA, public RGBFollower { +public: + class VisionDWAConfig : public RGBFollower::RGBFollowerConfig { + public: + VisionDWAConfig() : RGBFollower::RGBFollowerConfig() { + addParameter( + "control_horizon", + Parameter(2, 1, 1000, "Number of steps for applying the control")); + addParameter( + "prediction_horizon", + Parameter(10, 1, 1000, "Number of steps for future prediction")); + addParameter("distance_tolerance", + Parameter(0.1, 1e-6, 1e3, "Distance tolerance value (m)")); + addParameter("angle_tolerance", + Parameter(0.1, 1e-6, M_PI, "Angle tolerance value (rad)")); + addParameter( + "target_orientation", + Parameter(0.0, -M_PI, M_PI, + "Bearing angle to maintain with the target (rad)")); + addParameter( + "use_local_coordinates", + Parameter(true, + "Track the item in the local frame of the robot. This mode " + "cannot track the object velocity but can operate without " + "knowing the robot's absolute position (world frame)")); + addParameter("error_pose", Parameter(0.05, 1e-9, 1e9)); + addParameter("error_vel", Parameter(0.05, 1e-9, 1e9)); + addParameter("error_acc", Parameter(0.05, 1e-9, 1e9)); + + // Depth parameters + addParameter("depth_conversion_factor", + Parameter(1e-3, 1e-9, 1e9, + "Factor to convert depth image values to meters")); + addParameter( + "min_depth", + Parameter(0.0, 0.0, 1e3, "Range of interest minimum depth value")); + addParameter( + "max_depth", + Parameter(1e3, 1e-3, 1e9, "Range of interest minimum depth value")); + } + int control_horizon() const { return getParameter("control_horizon"); } + bool enable_vel_tracking() const { + return not getParameter("use_local_coordinates"); + } + int prediction_horizon() const { + return getParameter("prediction_horizon"); + } + double dist_tolerance() const { + return getParameter("distance_tolerance"); + } + double ang_tolerance() const { + return getParameter("angle_tolerance"); + } + double target_orientation() const { + return getParameter("target_orientation"); + } + double e_pose() const { return getParameter("error_pose"); } + double e_vel() const { return getParameter("error_vel"); } + double e_acc() const { return getParameter("error_acc"); } + double depth_conversion_factor() const { + return getParameter("depth_conversion_factor"); + } + Eigen::Vector2f depth_range() const { + return Eigen::Vector2f{getParameter("min_depth"), + getParameter("max_depth")}; + } + }; + + VisionDWA(const ControlType &robotCtrlType, + const ControlLimitsParams &ctrlLimits, const int maxLinearSamples, + const int maxAngularSamples, + const CollisionChecker::ShapeType &robotShapeType, + const std::vector &robotDimensions, + const Eigen::Vector3f &proximity_sensor_position_body, + const Eigen::Vector4f &proximity_sensor_rotation_body, + const Eigen::Vector3f &vision_sensor_position_body, + const Eigen::Vector4f &vision_sensor_rotation_body, + const double octreeRes, + const CostEvaluator::TrajectoryCostsWeights &costWeights, + const int maxNumThreads = 1, + const VisionDWAConfig &config = VisionDWAConfig()); + + // Default Destructor + ~VisionDWA() = default; + + void setCameraIntrinsics(const float focal_length_x, + const float focal_length_y, + const float principal_point_x, + const float principal_point_y); + + /** + * @brief Get the Tracking Control Result based on object tracking and DWA + * sampling by searching a set of given detections + * + * @tparam T + * @param tracking_pose + * @param current_vel + * @param sensor_points + * @return Control::TrajSearchResult + */ + template + Control::TrajSearchResult + getTrackingCtrl(const std::vector &detected_boxes, + const Velocity2D ¤t_vel, const T &sensor_points) { + std::optional tracked_pose = std::nullopt; + if (!detected_boxes.empty()) { + if (tracker_->trackerInitialized()) { + // Update the tracker with the detected boxes + bool tracking_updated = tracker_->updateTracking(detected_boxes); + if (!tracking_updated) { + LOG_WARNING( + "Tracker failed to update target with the detected boxes"); + } else { + tracked_pose = tracker_->getFilteredTrackedPose2D(); + } + } else { + throw std::runtime_error( + "Tracker is not initialized with an initial tracking target. Call " + "'VisionDWA::setInitialTracking' first"); + } + } + return this->getTrackingCtrl(tracked_pose, current_vel, sensor_points); + }; + + template + Control::TrajSearchResult + getTrackingCtrl(const Eigen::MatrixX &aligned_depth_img, + const std::vector &detected_boxes_2d, + const Velocity2D ¤t_vel, const T &sensor_points) { + if (!detector_) { + throw std::runtime_error( + "DepthDetector is not initialized with the camera intrinsics. Call " + "'VisionDWA::setCameraIntrinsics' first"); + } + if (!tracker_->trackerInitialized()) { + throw std::runtime_error( + "Tracker is not initialized with an initial tracking target. Call " + "'VisionDWA::setInitialTracking' first"); + } + std::optional tracked_pose = std::nullopt; + if (!detected_boxes_2d.empty()) { + if (track_velocity_) { + // Send current state to the detector + detector_->updateBoxes(aligned_depth_img, detected_boxes_2d, + currentState); + } else { + detector_->updateBoxes(aligned_depth_img, detected_boxes_2d); + } + auto boxes_3d = detector_->get3dDetections(); + if (boxes_3d) { + // Update the tracker with the detected boxes + bool tracking_updated = tracker_->updateTracking(boxes_3d.value()); + if (!tracking_updated) { + LOG_WARNING( + "Tracker failed to update target with the detected boxes"); + } else { + tracked_pose = tracker_->getFilteredTrackedPose2D(); + } + } else { + LOG_WARNING("Detector failed to find 3D boxes"); + } + } + return this->getTrackingCtrl(tracked_pose, current_vel, sensor_points); + }; + + /** + * @brief Set the initial image position of the target to be tracked + * + * @param pose_x_img + * @param pose_y_img + * @param detected_boxes + * @return true + * @return false + */ + bool setInitialTracking(const int pose_x_img, const int pose_y_img, + const std::vector &detected_boxes, + const float yaw = 0.0); + + /** + * @brief Set the initial image position of the target to be tracked using 2D + * detections + * + * @param pose_x_img + * @param pose_y_img + * @param detected_boxes_2d + * @return true + * @return false + */ + bool + setInitialTracking(const int pose_x_img, const int pose_y_img, + const Eigen::MatrixX &aligned_depth_image, + const std::vector &detected_boxes_2d, + const float yaw = 0.0); + + bool + setInitialTracking(const Eigen::MatrixX &aligned_depth_image, + const Bbox2D &target_box_2d, const float yaw = 0.0); + + Eigen::Vector2f getErrors() const { + return Eigen::Vector2f(dist_error_, orientation_error_); + } + +private: + VisionDWAConfig config_; + std::unique_ptr tracker_; + std::unique_ptr detector_; + Eigen::Isometry3f vision_sensor_tf_; + int track_velocity_; + + /** + * @brief Get the Tracking Reference Trajectory Segment + * + * @tparam T + * @param tracking_pose + * @return std::tuple + */ + Trajectory2D getTrackingReferenceSegment(const TrackedPose2D &tracking_pose); + + Trajectory2D + getTrackingReferenceSegmentDiffDrive(const TrackedPose2D &tracking_pose); + + /** + * @brief Get the Pure Tracking Control Command + * + * @param tracking_pose + * @return Velocity2D + */ + Velocity2D getPureTrackingCtrl(const TrackedPose2D &tracking_pose, + const bool update_global_error = false); + + /** + * @brief Get the Tracking Control Result based on object tracking and DWA + * sampling by direct following of the tracked target in local frame + * + * @tparam T + * @param tracking_pose + * @param current_vel + * @param sensor_points + * @return Control::TrajSearchResult + */ + template + Control::TrajSearchResult + getTrackingCtrl(const std::optional &tracked_pose, + const Velocity2D ¤t_vel, const T &sensor_points) { + if (tracked_pose.has_value()) { + // Reset recorded wait and search times + recorded_wait_time_ = 0.0; + recorded_search_time_ = 0.0; + LOG_INFO("Tracking target at position: ", tracked_pose->x(), ", ", + tracked_pose->y(), " with velocity: ", tracked_pose->v(), ", ", + tracked_pose->omega()); + LOG_INFO("Robot current position: ", currentState.x, ", ", + currentState.y); + // Generate reference to target + Trajectory2D ref_traj; + if (is_diff_drive_) { + ref_traj = getTrackingReferenceSegmentDiffDrive(tracked_pose.value()); + } else { + ref_traj = getTrackingReferenceSegment(tracked_pose.value()); + } + + TrajSearchResult result; + result.isTrajFound = true; + result.trajCost = 0.0; + result.trajectory = ref_traj; + latest_velocity_command_ = ref_traj.velocities.getFront(); + // --------------------------------------------------------------- + // Update reference to use in case goal is lost + auto referenceToTarget = + Path::Path(ref_traj.path.x, ref_traj.path.y, ref_traj.path.z, + config_.prediction_horizon()); + this->setCurrentPath(referenceToTarget, false); + // --------------------------------------------------------------- + return result; + } + if (this->hasPath() and !isGoalReached() and + this->currentPath->getSize() > 1) { + // The tracking sample has collisions -> use DWA-like sampling and control + return this->computeVelocityCommandsSet(current_vel, sensor_points); + } else { + // Start Search and/or Wait if enabled + if (config_.enable_search()) { + if (recorded_search_time_ < config_.target_search_timeout()) { + if (search_commands_queue_.empty()) { + LOG_DEBUG("Search commands queue is empty, generating new search " + "commands"); + int last_direction = 1; + if (latest_velocity_command_.omega() < 0) { + last_direction = -1; + } + getFindTargetCmds(last_direction); + } + LOG_DEBUG("Number of search commands remaining: ", + search_commands_queue_.size(), + "recorded search time: ", recorded_search_time_); + // Create search command + TrajectoryVelocities2D velocities(config_.control_horizon()); + TrajectoryPath path(config_.control_horizon()); + path.add(0, 0.0, 0.0); + std::array search_command; + for (int i = 0; i < config_.control_horizon() - 1; i++) { + if (search_commands_queue_.empty()) { + LOG_DEBUG("Search commands queue is empty. Ending Search "); + return TrajSearchResult(); + } + search_command = search_commands_queue_.front(); + search_commands_queue_.pop(); + recorded_search_time_ += config_.control_time_step(); + path.add(i + 1, 0.0, 0.0); + velocities.add(i, Velocity2D(search_command[0], search_command[1], + search_command[2])); + } + LOG_DEBUG("Sending ", config_.control_horizon(), + " search commands " + "to the controller"); + auto result = TrajSearchResult(); + result.isTrajFound = true; + result.trajCost = 0.0; + result.trajectory = Trajectory2D(velocities, path); + return result; + } + } else { + if (recorded_wait_time_ < config_.target_wait_timeout()) { + auto timeout = config_.target_wait_timeout() - recorded_wait_time_; + LOG_DEBUG("Target lost, waiting to get tracked target again ... " + "timeout in ", + timeout, " seconds"); + // Do nothing and wait + TrajectoryVelocities2D velocities(config_.control_horizon()); + TrajectoryPath path(config_.control_horizon()); + path.add(0, 0.0, 0.0); + for (int i = 0; i < config_.control_horizon() - 1; i++) { + recorded_wait_time_ += config_.control_time_step(); + velocities.add(i, Velocity2D(0.0, 0.0, 0.0)); + path.add(i + 1, 0.0, 0.0); + } + auto result = TrajSearchResult(); + result.isTrajFound = true; + result.trajCost = 0.0; + result.trajectory = Trajectory2D(velocities, path); + return result; + } + } + // Target is lost and not recovered from search or wait + LOG_WARNING("Target is lost and not recovered from search or wait"); + recorded_wait_time_ = 0.0; + recorded_search_time_ = 0.0; + // Empty the search commands queue + while (!search_commands_queue_.empty()) { + search_commands_queue_.pop(); + } + return TrajSearchResult(); + } + }; +}; + +} // namespace Control +} // namespace Kompass diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_follower.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_follower.h deleted file mode 100644 index db31c703..00000000 --- a/src/kompass_cpp/kompass_cpp/include/controllers/vision_follower.h +++ /dev/null @@ -1,105 +0,0 @@ -#pragma once - -#include "controller.h" -#include "datatypes/control.h" -#include "datatypes/parameter.h" -#include -#include -#include - -namespace Kompass { -namespace Control { - -class VisionFollower : public Controller { -public: - struct TrackingData { - std::array size_xy; // width and height of the bounding box - int img_width; - int img_height; - std::array - center_xy; // x, y coordinates of the object center in image frame - double depth; // -1 is equivalent to none - }; - - class VisionFollowerConfig : public ControllerParameters { - public: - VisionFollowerConfig() : ControllerParameters() { - addParameter("control_time_step", Parameter(0.1, 1e-4, 1e6)); - addParameter("control_horizon", Parameter(2, 1, 1000)); - addParameter("tolerance", Parameter(0.1, 1e-6, 1e3)); - addParameter("target_distance", - Parameter(-1.0, -1.0, 1e9)); // Use -1 for None - addParameter("target_wait_timeout", Parameter(30.0, 0.0, 1e3)); - addParameter("target_search_timeout", Parameter(30.0, 0.0, 1e3)); - addParameter("target_search_radius", Parameter(0.5, 1e-4, 1e4)); - addParameter("target_search_pause", Parameter(1.0, 0.0, 1e3)); - addParameter("rotation_multiple", Parameter(1.0, 1e-2, 10.0)); - addParameter("speed_depth_multiple", Parameter(1.0, 1e-2, 10.0)); - addParameter("min_vel", Parameter(0.1, 1e-9, 1e9)); - addParameter("enable_search", Parameter(false)); - } - bool enable_search() const { return getParameter("enable_search"); } - double control_time_step() const { - return getParameter("control_time_step"); - } - double target_search_timeout() const { - return getParameter("target_search_timeout"); - } - double target_wait_timeout() const { - return getParameter("target_wait_timeout"); - } - double target_search_radius() const { - return getParameter("target_search_radius"); - } - double search_pause() const { - return getParameter("target_search_pause"); - } - int control_horizon() const { return getParameter("control_horizon"); } - double tolerance() const { return getParameter("tolerance"); } - double target_distance() const { - double val = getParameter("target_distance"); - return val < 0 ? -1.0 : val; // Return -1 for None - } - void set_target_distance(double value) { - setParameter("target_distance", value); - } - double alpha() const { return getParameter("rotation_multiple"); } - double beta() const { return getParameter("speed_depth_multiple"); } - double min_vel() const { return getParameter("min_vel"); } - }; - - VisionFollower(const ControlType robotCtrlType, - const ControlLimitsParams ctrl_limits, - const VisionFollowerConfig config = VisionFollowerConfig()); - - // Default Destructor - ~VisionFollower() = default; - - void resetTarget(const TrackingData tracking); - - bool run(const std::optional tracking); - - const Velocities getCtrl() const; - -private: - ControlType _ctrlType; - ControlLimitsParams _ctrl_limits; - VisionFollowerConfig _config; - - bool _rotate_in_place; - Velocities _out_vel; - double _recorded_search_time = 0.0, _recorded_wait_time = 0.0; - std::queue> _search_commands_queue; - std::array _search_command; - std::unique_ptr _last_tracking = nullptr; - - void generateSearchCommands(double total_rotation, double search_radius, - int max_rotation_steps, - bool enable_pause = false); - void getFindTargetCmds(); - void trackTarget(const TrackingData &tracking); - // -}; - -} // namespace Control -} // namespace Kompass diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/control.h b/src/kompass_cpp/kompass_cpp/include/datatypes/control.h index 9c2a52c4..93003c51 100644 --- a/src/kompass_cpp/kompass_cpp/include/datatypes/control.h +++ b/src/kompass_cpp/kompass_cpp/include/datatypes/control.h @@ -12,6 +12,104 @@ namespace Control { // Enumeration for control modes enum class ControlType { ACKERMANN = 0, DIFFERENTIAL_DRIVE = 1, OMNI = 2 }; +class Pose3D { + +public: + Pose3D(const Eigen::Vector3f &position, const Eigen::Vector4f &orientation) + : position_(position), orientation_(orientation), + rotation_matrix_(orientation_.toRotationMatrix()) {}; + + Pose3D(const Eigen::Vector3f &position, const Eigen::Quaternionf &orientation) + : position_(position), orientation_(orientation), + rotation_matrix_(orientation.toRotationMatrix()) {}; + + /** + * @brief Construct a new Pose3D object using 2D pose information + * + * @param pose_x + * @param pose_y + * @param pose_yaw + */ + Pose3D(const float &pose_x, const float &pose_y, const float &pose_yaw) { + update(pose_x, pose_y, pose_yaw); + }; + + void setFrame(const std::string &frame_id) { frame_id_ = frame_id; } + + std::string getFrame() const { return frame_id_; } + + bool inFrame(const std::string &frame_id) const { + return frame_id == frame_id_; + } + + float norm() const { return position_.norm(); }; + + /** + * @brief Extract x coordinates + * + * @return float + */ + float x() const { return position_(0); }; + + /** + * @brief Extract y coordinates + * + * @return float + */ + float y() const { return position_(1); }; + + /** + * @brief Extract z coordinates + * + * @return float + */ + float z() const { return position_(2); }; + + /** + * @brief Extract pitch (y-axis rotation) from the rotation matrix + * + * @return float + */ + float pitch() const { return std::asin(rotation_matrix_(2, 0)); }; + + /** + * @brief Extract roll (x-axis rotation) from the rotation matrix + * + * @return float + */ + float roll() const { + return std::atan2(rotation_matrix_(2, 1), rotation_matrix_(2, 2)); + }; + + /** + * @brief Extract yaw (x-axis rotation) from the rotation matrix + * + * @return float + */ + float yaw() const { + return std::atan2(rotation_matrix_(1, 0), rotation_matrix_(0, 0)); + }; + + void update(const float &pose_x, const float &pose_y, const float &pose_yaw) { + position_ = {pose_x, pose_y, 0.0}; + setRotation(0.0, 0.0, pose_yaw); + } + + void setRotation(const float pitch, const float roll, const float yaw) { + Eigen::AngleAxisf rotZ(yaw, Eigen::Vector3f::UnitZ()); + Eigen::AngleAxisf rotY(pitch, Eigen::Vector3f::UnitY()); + Eigen::AngleAxisf rotX(roll, Eigen::Vector3f::UnitX()); + orientation_ = rotZ * rotY * rotX; + rotation_matrix_ = orientation_.toRotationMatrix(); + } + +protected: + Eigen::Vector3f position_; + Eigen::Quaternionf orientation_; + Eigen::Matrix3f rotation_matrix_; + std::string frame_id_; +}; + class Velocity2D { public: // Default constructor @@ -33,10 +131,63 @@ class Velocity2D { void setOmega(double const value) { velocity_(2) = value; } void setSteerAng(double const value) { velocity_(3) = value; } + // Overload the unary minus operator + Velocity2D operator-() const { + return Velocity2D(-velocity_(0), -velocity_(1), -velocity_(2)); + } + private: Eigen::Vector4d velocity_{0.0, 0.0, 0.0, 0.0}; }; +class TrackedPose2D : public Pose3D { + +public: + TrackedPose2D(const Eigen::Vector3f &position, + const Eigen::Vector4f &orientation, const Velocity2D &vel) + : Pose3D(position, orientation), vel_(vel) {}; + + TrackedPose2D(const Eigen::Vector3f &position, + const Eigen::Quaternionf &orientation, const Velocity2D &vel) + : Pose3D(position, orientation), vel_(vel) {}; + + TrackedPose2D(const float &pose_x, const float &pose_y, const float &pose_yaw, + const Velocity2D &vel) + : Pose3D(pose_x, pose_y, pose_yaw), vel_(vel) {}; + + TrackedPose2D(const float &pose_x, const float &pose_y, const float &pose_yaw, + const float &vx, const float &vy, const float &omega) + : Pose3D(pose_x, pose_y, pose_yaw), vel_(vx, vy, omega) {}; + + float v() const { return Eigen::Vector2f{vel_.vx(), vel_.vy()}.norm(); }; + + float omega() const { return vel_.omega(); }; + + void update(const float timeStep) { + position_(0) += + (vel_.vx() * cos(this->yaw()) - vel_.vy() * sin(this->yaw())) * + timeStep; + position_(1) += + (vel_.vx() * sin(this->yaw()) + vel_.vy() * cos(this->yaw())) * + timeStep; + float yaw = this->yaw() + vel_.omega() * timeStep; + setRotation(0.0, 0.0, yaw); + } + + void update(const Velocity2D &vel, const float timeStep) { + vel_ = vel; + update(timeStep); + } + + float distance(const float x, const float y, const float z = 0.0) const { + return sqrt(pow(position_.x() - x, 2) + pow(position_.y() - y, 2) + + pow(position_.z() - z, 2)); + } + +protected: + Velocity2D vel_; +}; + struct Velocities { std::vector vx; // Speed on x-asix (m/s) std::vector vy; @@ -85,22 +236,28 @@ struct Velocities { // Structure for Forward Linear Velocity Control parameters struct LinearVelocityControlParams { - double maxVel; // Maximum allowed speed - double maxAcceleration; // Maximum acceleration in units per second squared - double maxDeceleration; // Maximum deceleration in units per second squared + double maxVel = 1.0; + double maxAcceleration = 10.0; + double maxDeceleration = 10.0; + + LinearVelocityControlParams(const LinearVelocityControlParams &) = default; + // Parameterized constructor LinearVelocityControlParams(double maxVel = 1.0, double maxAcc = 10.0, double maxDec = 10.0) : maxVel(maxVel), maxAcceleration(maxAcc), maxDeceleration(maxDec) {} }; -// Structure for Forward Linear Velocity Control parameters +// Structure for Angular Velocity Control parameters struct AngularVelocityControlParams { - double maxAngle; // Maximum allowed steering angle - double maxOmega; - double maxAcceleration; // Maximum acceleration in units per second squared - double maxDeceleration; // Maximum deceleration in units per second squared + double maxAngle = M_PI; + double maxOmega = 1.0; + double maxAcceleration = 10.0; + double maxDeceleration = 10.0; + AngularVelocityControlParams(const AngularVelocityControlParams &) = default; + + // Parameterized constructor AngularVelocityControlParams(double maxAng = M_PI, double maxOmg = 1.0, double maxAcc = 10.0, double maxDec = 10.0) : maxAngle(maxAng), maxOmega(maxOmg), maxAcceleration(maxAcc), @@ -113,11 +270,13 @@ struct ControlLimitsParams { LinearVelocityControlParams velYParams; AngularVelocityControlParams omegaParams; - ControlLimitsParams( - LinearVelocityControlParams velXCtrParams = LinearVelocityControlParams(), - LinearVelocityControlParams velYCtrParams = LinearVelocityControlParams(), - AngularVelocityControlParams omegaCtrParams = - AngularVelocityControlParams()) + ControlLimitsParams() = default; + ControlLimitsParams(const ControlLimitsParams &) = default; + + // Parameterized constructor + ControlLimitsParams(const LinearVelocityControlParams &velXCtrParams, + const LinearVelocityControlParams &velYCtrParams, + const AngularVelocityControlParams &omegaCtrParams) : velXParams(velXCtrParams), velYParams(velYCtrParams), omegaParams(omegaCtrParams) {} }; diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/path.h b/src/kompass_cpp/kompass_cpp/include/datatypes/path.h index d33e1f71..b7702505 100644 --- a/src/kompass_cpp/kompass_cpp/include/datatypes/path.h +++ b/src/kompass_cpp/kompass_cpp/include/datatypes/path.h @@ -1,10 +1,9 @@ #pragma once +#include "datatypes/control.h" #include "utils/spline.h" #include #include -#include -#include #include namespace Path { @@ -20,7 +19,15 @@ struct State { State(double poseX = 0.0, double poseY = 0.0, double PoseYaw = 0.0, double speedValue = 0.0) - : x(poseX), y(poseY), yaw(PoseYaw), speed(speedValue) {} + : x(poseX), y(poseY), yaw(PoseYaw), speed(speedValue) {}; + + void update(const Kompass::Control::Velocity2D &vel, const float timeStep) { + this->x += + (vel.vx() * cos(this->yaw) - vel.vy() * sin(this->yaw)) * timeStep; + this->y += + (vel.vx() * sin(this->yaw) + vel.vy() * cos(this->yaw)) * timeStep; + this->yaw += vel.omega() * timeStep; + }; }; // Point in 3D space @@ -28,36 +35,47 @@ typedef Eigen::Vector3f Point; // Structure for Path Control parameters struct Path { - std::vector points; // Vector of points defining the path std::vector segments; // List of path segments - tk::spline *_spline; // get all x values from points - const std::vector& getX() const; + const Eigen::VectorXf getX() const; // get all y values from points - const std::vector& getY() const; + const Eigen::VectorXf getY() const; // get all z values from points - const std::vector& getZ() const; - // Max interpolation distance and total path distance are updated from user - // config - double _max_interpolation_dist{0.0}, _max_path_length{10.0}; + const Eigen::VectorXf getZ() const; // Max segment size and max total path points size is calculated after // interpolation int max_segment_size{10}; - size_t max_size{10}; - size_t max_interpolation_iterations{500}; // Max number of iterations for interpolation between two path points - Path(const std::vector &points = {}); + Path(const Path &other) = default; + + Path(const std::vector &points = {}, const size_t new_max_size = 10); + + Path(const Eigen::VectorXf &x_points, const Eigen::VectorXf &y_points, + const Eigen::VectorXf &z_points, const size_t new_max_size = 10); size_t getMaxNumSegments(); void setMaxLength(double max_length); + void resize(const size_t max_new_size); + bool endReached(State currentState, double minDist); Point getEnd() const; Point getStart() const; + Point getIndex(const size_t index) const; + + Path getPart(const size_t start, const size_t end, + const size_t max_part_size = 0) const; + + void pushPoint(const Point &point); + + size_t getSize() const; + + size_t getMaxSize() const; + float getEndOrientation() const; float getStartOrientation() const; @@ -84,10 +102,44 @@ struct Path { // Segment using a segment points number void segmentByPointsNumber(int segmentLength); + struct Iterator { + using iterator_category = std::forward_iterator_tag; + using value_type = Point; + using difference_type = std::ptrdiff_t; + + Iterator(const Path &p, size_t idx) : path(p), index(idx) {} + + Point operator*() const { return path.getIndex(index); } + + Iterator &operator++() { + ++index; + return *this; + } + + bool operator!=(const Iterator &other) const { + return index != other.index; + } + + private: + const Path &path; + size_t index; + }; + + Iterator begin() const { return Iterator(*this, 0); } + Iterator end() const { return Iterator(*this, current_size_); } + private: - std::vector X_; // Vector of X coordinates - std::vector Y_; // Vector of Y coordinates - std::vector Z_; // Vector of Z coordinates + Eigen::VectorXf X_; // Vector of X coordinates + Eigen::VectorXf Y_; // Vector of Y coordinates + Eigen::VectorXf Z_; // Vector of Z coordinates + size_t current_size_{0}; // Current size of the path + size_t max_interpolation_iterations_; // Max number of iterations for + // interpolation between two path points + // Max interpolation distance and total path distance are updated from user + // config + float max_path_length_{10.0}, max_interpolation_dist_{0.0}; + tk::spline *spline_; + size_t max_size_{10}; }; struct PathPosition { diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h b/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h new file mode 100644 index 00000000..244c981d --- /dev/null +++ b/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h @@ -0,0 +1,202 @@ +#pragma once + +#include "datatypes/control.h" +#include "utils/logger.h" +#include +#include +#include +#include + +namespace Kompass { + +struct Bbox2D { + Eigen::Vector2i top_corner = {0, 0}; + Eigen::Vector2i size = {0, 0}; + float timestamp = 0.0; // Timestamp of the detection in seconds + std::string label = ""; // Label of the detection, e.g. "car", "pedestrian" + Eigen::Vector2i img_size = {640, 480}; // Size of the image frame + Eigen::Vector3f vel = {0.0, 0.0, 0.0}; + + Bbox2D(){}; + + Bbox2D(const Bbox2D &box) + : top_corner(box.top_corner), size(box.size), timestamp(box.timestamp), + label(box.label), img_size(box.img_size){}; + + Bbox2D(const Eigen::Vector2i top_corner, const Eigen::Vector2i size, + const float timestamp = 0.0, const std::string &label = "", + const Eigen::Vector2i img_size = {640, 480}) + : top_corner(top_corner), size(size), timestamp(timestamp), label(label), + img_size(img_size) { + if (size.x() <= 0 || size.y() <= 0) { + throw std::invalid_argument("Invalid bounding box size"); + } + if (img_size.x() <= 0 || img_size.y() <= 0) { + throw std::invalid_argument("Invalid image size"); + } + }; + + Eigen::Vector2i getXLimits() const { + return {top_corner.x(), top_corner.x() + size.x()}; + }; + + Eigen::Vector2i getYLimits() const { + return {top_corner.y(), top_corner.y() + size.y()}; + }; + + Eigen::Vector2i getCenter() const { + return {top_corner.x() + size.x() / 2, top_corner.y() + size.y() / 2}; + }; + + void setImgSize(const Eigen::Vector2i &size) { + if (size.x() <= 0 || size.y() <= 0) { + throw std::invalid_argument("Invalid image size"); + } + this->img_size = size; + }; + + void setVel(const Eigen::Vector3f &vel) { this->vel = vel; }; +}; + +struct Bbox3D { + Eigen::Vector3f center = {0.0, 0.0, 0.0}; + Eigen::Vector3f size = {0.0, 0.0, 0.0}; + Eigen::Vector2i center_img_frame = {0, 0}; + Eigen::Vector2i size_img_frame = {0, 0}; + std::vector pc_points = {}; + float timestamp = 0.0; // Timestamp of the detection in seconds + std::string label = ""; + + Bbox3D(){}; + + Bbox3D(const Bbox3D &box) + : center(box.center), size(box.size), + center_img_frame(box.center_img_frame), + size_img_frame(box.size_img_frame), pc_points(box.pc_points), + timestamp(box.timestamp), label(box.label){}; + + Bbox3D(const Eigen::Vector3f ¢er, const Eigen::Vector3f &size, + const Eigen::Vector2i center_img_frame, + const Eigen::Vector2i size_img_frame, const float timestamp = 0.0, + const std::string &label = "", + const std::vector pc_points = {}) + : center(center), size(size), center_img_frame(center_img_frame), + size_img_frame(size_img_frame), pc_points(pc_points), + timestamp(timestamp), label(label){}; + + Bbox3D(const Eigen::Vector3f ¢er, const Eigen::Vector3f &size, + const Bbox2D &box2d, std::vector pc_points = {}) + : center(center), size(size), + center_img_frame( + box2d.top_corner + + Eigen::Vector2i{box2d.size.x() / 2, box2d.size.y() / 2}), + size_img_frame(box2d.size), pc_points(pc_points), + timestamp(box2d.timestamp), label(box2d.label){}; + + Bbox3D(const Bbox2D &box2d) + : center_img_frame( + box2d.top_corner + + Eigen::Vector2i{box2d.size.x() / 2, box2d.size.y() / 2}), + size_img_frame(box2d.size), timestamp(box2d.timestamp), + label(box2d.label){}; + + Eigen::Vector2f getXLimitsImg() const { + return {center_img_frame.x() - (size_img_frame.x() / 2), + center_img_frame.x() + (size_img_frame.x() / 2)}; + }; + + Eigen::Vector2f getYLimitsImg() const { + return {center_img_frame.y() - (size_img_frame.y() / 2), + center_img_frame.y() + (size_img_frame.y() / 2)}; + }; +}; + +struct TrackedBbox3D { + Bbox3D box; + Eigen::Vector3f vel = {0.0, 0.0, 0.0}; + Eigen::Vector3f acc = {0.0, 0.0, 0.0}; + int unique_id = 0; + + TrackedBbox3D(const Bbox3D &box) : box(box){}; + + void setState(const Eigen::Matrix &state_vector) { + this->box.center = {state_vector(0), state_vector(1), 0.0f}; + this->vel = {state_vector(3), state_vector(4), 0.0f}; + this->acc = {state_vector(6), state_vector(7), 0.0f}; + }; + + void setSize(const Eigen::Vector3f &size) { this->box.size = size; }; + + void setfromBox(const Bbox3D &box) { this->box = box; }; + + void updateFromNewDetection(const Bbox3D &new_box) { + if (new_box.label != this->box.label) { + LOG_ERROR("Box label mismatch, cannot update tracking."); + return; + } + float time_step = new_box.timestamp - this->box.timestamp; + Eigen::Vector3f new_vel; + if (time_step <= 0.0) { + LOG_ERROR( + "Box updated with invalid time step, Velocity wil be reset to zero."); + this->vel = {0.0, 0.0, 0.0}; + this->acc = {0.0, 0.0, 0.0}; + } else { + // Compute velocity and acceleration based on location change + Eigen::Vector3f new_vel = (new_box.center - this->box.center) / time_step; + this->acc = (new_vel - this->vel) / time_step; + this->vel = new_vel; + } + // Update + setfromBox(new_box); + } + + TrackedBbox3D predictConstantVel(const float &dt) { + auto predicted_tracking = TrackedBbox3D(*this); + predicted_tracking.box.center += predicted_tracking.vel * dt; + // Set acceleration to zero for constant velocity prediction + predicted_tracking.acc = {0.0, 0.0, 0.0}; + predicted_tracking.box.timestamp += dt; + return predicted_tracking; + }; + + TrackedBbox3D predictConstantAcc(const float &dt) { + auto predicted_tracking = TrackedBbox3D(*this); + predicted_tracking.vel += this->acc * dt; + predicted_tracking.box.center += predicted_tracking.vel * dt; + predicted_tracking.box.timestamp += dt; + return predicted_tracking; + }; + + float v() const { return Eigen::Vector2f{vel.x(), vel.y()}.norm(); }; + + float x() const { return box.center.x(); }; + + float y() const { return box.center.y(); }; + + float yaw() const { return std::atan2(this->vel(1), this->vel(0)); }; + + float omega() const { return 0.0; } + + float ang_acc() const { return 0.0; } + + float timestamp() const { return box.timestamp; }; + + void update(const float timeStep) { + box.center(0) += vel.x() * timeStep; + box.center(1) += vel.y() * timeStep; + box.timestamp += timeStep; + } + + float distance(const float x, const float y, const float z = 0.0) const { + return sqrt(pow(box.center.x() - x, 2) + pow(box.center.y() - y, 2) + + pow(box.center.z() - z, 2)); + } + + Control::TrackedPose2D getTrackedPose() const { + return Control::TrackedPose2D(box.center.x(), box.center.y(), yaw(), + vel.x(), vel.y(), 0.0); + } +}; + +} // namespace Kompass diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/trajectory.h b/src/kompass_cpp/kompass_cpp/include/datatypes/trajectory.h index 9cc9b619..ae79fc2e 100644 --- a/src/kompass_cpp/kompass_cpp/include/datatypes/trajectory.h +++ b/src/kompass_cpp/kompass_cpp/include/datatypes/trajectory.h @@ -90,7 +90,8 @@ struct TrajectoryVelocities2D { // get the first element Velocity2D getFront() const { assert(numPointsPerTrajectory_ > 1 && "Velocities are empty"); - return Velocity2D(vx(0), vy(0), omega(0)); }; + return Velocity2D(vx(0), vy(0), omega(0)); + }; // get the last element Velocity2D getEnd() const { assert(numPointsPerTrajectory_ > 1 && "Velocities are empty"); @@ -148,14 +149,15 @@ struct TrajectoryPath { // initialize from a Path explicit TrajectoryPath(const Path::Path &path) { - x.resize(path.points.size()); - y.resize(path.points.size()); - z.resize(path.points.size()); - numPointsPerTrajectory_ = path.points.size(); - for (size_t i = 0; i < path.points.size(); ++i) { - x(i) = path.points[i].x(); - y(i) = path.points[i].y(); - z(i) = path.points[i].z(); + x.resize(path.getSize()); + y.resize(path.getSize()); + z.resize(path.getSize()); + numPointsPerTrajectory_ = path.getSize(); + // Set the current path points + for (size_t i = 0; i < path.getSize(); ++i) { + x(i) = path.getIndex(i).x(); + y(i) = path.getIndex(i).y(); + z(i) = path.getIndex(i).z(); } }; @@ -210,7 +212,8 @@ struct TrajectoryPath { // get the first element Path::Point getFront() const { assert(numPointsPerTrajectory_ > 0 && "Path is empty"); - return Path::Point(x(0), y(0), z(0)); }; + return Path::Point(x(0), y(0), z(0)); + }; // get the last element Path::Point getEnd() const { assert(numPointsPerTrajectory_ > 0 && "Path is empty"); @@ -278,7 +281,7 @@ struct Trajectory2D { // initialize with path object and velocity vector explicit Trajectory2D(std::vector &velocities, Path::Path &path) { - if (velocities.size() != path.points.size()) { + if (velocities.size() != path.getSize()) { throw std::invalid_argument( "Velocity2D vector and path points vector should have the same size " "must have the same numPointsPerTrajectory"); @@ -400,14 +403,15 @@ struct TrajectoryPathSamples { // Add a new path from a Path struct. void push_back(const Path::Path &path) { - assert(path.points.size() == numPointsPerTrajectory_ && - "Path points vector must have size equivalent to numPointsPerTrajectory"); + assert(path.getSize() == numPointsPerTrajectory_ && + "Path points vector must have size equivalent to " + "numPointsPerTrajectory"); pathIndex_++; for (size_t i = 0; i < numPointsPerTrajectory_; ++i) { - x(pathIndex_, i) = path.points[i].x(); - y(pathIndex_, i) = path.points[i].y(); - z(pathIndex_, i) = path.points[i].z(); + x(pathIndex_, i) = path.getIndex(i).x(); + y(pathIndex_, i) = path.getIndex(i).y(); + z(pathIndex_, i) = path.getIndex(i).z(); } } @@ -577,7 +581,10 @@ struct TrajectorySamples2D { struct TrajSearchResult { Trajectory2D trajectory; bool isTrajFound = false; - float trajCost; + float trajCost = 0.0; + + // default constructor + TrajSearchResult() = default; }; // Lowest cost and its associated index for the trajectory sample diff --git a/src/kompass_cpp/kompass_cpp/include/planning/ompl.h b/src/kompass_cpp/kompass_cpp/include/planning/ompl.h new file mode 100644 index 00000000..0e481674 --- /dev/null +++ b/src/kompass_cpp/kompass_cpp/include/planning/ompl.h @@ -0,0 +1,93 @@ +#pragma once + +#include "utils/collision_check.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace Kompass { + +namespace Planning { + +class OMPL2DGeometricPlanner { +public: + OMPL2DGeometricPlanner(const CollisionChecker::ShapeType &robot_shape_type, + const std::vector &robot_dimensions, + const ompl::geometric::SimpleSetup &setup, + const float map_resolution = 0.01); + ~OMPL2DGeometricPlanner(); + + /** + * @brief Setup OMPL planning problem + * + * @param start_x + * @param start_y + * @param start_yaw + * @param goal_x + * @param goal_y + * @param goal_yaw + * @param map_3d + */ + void setupProblem(double start_x, double start_y, double start_yaw, + double goal_x, double goal_y, double goal_yaw, + const std::vector &map_3d); + /** + * @brief Solve the planning problem + * + * @param planning_timeout + * @return true + * @return false + */ + bool solve(double planning_timeout); + + /** + * @brief Get the Path object + * + * @return std::optional + */ + std::optional getPath() const; + + /** + * @brief Get the Cost object + * + * @return float + */ + float getCost() const; + + /** + * @brief Set the Space Bounds From Map object + * + * @param origin_x + * @param origin_y + * @param width + * @param height + * @param resolution + */ + void setSpaceBoundsFromMap(const float origin_x, const float origin_y, + const int width, const int height, + const float resolution); + +private: + bool gotSolution_ = false; + ompl::geometric::SimpleSetupPtr setup_; + std::shared_ptr collision_checker_; + + /** + * @brief State validity checking function (uses collision checker) + * + * @param state + * @return true + * @return false + */ + bool state_validity_checker(const ompl::base::State *state); +}; + +} // namespace Planning + +} // namespace Kompass diff --git a/src/kompass_cpp/kompass_cpp/include/utils/collision_check.h b/src/kompass_cpp/kompass_cpp/include/utils/collision_check.h index a8e98ff7..79f9c7b6 100644 --- a/src/kompass_cpp/kompass_cpp/include/utils/collision_check.h +++ b/src/kompass_cpp/kompass_cpp/include/utils/collision_check.h @@ -1,9 +1,6 @@ #pragma once -#include -#include -#include -#include +#include #include #include @@ -50,8 +47,8 @@ class CollisionChecker { */ CollisionChecker(const ShapeType robot_shape_type, const std::vector &robot_dimensions, - const std::array &sensor_position_body, - const std::array &sensor_rotation_body, + const Eigen::Vector3f &sensor_position_body, + const Eigen::Quaternionf &sensor_rotation_body, const double octree_resolution = 0.01); /** @@ -100,6 +97,15 @@ class CollisionChecker { void updatePointCloud(const pcl::PointCloud::Ptr &cloud, const bool global_frame = true); + /** + * @brief Update the sensor input from Map points (Eigen::Matrix3Xf) data + * + * @param points + * @param global_frame + */ + void update3DMap(const std::vector &points, + const bool global_frame = true); + /** * @brief Update the sensor input from PointCloud like struct * std::vector @@ -221,6 +227,8 @@ class CollisionChecker { */ bool checkCollisions(const Path::State current_state); + float getRadius() const; + protected: double robotHeight_{1.0}, robotRadius_; // sensor tf with respect to the world @@ -245,13 +253,12 @@ class CollisionChecker { std::shared_ptr octTree_; // Octomap octree used to get data from laserscan // or pointcloud and convert it to an Octree - std::unique_ptr fclTree_ = + std::shared_ptr fclTree_ = nullptr; // FCL Octree updated after converting the Octomap octree // (required for creating the collision object) octomap::Pointcloud octomapCloud_; - std::vector - OctreeBoxes; // Vector of Boxes collision objects used to check - // collisions with an octTree + std::unique_ptr + OctreeCollObj_; // Octree collision object double octree_resolution_{0.01}; @@ -262,14 +269,14 @@ class CollisionChecker { void updateOctreePtr(); /** - * @brief Generates a vector of fcl::Box collision objects from an - * fcl::Octree + * @brief Helper method to generate a vector of fcl::Box collision objects + * from an fcl::Octree * * @param boxes * @param tree */ - void generateBoxesFromOctomap(std::vector &boxes, - fcl::OcTreef &tree); + std::vector + generateBoxesFromOctomap(fcl::OcTreef &tree); /** * @brief Helper method to convert PointCloud data to an Octomap diff --git a/src/kompass_cpp/kompass_cpp/include/utils/cost_evaluator.h b/src/kompass_cpp/kompass_cpp/include/utils/cost_evaluator.h index 68442f12..de4a7030 100644 --- a/src/kompass_cpp/kompass_cpp/include/utils/cost_evaluator.h +++ b/src/kompass_cpp/kompass_cpp/include/utils/cost_evaluator.h @@ -63,8 +63,8 @@ class CostEvaluator { ControlLimitsParams ctrLimits, size_t maxNumTrajectories, size_t numPointsPerTrajectory, size_t maxRefPathSegmentSize); CostEvaluator(TrajectoryCostsWeights &costsWeights, - const std::array &sensor_position_body, - const std::array &sensor_rotation_body, + const Eigen::Vector3f &sensor_position_body, + const Eigen::Quaternionf &sensor_rotation_body, ControlLimitsParams ctrLimits, size_t maxNumTrajectories, size_t numPointsPerTrajectory, size_t maxRefPathSegmentSize); @@ -105,7 +105,7 @@ class CostEvaluator { */ TrajSearchResult getMinTrajectoryCost(const std::unique_ptr &trajs, - const Path::Path &reference_path, + const Path::Path *reference_path, const Path::Path &tracked_segment); /** @@ -148,7 +148,9 @@ class CostEvaluator { }; void setPointScan(const std::vector &cloud, - const Path::State ¤t_state, const float max_sensor_range, const float max_obstacle_cost_range_multiple = 3.0) { + const Path::State ¤t_state, + const float max_sensor_range, + const float max_obstacle_cost_range_multiple = 3.0) { obstaclePointsX.clear(); obstaclePointsY.clear(); maxObstaclesDist = max_sensor_range / max_obstacle_cost_range_multiple; @@ -209,6 +211,7 @@ class CostEvaluator { float *m_devicePtrTempCosts = nullptr; LowestCost *m_minCost; sycl::queue m_q; + sycl::vec m_deviceRefPathEnd; void initializeGPUMemory(); /** * @brief Trajectory cost based on the distance to a given reference path @@ -228,9 +231,7 @@ class CostEvaluator { * @param trajectories * @param reference_path */ - sycl::event goalCostFunc(const size_t trajs_size, - const Path::Point &last_ref_point, - const float ref_path_length, + sycl::event goalCostFunc(const size_t trajs_size, const float ref_path_length, const double cost_weight); /** @@ -283,7 +284,7 @@ class CostEvaluator { * @return float */ float goalCostFunc(const Trajectory2D &trajectory, - const Path::Path &reference_path, + const Path::Point &reference_path_end_point, const float ref_path_length); /** diff --git a/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check.h b/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check.h index b7754581..9fa4e96a 100644 --- a/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check.h +++ b/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check.h @@ -21,10 +21,10 @@ class CriticalZoneChecker { */ CriticalZoneChecker(const CollisionChecker::ShapeType robot_shape_type, const std::vector &robot_dimensions, - const std::array &sensor_position_body, - const std::array &sensor_rotation_body, - const float critical_angle, - const float critical_distance); + const Eigen::Vector3f &sensor_position_body, + const Eigen::Vector4f &sensor_rotation_body, + const float critical_angle, const float critical_distance, + const float slowdown_distance); /** * @brief Destroy the CriticalZoneChecker object @@ -34,8 +34,17 @@ class CriticalZoneChecker { void preset(const std::vector &angles); - bool check(const std::vector &ranges, - const std::vector &angles, const bool forward); + /** + * @brief Check if the robot is in the slowdown or critical zone + * + * @param ranges LaserScan ranges + * @param angles LaserScan angles + * @param forward True if the robot is moving forward, false otherwise + * @return Slowdown factor (0.0 - 1.0) if in the slowdown zone, 0.0 if in + * the critical zone (stop), 1.0 otherwise + */ + float check(const std::vector &ranges, + const std::vector &angles, const bool forward); protected: double robotHeight_{1.0}, robotRadius_; @@ -43,7 +52,7 @@ class CriticalZoneChecker { angle_left_backward_; std::vector indicies_forward_, indicies_backward_; bool preset_{false}; - float critical_distance_; + float critical_distance_, slowdown_distance_; Eigen::Isometry3f sensor_tf_body_ = Eigen::Isometry3f::Identity(); // Sensor transformation with diff --git a/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check_gpu.h b/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check_gpu.h index 2a56a43e..39e9695c 100644 --- a/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check_gpu.h +++ b/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check_gpu.h @@ -18,14 +18,16 @@ class CriticalZoneCheckerGPU : public CriticalZoneChecker { // Constructor CriticalZoneCheckerGPU(const CollisionChecker::ShapeType robot_shape_type, const std::vector &robot_dimensions, - const std::array &sensor_position_body, - const std::array &sensor_rotation_body, + const Eigen::Vector3f &sensor_position_body, + const Eigen::Vector4f &sensor_rotation_body, const float critical_angle, const float critical_distance, + const float slowdown_distance, const std::vector &angles) : CriticalZoneChecker(robot_shape_type, robot_dimensions, sensor_position_body, sensor_rotation_body, - critical_angle, critical_distance), + critical_angle, critical_distance, + slowdown_distance), m_scanSize(angles.size()) { m_q = sycl::queue{sycl::default_selector_v, sycl::property::queue::in_order{}}; @@ -35,7 +37,7 @@ class CriticalZoneCheckerGPU : public CriticalZoneChecker { m_devicePtrRanges = sycl::malloc_device(m_scanSize, m_q); m_devicePtrAngles = sycl::malloc_device(m_scanSize, m_q); - m_result = sycl::malloc_shared(1, m_q); + m_result = sycl::malloc_shared(1, m_q); // set forward and backward indices preset(angles); @@ -44,13 +46,15 @@ class CriticalZoneCheckerGPU : public CriticalZoneChecker { m_devicePtrBackward = sycl::malloc_device(indicies_backward_.size(), m_q); m_q.memcpy(m_devicePtrForward, indicies_forward_.data(), - sizeof(size_t) * indicies_forward_.size()).wait(); + sizeof(size_t) * indicies_forward_.size()) + .wait(); m_q.memcpy(m_devicePtrBackward, indicies_backward_.data(), - sizeof(size_t) * indicies_backward_.size()).wait(); + sizeof(size_t) * indicies_backward_.size()) + .wait(); m_scan_in_zone = std::max(indicies_forward_.size(), indicies_backward_.size()); - m_devicePtrOutput = sycl::malloc_device(m_scan_in_zone, m_q); + m_devicePtrOutput = sycl::malloc_device(m_scan_in_zone, m_q); } // Destructor @@ -83,8 +87,8 @@ class CriticalZoneCheckerGPU : public CriticalZoneChecker { * @param ranges LaserScan ranges in meters * @param forward If the robot is moving forward */ - bool check(const std::vector &ranges, - const std::vector &angles, const bool forward); + float check(const std::vector &ranges, + const std::vector &angles, const bool forward); private: const int m_scanSize; @@ -93,8 +97,8 @@ class CriticalZoneCheckerGPU : public CriticalZoneChecker { double *m_devicePtrAngles; size_t *m_devicePtrForward; size_t *m_devicePtrBackward; - bool *m_devicePtrOutput; - bool *m_result; + float *m_devicePtrOutput; + float *m_result; sycl::queue m_q; }; diff --git a/src/kompass_cpp/kompass_cpp/include/utils/gpu_check.h b/src/kompass_cpp/kompass_cpp/include/utils/gpu_check.h new file mode 100644 index 00000000..c9498226 --- /dev/null +++ b/src/kompass_cpp/kompass_cpp/include/utils/gpu_check.h @@ -0,0 +1,18 @@ +#pragma once + +#include + +/** + * @brief Retrieves the names of SYCL-compatible accelerator devices available + * on the system. + * + * This function queries the system using SYCL's runtime API to list the names + * of all devices across all platforms (e.g., GPUs, CPUs, and accelerators). The + * function only executes if the GPU macro is defined at compile time (e.g., via + * -DGPU=1 or target_compile_definitions in CMake). If the GPU macro is not + * defined, or no devices are available, an empty string is returned. + * + * @return A newline-separated string of device names, or an empty string if GPU + * support is not enabled or no devices are found. + */ +std::string getAvailableAccelerators(); diff --git a/src/kompass_cpp/kompass_cpp/include/utils/kalman_filter.h b/src/kompass_cpp/kompass_cpp/include/utils/kalman_filter.h new file mode 100644 index 00000000..ec4ed654 --- /dev/null +++ b/src/kompass_cpp/kompass_cpp/include/utils/kalman_filter.h @@ -0,0 +1,48 @@ +#pragma once + +#include +#include + +using Eigen::MatrixXf; +using namespace std; + +namespace Kompass { + +class LinearSSKalmanFilter { +private: + bool state_initialized = false, system_initialized = false; + Eigen::MatrixXf state; + Eigen::MatrixXf A; // state matrix + Eigen::MatrixXf B; // input matrix + Eigen::MatrixXf H; // observation matrix + Eigen::MatrixXf P; // uncertainty + Eigen::MatrixXf Q; // process noise + Eigen::MatrixXf R; // observation noise + +public: + // constructor + LinearSSKalmanFilter(const size_t num_states, const size_t num_inputs); + + // set up the filter + bool setup(const Eigen::MatrixXf &A, const Eigen::MatrixXf &B, + const Eigen::MatrixXf &Q, const Eigen::MatrixXf &H, + const Eigen::MatrixXf &R); + + void setInitialState(const Eigen::VectorXf &initial_state); + + // set A (sometimes sampling time will differ) + void setA(const Eigen::MatrixXf &A); + + // state estimate + void estimate(const Eigen::MatrixXf &z, const Eigen::MatrixXf &u, + const int numberSteps = 1); + + void estimate(const Eigen::MatrixXf &z, const int numberSteps = 1); + + // read output from the state + double getState(const size_t state_index); + + std::optional getState(); +}; + +} // namespace Kompass diff --git a/src/kompass_cpp/kompass_cpp/include/utils/logger.h b/src/kompass_cpp/kompass_cpp/include/utils/logger.h index 1143d98e..14601346 100644 --- a/src/kompass_cpp/kompass_cpp/include/utils/logger.h +++ b/src/kompass_cpp/kompass_cpp/include/utils/logger.h @@ -8,7 +8,7 @@ #ifdef GPU #include -#endif //!GPU +#endif //! GPU namespace Kompass { @@ -136,6 +136,6 @@ inline void setLogFile(const std::string &filename) { #else #define KERNEL_DEBUG(...) #endif -#endif // !GPU +#endif // !GPU } // namespace Kompass diff --git a/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h b/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h index 47c9684e..dc58d5d1 100644 --- a/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h +++ b/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h @@ -7,6 +7,7 @@ #include "datatypes/trajectory.h" #include #include +#include #include #ifndef MIN_VEL @@ -77,16 +78,16 @@ class TrajectorySampler { int maxAngularSamples, const CollisionChecker::ShapeType robotShapeType, const std::vector robotDimensions, - const std::array &sensor_position_body, - const std::array &sensor_rotation_body, + const Eigen::Vector3f &sensor_position_body, + const Eigen::Quaternionf &sensor_rotation_body, const double octreeRes, const int maxNumThreads = 1); TrajectorySampler(TrajectorySamplerParameters config, ControlLimitsParams controlLimits, ControlType controlType, const CollisionChecker::ShapeType robotShapeType, const std::vector robotDimensions, - const std::array &sensor_position_body, - const std::array &sensor_rotation_body, + const Eigen::Vector3f &sensor_position_body, + const Eigen::Quaternionf &sensor_rotation_body, const int maxNumThreads = 1); /** @@ -95,6 +96,8 @@ class TrajectorySampler { */ ~TrajectorySampler() = default; + void updateState(const Path::State ¤t_state); + void setSampleDroppingMode(const bool drop_samples); /** @@ -124,6 +127,16 @@ class TrajectorySampler { */ void resetOctreeResolution(const double resolution); + float getRobotRadius() const; + + Trajectory2D + generateSingleSampleFromVel(const Velocity2D &vel, + const Path::State &pose = Path::State()); + + template + bool checkStatesFeasibility(const std::vector &states, + const T &sensor_points); + size_t numTrajectories; size_t numPointsPerTrajectory; diff --git a/src/kompass_cpp/kompass_cpp/include/utils/transformation.h b/src/kompass_cpp/kompass_cpp/include/utils/transformation.h index 52be646d..3aa5700e 100644 --- a/src/kompass_cpp/kompass_cpp/include/utils/transformation.h +++ b/src/kompass_cpp/kompass_cpp/include/utils/transformation.h @@ -3,8 +3,6 @@ #include "datatypes/path.h" #include #include -#include -#include namespace Kompass { @@ -25,11 +23,11 @@ getTransformation(const RotationType &rotation_src_to_goal, // Create a transformation matrix Eigen::Isometry3f transform_src_to_goal = Eigen::Isometry3f::Identity(); - // Set rotation based on the type of RotationType - transform_src_to_goal.rotate(rotation_src_to_goal); - // Set translation - transform_src_to_goal.pretranslate(translation_src_to_goal); + transform_src_to_goal.translate(translation_src_to_goal); + + // Set rotation based on the type of RotationType + transform_src_to_goal.rotate(Eigen::Quaternionf(rotation_src_to_goal)); return transform_src_to_goal; } diff --git a/src/kompass_cpp/kompass_cpp/include/vision/depth_detector.h b/src/kompass_cpp/kompass_cpp/include/vision/depth_detector.h new file mode 100644 index 00000000..3470788e --- /dev/null +++ b/src/kompass_cpp/kompass_cpp/include/vision/depth_detector.h @@ -0,0 +1,71 @@ +/** + * @brief Implementation based on the following work [Xu2024OnboardDO] + * + * "Onboard Dynamic-Object Detection and Tracking for Autonomous Robot + * Navigation With RGB-D Camera" Z. Xu, X. Zhan, Y. Xiu, C. Suzuki, and K. + * Shimada. IEEE Robotics and Automation Letters, vol. 9, no. 1, pp. 651–658, + * 2024. doi:10.1109/LRA.2023.3334683 + * + * @article{Xu2024OnboardDO, + * title = {Onboard Dynamic-Object Detection and Tracking for Autonomous + * Robot Navigation With RGB-D Camera}, author = {Z. Xu and X. Zhan and Y. Xiu + * and C. Suzuki and K. Shimada}, journal = {IEEE Robotics and Automation + * Letters}, volume = {9}, number = {1}, pages = {651--658}, year = + * {2024}, doi = {10.1109/LRA.2023.3334683}, keywords = { Detectors, + * Cameras, Three-dimensional displays, Point cloud compression, Robot vision + * systems, Heuristic algorithms, Collision avoidance, RGB-D perception, + * Vision-based navigation, Visual tracking, 3D object detection + * } + * } + */ + +#pragma once + +#include "datatypes/path.h" +#include "datatypes/tracking.h" +#include +#include +#include +#include +#include + +namespace Kompass { + +class DepthDetector { +public: + DepthDetector(const Eigen::Vector2f &depth_range, + const Eigen::Vector3f &camera_in_body_translation, + const Eigen::Quaternionf &camera_in_body_rotation, + const Eigen::Vector2f &focal_length, + const Eigen::Vector2f &principal_point, + const float depth_conversion_factor = 1e-3); + + DepthDetector(const Eigen::Vector2f &depth_range, + const Eigen::Isometry3f &camera_in_body_tf, + const Eigen::Vector2f &focal_length, + const Eigen::Vector2f &principal_point, + const float depth_conversion_factor = 1e-3); + + void + updateBoxes(const Eigen::MatrixX aligned_depth_img, + const std::vector &detections, + const std::optional &robot_state = std::nullopt); + + std::optional> get3dDetections() const; + +private: + float cx_, cy_, fx_, fy_; // Depth Image camera intrinsics + float minDepth_, maxDepth_, depthConversionFactor_; + Eigen::MatrixX alignedDepthImg_; + Eigen::Isometry3f camera_in_body_tf_, body_in_world_tf_; + std::unique_ptr> boxes_; + + std::optional convert2Dboxto3Dbox(const Bbox2D &box2d); + + static void calculateMAD(const std::vector &depthValues, float &median, + float &mad); + + static float getMedian(const std::vector &values); +}; + +} // namespace Kompass diff --git a/src/kompass_cpp/kompass_cpp/include/vision/tracker.h b/src/kompass_cpp/kompass_cpp/include/vision/tracker.h new file mode 100644 index 00000000..0e3b5e23 --- /dev/null +++ b/src/kompass_cpp/kompass_cpp/include/vision/tracker.h @@ -0,0 +1,56 @@ +#pragma once + +#include "datatypes/control.h" +#include "datatypes/tracking.h" +#include "utils/kalman_filter.h" +#include +#include +#include +#include + +namespace Kompass { + +class FeatureBasedBboxTracker { +public: + FeatureBasedBboxTracker(const float &time_step, const float &e_pos, + const float &e_vel, const float &e_acc); + + static constexpr int StateSize = 9; + + using FeaturesVector = Eigen::Vector; + + bool setInitialTracking(const TrackedBbox3D &bBox); + + bool setInitialTracking(const Bbox3D &bBox, const float yaw = 0.0); + + bool setInitialTracking(const int &pose_x_img, const int &pose_y_img, + const std::vector &detected_boxes, + const float yaw = 0.0); + + bool trackerInitialized() const; + + bool updateTracking(const std::vector &detected_boxes); + + std::optional getRawTracking() const; + + std::optional getTrackedState() const; + + std::optional getFilteredTrackedPose2D() const; + +private: + float timeStep_, minAcceptedSimilarityScore_ = 0.0; + std::string trackedLabel_; + std::unique_ptr trackedBox_; + std::unique_ptr stateKalmanFilter_; + + FeaturesVector extractFeatures(const TrackedBbox3D &bBox) const; + + FeaturesVector extractFeatures(const Bbox3D &bBox) const; + + Eigen::Vector3f + computePointsStdDev(const std::vector &pc_points) const; + + void updateTrackedBoxState(const int numberSteps = 1); +}; + +} // namespace Kompass diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp index 7515be37..02ccd52b 100644 --- a/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp +++ b/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp @@ -3,10 +3,8 @@ #include "datatypes/control.h" #include "datatypes/path.h" #include "datatypes/trajectory.h" -#include -#include #include -#include +#include #include #include @@ -17,17 +15,16 @@ DWA::DWA(ControlLimitsParams controlLimits, ControlType controlType, int maxLinearSamples, int maxAngularSamples, const CollisionChecker::ShapeType robotShapeType, const std::vector robotDimensions, - const std::array &sensor_position_body, - const std::array &sensor_rotation_body, - const double octreeRes, + const Eigen::Vector3f &sensor_position_body, + const Eigen::Vector4f &sensor_rotation_body, const double octreeRes, CostEvaluator::TrajectoryCostsWeights costWeights, const int maxNumThreads) : Follower() { // Setup the trajectory sampler and cost evaluator configure(controlLimits, controlType, timeStep, predictionHorizon, - controlHorizon, maxLinearSamples, maxAngularSamples, - robotShapeType, robotDimensions, sensor_position_body, - sensor_rotation_body, octreeRes, costWeights, maxNumThreads); + controlHorizon, maxLinearSamples, maxAngularSamples, robotShapeType, + robotDimensions, sensor_position_body, sensor_rotation_body, + octreeRes, costWeights, maxNumThreads); // Update the max forward distance the robot can make if (controlType == ControlType::OMNI) { @@ -37,21 +34,23 @@ DWA::DWA(ControlLimitsParams controlLimits, ControlType controlType, } else { max_forward_distance_ = controlLimits.velXParams.maxVel * predictionHorizon; } + + initJitCompile(); } DWA::DWA(TrajectorySampler::TrajectorySamplerParameters config, ControlLimitsParams controlLimits, ControlType controlType, const CollisionChecker::ShapeType robotShapeType, const std::vector robotDimensions, - const std::array &sensor_position_body, - const std::array &sensor_rotation_body, + const Eigen::Vector3f &sensor_position_body, + const Eigen::Vector4f &sensor_rotation_body, CostEvaluator::TrajectoryCostsWeights costWeights, const int maxNumThreads) : Follower() { // Setup the trajectory sampler and cost evaluator - configure(config, controlLimits, controlType, robotShapeType, - robotDimensions, sensor_position_body, sensor_rotation_body, - costWeights, maxNumThreads); + configure(config, controlLimits, controlType, robotShapeType, robotDimensions, + sensor_position_body, sensor_rotation_body, costWeights, + maxNumThreads); // Update the max forward distance the robot can make double timeHorizon = config.getParameter("control_horizon"); @@ -62,46 +61,65 @@ DWA::DWA(TrajectorySampler::TrajectorySamplerParameters config, } else { max_forward_distance_ = controlLimits.velXParams.maxVel * timeHorizon; } + + initJitCompile(); } -void DWA::configure(ControlLimitsParams controlLimits, - ControlType controlType, double timeStep, - double predictionHorizon, double controlHorizon, - int maxLinearSamples, int maxAngularSamples, - const CollisionChecker::ShapeType robotShapeType, - const std::vector robotDimensions, - const std::array &sensor_position_body, - const std::array &sensor_rotation_body, - const double octreeRes, - CostEvaluator::TrajectoryCostsWeights costWeights, - const int maxNumThreads) { +void DWA::initJitCompile() { + const int dummyNumSamples = 1; + const int dummyNumPoints = 2; + TrajectoryVelocitySamples2D velocities(dummyNumSamples, dummyNumPoints); + TrajectoryPathSamples paths(dummyNumSamples, dummyNumPoints); + velocities.push_back({Velocity2D(1.0, 0.0, 0.0)}); + auto dummyPath = Path::Path( + {Path::Point(0.0f, 0.0f, 0.0f), Path::Point(1.0f, 1.0f, 0.0f)}); + paths.push_back(dummyPath); + std::unique_ptr dummySamples = + std::make_unique(velocities, paths); + trajCostEvaluator->getMinTrajectoryCost(dummySamples, &dummyPath, dummyPath); +} + +void DWA::configure(ControlLimitsParams controlLimits, ControlType controlType, + double timeStep, double predictionHorizon, + double controlHorizon, int maxLinearSamples, + int maxAngularSamples, + const CollisionChecker::ShapeType robotShapeType, + const std::vector robotDimensions, + const Eigen::Vector3f &sensor_position_body, + const Eigen::Vector4f &sensor_rotation_body, + const double octreeRes, + CostEvaluator::TrajectoryCostsWeights costWeights, + const int maxNumThreads) { trajSampler = std::make_unique( controlLimits, controlType, timeStep, predictionHorizon, controlHorizon, maxLinearSamples, maxAngularSamples, robotShapeType, robotDimensions, - sensor_position_body, sensor_rotation_body, octreeRes, maxNumThreads); + sensor_position_body, Eigen::Quaternionf(sensor_rotation_body), octreeRes, + maxNumThreads); trajCostEvaluator = std::make_unique( - costWeights, sensor_position_body, sensor_rotation_body, controlLimits, + costWeights, sensor_position_body, + Eigen::Quaternionf(sensor_rotation_body), controlLimits, trajSampler->numTrajectories, trajSampler->numPointsPerTrajectory, maxSegmentSize); this->maxNumThreads = maxNumThreads; } void DWA::configure(TrajectorySampler::TrajectorySamplerParameters config, - ControlLimitsParams controlLimits, - ControlType controlType, - const CollisionChecker::ShapeType robotShapeType, - const std::vector robotDimensions, - const std::array &sensor_position_body, - const std::array &sensor_rotation_body, - CostEvaluator::TrajectoryCostsWeights costWeights, - const int maxNumThreads) { + ControlLimitsParams controlLimits, ControlType controlType, + const CollisionChecker::ShapeType robotShapeType, + const std::vector robotDimensions, + const Eigen::Vector3f &sensor_position_body, + const Eigen::Vector4f &sensor_rotation_body, + CostEvaluator::TrajectoryCostsWeights costWeights, + const int maxNumThreads) { trajSampler = std::make_unique( config, controlLimits, controlType, robotShapeType, robotDimensions, - sensor_position_body, sensor_rotation_body, maxNumThreads); + sensor_position_body, Eigen::Quaternionf(sensor_rotation_body), + maxNumThreads); trajCostEvaluator = std::make_unique( - costWeights, sensor_position_body, sensor_rotation_body, controlLimits, + costWeights, sensor_position_body, + Eigen::Quaternionf(sensor_rotation_body), controlLimits, trajSampler->numTrajectories, trajSampler->numPointsPerTrajectory, maxSegmentSize); this->maxNumThreads = maxNumThreads; @@ -120,119 +138,59 @@ void DWA::addCustomCost( trajCostEvaluator->addCustomCost(weight, custom_cost_function); } +void DWA::setCurrentState(const Path::State &position) { + this->currentState = position; + this->trajSampler->updateState(position); +} + Path::Path DWA::findTrackedPathSegment() { - std::vector trackedPoints; + size_t segment_index{current_segment_index_ + 1}; Path::Path currentSegment = currentPath->segments[current_segment_index_]; // If we reached end of the current segment and a new segment is available -> // take the next segment - if (closestPosition->index > currentSegment.points.size() - 1 and + if (closestPosition->index >= currentSegment.getSize() - 1 and current_segment_index_ < max_segment_index_) { - trackedPoints = currentPath->segments[current_segment_index_ + 1].points; segment_index = current_segment_index_ + 1; + return currentPath->segments[current_segment_index_ + 1]; + } else if (closestPosition->index >= currentSegment.getSize() - 1) { + // Return current segment directly (last segment) + return currentPath->segments[current_segment_index_]; } // Else take the segment points from the current point onwards else { - trackedPoints = {currentSegment.points.begin() + closestPosition->index, - currentSegment.points.end()}; - } - size_t point_index{0}; - - float segment_length = 0.0; - for (size_t i = 1; i < trackedPoints.size(); ++i) { - segment_length += - Path::Path::distance(trackedPoints[i - 1], trackedPoints[i]); - } - - // If the segment does not have the required number of points add more points - // from next path segment - while (segment_length < max_forward_distance_ and - segment_index <= max_segment_index_ and - trackedPoints.size() < maxSegmentSize) { - if (point_index >= currentPath->segments[segment_index].points.size()) { - point_index = 0; - segment_index++; - if (segment_index > max_segment_index_) { - break; + auto trackedPath = currentSegment.getPart(closestPosition->index, + currentSegment.getSize() - 1, + this->maxSegmentSize); + size_t point_index{0}; + + float segment_length = trackedPath.totalPathLength(); + + // If the segment does not have the required number of points add more + // points from next path segment + while (segment_length < max_forward_distance_ and + segment_index <= max_segment_index_ and + trackedPath.getSize() < maxSegmentSize - 1) { + if (point_index >= currentPath->segments[segment_index].getSize()) { + point_index = 0; + segment_index++; + if (segment_index > max_segment_index_) { + break; + } } + // Add distance between last point and new point + Path::Point back_point = trackedPath.getEnd(); + segment_length += calculateDistance( + back_point, + currentPath->segments[segment_index].getIndex(point_index)); + trackedPath.pushPoint( + currentPath->segments[segment_index].getIndex(point_index)); + point_index++; } - // Add distance between last point and new point - Path::Point back_point = trackedPoints.back(); - segment_length += calculateDistance( - back_point, currentPath->segments[segment_index].points[point_index]); - trackedPoints.push_back( - currentPath->segments[segment_index].points[point_index]); - point_index++; - } - - return Path::Path(trackedPoints); -} - -template -TrajSearchResult DWA::findBestPath(const Velocity2D &global_vel, - const T &scan_points) { - // Throw an error if the global path is not set - if (!currentPath) { - throw std::invalid_argument( - "Pointer to global path is NULL. Cannot use DWA local planner without " - "setting a global path"); - } - // find closest segment to use in cost computation - determineTarget(); - - // Generate set of valid trajectories in the DW - std::unique_ptr samples_ = - trajSampler->generateTrajectories(global_vel, currentState, scan_points); - if (samples_->size() == 0) { - return TrajSearchResult(); - } - - trajCostEvaluator->setPointScan(scan_points, currentState, maxLocalRange_); - - Path::Path trackedRefPathSegment = findTrackedPathSegment(); - - // Evaluate the samples and get the sample with the minimum cost - return trajCostEvaluator->getMinTrajectoryCost(samples_, *currentPath, - trackedRefPathSegment); -} - -template -Controller::Result DWA::computeVelocityCommand(const Velocity2D &global_vel, - const T &scan_points) { - TrajSearchResult searchRes = findBestPath(global_vel, scan_points); - Controller::Result finalResult; - if (searchRes.isTrajFound) { - finalResult.status = Controller::Result::Status::COMMAND_FOUND; - // Get the first command to be applied - finalResult.velocity_command = searchRes.trajectory.velocities.getFront(); - latest_velocity_command_ = finalResult.velocity_command; - } else { - finalResult.status = Controller::Result::Status::NO_COMMAND_POSSIBLE; - } - return finalResult; -} - -TrajSearchResult DWA::computeVelocityCommandsSet(const Velocity2D &global_vel, - const LaserScan &scan) { - TrajSearchResult searchRes = findBestPath(global_vel, scan); - // Update latest velocity command - if (searchRes.isTrajFound) { - latest_velocity_command_ = searchRes.trajectory.velocities.getFront(); - } - return searchRes; -} - -TrajSearchResult -DWA::computeVelocityCommandsSet(const Velocity2D &global_vel, - const std::vector &cloud) { - TrajSearchResult searchRes = findBestPath(global_vel, cloud); - // Update latest velocity command - if (searchRes.isTrajFound) { - latest_velocity_command_ = searchRes.trajectory.velocities.getFront(); + return trackedPath; } - return searchRes; } std::tuple DWA::getDebuggingSamples() const { diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp index e37396ba..568a4513 100644 --- a/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp +++ b/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp @@ -1,6 +1,5 @@ #include #include -#include #include #include #include @@ -35,21 +34,7 @@ Follower::Follower() : Controller(), config() { Follower::Follower(FollowerParameters config) : Controller() { this->config = config; - lookahead_distance = config.getParameter("lookahead_distance"); - enable_reverse_driving = config.getParameter("enable_reverse_driving"); - goal_dist_tolerance = config.getParameter("goal_dist_tolerance"); - goal_orientation_tolerance = - config.getParameter("goal_orientation_tolerance"); - loosing_goal_distance = config.getParameter("loosing_goal_distance"); - path_segment_length = config.getParameter("path_segment_length"); - maxDist = config.getParameter("max_point_interpolation_distance"); - // Set rotate_in_place based on the robot type - if (ctrType == Control::ControlType::ACKERMANN) { - rotate_in_place = false; - } else { - rotate_in_place = true; - } - maxSegmentSize = getMaxSegmentSize(); + Follower(); } size_t Follower::getMaxSegmentSize() const { @@ -81,15 +66,16 @@ void Follower::clearCurrentPath() { return; } -void Follower::setCurrentPath(const Path::Path &path) { - currentPath = std::make_unique(path.points); - refPath = std::make_unique(path.points); +void Follower::setCurrentPath(const Path::Path &path, const bool interpolate) { + currentPath = std::make_unique(path); + refPath = std::make_unique(path); currentPath->setMaxLength( this->config.getParameter("max_path_length")); - currentPath->interpolate(maxDist, interpolationType); - + if (interpolate) { + currentPath->interpolate(maxDist, interpolationType); + } // Segment path currentPath->segment(path_segment_length); @@ -113,7 +99,7 @@ size_t Follower::getCurrentSegmentIndex() { return current_segment_index_; } bool Follower::isGoalReached() { if (!path_processing_) { - return reached_goal_; + return true; } const Path::Point goal_point = currentPath->getEnd(); @@ -220,8 +206,7 @@ Path::State Follower::projectPointOnSegment(const Path::Point &a, Path::PathPosition Follower::findClosestPointOnSegment(size_t segment_index) { - const std::vector &segment_points = - currentPath->segments[segment_index].points; + const Path::Path &segment_path = currentPath->segments[segment_index]; double min_distance = std::numeric_limits::max(); Path::State closest_point; double segment_position = 0.0; // in [0, 1] @@ -232,17 +217,16 @@ Path::PathPosition Follower::findClosestPointOnSegment(size_t segment_index) { double segment_heading = std::atan2(end.y() - start.y(), end.x() - start.x()); - for (size_t i = 0; i < segment_points.size(); ++i) { - - Path::Point projected_point = segment_points[i]; + for (auto projected_point : segment_path) { double distance = calculateDistance(currentState, projected_point); if (distance < min_distance) { min_distance = distance; closest_point = {projected_point.x(), projected_point.y(), segment_heading}; - segment_position = static_cast(i) / (segment_points.size() - 1); - point_index = i; + segment_position = + static_cast(point_index) / (segment_path.getSize() - 1); + point_index++; } } @@ -274,12 +258,16 @@ Path::PathPosition Follower::findClosestPointOnSegment(size_t segment_index) { void Follower::determineTarget() { currentTrackedTarget_ = std::make_unique(); - // closestPosition = new Path::PathPosition(); - - // closest position is never updated - // OR If we reached end of segment or end of path -> Find new point + LOG_DEBUG("Closest point index on segment ", closestPosition->index, + " max index on segment is", + currentPath->segments[current_segment_index_].getSize() - 1, + " its segment length = ", closestPosition->segment_length); + // If closest position is never updated + // OR If we reached end of a segment or end of the path -> Find new segment + // then new point on segment if ((closestPosition->segment_length <= 0.0) || - (closestPosition->segment_index >= currentPath->points.size() - 1) || + (closestPosition->index >= + currentPath->segments[current_segment_index_].getSize() - 1) || (closestPosition->segment_length >= 1.0)) { *closestPosition = findClosestPathPoint(); } @@ -288,7 +276,7 @@ void Follower::determineTarget() { *closestPosition = findClosestPointOnSegment(closestPosition->segment_index); } - currentTrackedTarget_->segment_index = closestPosition->segment_index; + currentTrackedTarget_->segment_index = current_segment_index_; currentTrackedTarget_->position_in_segment = closestPosition->segment_length; currentTrackedTarget_->movement = closestPosition->state; @@ -332,7 +320,7 @@ const double Follower::getPathLength() const { return currentPath->totalPathLength(); } const bool Follower::hasPath() const { - if (!path_processing_) { + if (!currentPath or !path_processing_) { return false; } return currentPath->totalPathLength() > 0.0; diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/rgb_follower.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/rgb_follower.cpp new file mode 100644 index 00000000..5835649f --- /dev/null +++ b/src/kompass_cpp/kompass_cpp/src/controllers/rgb_follower.cpp @@ -0,0 +1,247 @@ +#include "controllers/rgb_follower.h" +#include "datatypes/control.h" +#include "datatypes/tracking.h" +#include "utils/logger.h" +#include +#include +#include + +namespace Kompass { +namespace Control { + +RGBFollower::RGBFollower(const ControlType robotCtrlType, + const ControlLimitsParams robotCtrlLimits, + const RGBFollowerConfig config) { + ctrl_limits_ = robotCtrlLimits; + config_ = config; + is_diff_drive_ = robotCtrlType == ControlType::DIFFERENTIAL_DRIVE; +} + +void RGBFollower::resetTarget(const Bbox2D &target) { + // empty the search command queue + std::queue> empty; + std::swap(search_commands_queue_, empty); + // Set the target as the current tracking bounding + // box size + LOG_DEBUG("Got target size: ", target.size.x(), ", ", target.size.y(), + ". Image size=", target.img_size.x(), ", ", target.img_size.y(), + ". Center=", target.getCenter().x(), ", ", target.getCenter().y()); + float size = static_cast(target.size.x() * target.size.y()) / + static_cast(target.img_size.x() * target.img_size.y()); + LOG_DEBUG("Setting vision target reference distance to size: ", size); + config_.set_target_distance(size); +} + +void RGBFollower::generateSearchCommands(float total_rotation, + float search_radius, + float max_rotation_time, + bool enable_pause) { + // Calculate rotation direction and magnitude + double rotation_sign = (total_rotation < 0.0) ? -1.0 : 1.0; + float rotation_time = max_rotation_time; + int num_pause_steps = + static_cast(config_.search_pause() / config_.control_time_step()); + if (enable_pause) { + // Modify the total number of active rotation to include the pause steps + rotation_time = + max_rotation_time * (1 - num_pause_steps / config_.control_time_step()); + } + // Angular velocity to rotate 'total_rotation' in total time steps + // 'rotation_steps' with dt = control_time_step + double omega_val = total_rotation / rotation_time; + + omega_val = std::max(std::min(omega_val, ctrl_limits_.omegaParams.maxOmega), + config_.min_vel()); + // Generate velocity commands + for (float t = 0.0f; t <= max_rotation_time; + t = t + config_.control_time_step()) { + if (is_diff_drive_) { + // In-place rotation + search_commands_queue_.emplace( + std::array{0.0, 0.0, rotation_sign * omega_val}); + } else { + // Angular velocity based on linear velocity and radius + double omega_ackermann = + rotation_sign * ctrl_limits_.velXParams.maxVel / search_radius; + // Non-holonomic circular motion + search_commands_queue_.emplace(std::array{ + ctrl_limits_.velXParams.maxVel, 0.0, omega_ackermann}); + } + if (enable_pause) { + // Add zero commands for search pause + for (int j = 0; j <= num_pause_steps; j++) { + search_commands_queue_.emplace(std::array{0.0, 0.0, 0.0}); + } + } + } + return; +} + +void RGBFollower::getFindTargetCmds(const int last_direction) { + // Generate new search commands if starting a new search or no commands are + // available + LOG_DEBUG("Generating new search commands in direction: ", last_direction); + + search_commands_queue_ = std::queue>(); + const float target_searchtimeout_part = config_.target_search_timeout() / 4; + // rotate pi + generateSearchCommands(last_direction * M_PI, config_.target_search_radius(), + target_searchtimeout_part); + // go back + generateSearchCommands(-last_direction * M_PI, config_.target_search_radius(), + target_searchtimeout_part); + // rotate -pi + generateSearchCommands(-last_direction * M_PI, config_.target_search_radius(), + target_searchtimeout_part); + // go back + generateSearchCommands(last_direction * M_PI, config_.target_search_radius(), + target_searchtimeout_part); +} + +bool RGBFollower::run(const std::optional target) { + // Check if tracking has a value + if (target.has_value()) { + // clear all + recorded_wait_time_ = 0.0; + recorded_search_time_ = 0.0; + // Access the TrackingData object + const auto &data = target.value(); + last_tracking_ = std::make_unique(data); + // Track the target + trackTarget(data); + return true; + } + // Tracking not available + if (config_.enable_search()) { + if (recorded_search_time_ < config_.target_search_timeout()) { + if (search_commands_queue_.empty()) { + int last_direction = 1; + if (last_tracking_ != nullptr) { + auto last_center = last_tracking_->getCenter(); + last_direction = + ((last_center.x() - last_center.y() / 2.0) > 0.0) ? 1 : -1; + last_tracking_ = nullptr; + } + getFindTargetCmds(last_direction); + } + search_command_ = search_commands_queue_.front(); + search_commands_queue_.pop(); + recorded_search_time_ += config_.control_time_step(); + return true; + } else { + recorded_search_time_ = 0.0; + // Failed to find target + return false; + } + } else { + if (recorded_wait_time_ < config_.target_wait_timeout()) { + LOG_DEBUG("Target lost, waiting to get tracked target again ..."); + last_tracking_ = nullptr; + // Do nothing and wait + recorded_wait_time_ += config_.control_time_step(); + return true; + } else { + recorded_wait_time_ = 0.0; + // Failed to get target after waiting + return false; + } + } +} + +void RGBFollower::trackTarget(const Bbox2D &target) { + float current_dist = + static_cast(target.size.x() * target.size.y()) / + static_cast(target.img_size.x() * target.img_size.y()); + + dist_error_ = config_.target_distance() - current_dist; + + float distance_tolerance = config_.tolerance() * config_.target_distance(); + + // Error to have the target in the center of the image (imgz_size / 2) + float error_y = 2.0f * (static_cast(target.getCenter().y()) / static_cast(target.img_size.y()) - 0.5f); + float error_x = 2.0f * (static_cast(target.getCenter().x()) / static_cast(target.img_size.x()) - 0.5f); + + orientation_error_ = error_x; + + LOG_DEBUG("Current size: ", current_dist, + ", Reference size: ", config_.target_distance(), + "size_error=", dist_error_, ", tolerance=", distance_tolerance); + + LOG_DEBUG("Img error x: ", error_x, + ", center_x: ", static_cast(target.getCenter().x()), ", img_size_x: ", static_cast(target.img_size.x())); + LOG_DEBUG("Img error y: ", error_y, + ", center_y: ", static_cast(target.getCenter().y()), ", img_size_y: ", static_cast(target.img_size.y())); + + float dist_speed, omega, v; + // If all errors are within limits -> break + if (std::abs(dist_error_) < distance_tolerance && + std::abs(error_y) < config_.tolerance() && + std::abs(error_x) < config_.tolerance()) { + // Set command to zero + out_vel_ = Velocities(1); + out_vel_.set(0, 0.0, 0.0, 0.0); + return; + } + + dist_speed = std::abs(dist_error_) > distance_tolerance + ? (dist_error_ / config_.target_distance()) * ctrl_limits_.velXParams.maxVel + : 0.0; + + // X center error : (2.0 * tracking.center_xy[0] / tracking.img_width - 1.0) + // in [-1, 1] Omega in [-K_omega * omega_max, K_omega * omega_max] + omega = -config_.K_omega() * error_x * ctrl_limits_.omegaParams.maxOmega; + + // Y center error : (2.0 * tracking.center_xy[1] / tracking.img_height + // - 1.0) in [-1, 1] V in [-K_v * speed_max, K_v * speed_max] + v = config_.K_v() * dist_speed; + + // Limit by the minimum allowed velocity to avoid sending meaningless low + // commands to the robot + omega = std::abs(omega) >= config_.min_vel() ? omega : 0.0; + float omega_limit = static_cast(ctrl_limits_.omegaParams.maxOmega); + omega = std::clamp(omega, -omega_limit, omega_limit); + + float v_limit = static_cast(ctrl_limits_.velXParams.maxVel); + v = std::abs(v) >= config_.min_vel() ? v : 0.0; + v = std::clamp(v, -v_limit, v_limit); + + LOG_DEBUG("dist_error ", dist_error_, ", error_x: ", error_x); + LOG_DEBUG("v=", v, ", omega= ", omega); + + if (is_diff_drive_ and std::abs(v) >= config_.min_vel() and + std::abs(omega) >= config_.min_vel()) { + // Rotate then move + out_vel_ = Velocities(2); + out_vel_.set(0, 0.0, 0.0, omega); + out_vel_.set(1, v, 0.0, 0.0); + } else { + out_vel_ = Velocities(1); + out_vel_.set(0, v, 0.0, omega); + } + + return; +} + +const Velocities RGBFollower::getCtrl() const { + if (recorded_search_time_ <= 0.0 && recorded_wait_time_ <= 0.0) { + return out_vel_; + } + // If search is on + else if (recorded_search_time_ > 0.0) { + Velocities _search(1); + LOG_DEBUG( + "Number of search commands remaining: ", search_commands_queue_.size(), + "recorded search time: ", recorded_search_time_); + _search.set(0, search_command_[0], search_command_[1], search_command_[2]); + return _search; + } + // If search not active -> wait till timeout + else { + // send 0.0 to wait + Velocities _wait(1); + return _wait; + } +} + +} // namespace Control +} // namespace Kompass diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp new file mode 100644 index 00000000..e64b17e4 --- /dev/null +++ b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp @@ -0,0 +1,315 @@ +#include "controllers/vision_dwa.h" +#include "datatypes/control.h" +#include "datatypes/path.h" +#include "datatypes/tracking.h" +#include "utils/angles.h" +#include "utils/logger.h" +#include "utils/transformation.h" +#include "vision/depth_detector.h" +#include +#include +#include +#include + +namespace Kompass { +namespace Control { + +VisionDWA::VisionDWA(const ControlType &robotCtrlType, + const ControlLimitsParams &ctrlLimits, + const int maxLinearSamples, const int maxAngularSamples, + const CollisionChecker::ShapeType &robotShapeType, + const std::vector &robotDimensions, + const Eigen::Vector3f &proximity_sensor_position_body, + const Eigen::Vector4f &proximity_sensor_rotation_body, + const Eigen::Vector3f &vision_sensor_position_body, + const Eigen::Vector4f &vision_sensor_rotation_body, + const double octreeRes, + const CostEvaluator::TrajectoryCostsWeights &costWeights, + const int maxNumThreads, const VisionDWAConfig &config) + : DWA(ctrlLimits, robotCtrlType, config.control_time_step(), + config.prediction_horizon(), config.control_horizon(), + maxLinearSamples, maxAngularSamples, robotShapeType, robotDimensions, + proximity_sensor_position_body, proximity_sensor_rotation_body, + octreeRes, costWeights, maxNumThreads), + RGBFollower(robotCtrlType, ctrlLimits) { + ctrl_limits_ = ctrlLimits; + config_ = config; + // Set the reaching goal distance (used in the DWA mode when vision target is + // lost) + track_velocity_ = config_.enable_vel_tracking(); + goal_dist_tolerance = config_.e_pose(); + // Initialize the bounding box tracker + tracker_ = std::make_unique( + config.control_time_step(), config.e_pose(), config.e_vel(), + config.e_acc()); + vision_sensor_tf_ = getTransformation(vision_sensor_rotation_body, + vision_sensor_position_body); +} + +void VisionDWA::setCameraIntrinsics(const float focal_length_x, + const float focal_length_y, + const float principal_point_x, + const float principal_point_y) { + detector_ = std::make_unique( + config_.depth_range(), vision_sensor_tf_, + Eigen::Vector2f{focal_length_x, focal_length_y}, + Eigen::Vector2f{principal_point_x, principal_point_y}, + config_.depth_conversion_factor()); +} + +Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose, const bool update_global_error) { + float distance, psi, gamma = 0.0f; + if (track_velocity_) { + distance = tracking_pose.distance(currentState.x, currentState.y, 0.0) - + trajSampler->getRobotRadius(); + psi = Angle::normalizeToMinusPiPlusPi( + std::atan2(tracking_pose.y(), tracking_pose.x()) - currentState.yaw); + gamma = + Angle::normalizeToMinusPiPlusPi(tracking_pose.yaw() - currentState.yaw); + } else { + distance = + tracking_pose.distance(0.0, 0.0, 0.0) - trajSampler->getRobotRadius(); + psi = Angle::normalizeToMinusPiPlusPi( + std::atan2(tracking_pose.y(), tracking_pose.x())); + } + distance = std::max(distance, 0.0f); + + float distance_error = config_.target_distance() - distance; + float angle_error = Angle::normalizeToMinusPiPlusPi( + config_.target_orientation() - gamma - psi); + + // Update error is enabled (to avoid update for simulated states) + if (update_global_error) { + dist_error_ = distance_error; + orientation_error_ = angle_error; + } + + Velocity2D followingVel; + if (abs(distance_error) > config_.dist_tolerance() or + abs(angle_error) > config_.ang_tolerance()) { + double v = + track_velocity_ * (tracking_pose.v() * cos(gamma - psi)) - + config_.K_v() * ctrl_limits_.velXParams.maxVel * tanh(distance_error); + + v = std::clamp(v, -ctrl_limits_.velXParams.maxVel, + ctrl_limits_.velXParams.maxVel); + if (std::abs(v) < config_.min_vel()) { + v = 0.0; + } + followingVel.setVx(v); + double omega; + + omega = 2.0 * + (track_velocity_ * tracking_pose.v() * sin(gamma - psi) / distance + + v * sin(psi) / distance - + config_.K_omega() * ctrl_limits_.omegaParams.maxOmega * + tanh(angle_error)); + + omega = std::clamp(omega, -ctrl_limits_.omegaParams.maxOmega, + ctrl_limits_.omegaParams.maxOmega); + if (std::abs(omega) < config_.min_vel()) { + omega = 0.0; + } + followingVel.setOmega(omega); + } + return followingVel; +} + +bool VisionDWA::setInitialTracking(const int pose_x_img, const int pose_y_img, + const std::vector &detected_boxes, + const float yaw) { + return tracker_->setInitialTracking(pose_x_img, pose_y_img, detected_boxes, + yaw); +} + +bool VisionDWA::setInitialTracking( + const int pose_x_img, const int pose_y_img, + const Eigen::MatrixX &aligned_depth_image, + const std::vector &detected_boxes, const float yaw) { + + std::unique_ptr target_box; + for (auto box : detected_boxes) { + auto limits_x = box.getXLimits(); + if (pose_x_img >= limits_x(0) and pose_x_img <= limits_x(1)) { + auto limits_y = box.getYLimits(); + if (pose_y_img >= limits_y(0) and pose_y_img <= limits_y(1)) { + target_box = std::make_unique(box); + break; + } + } + } + if (!target_box) { + LOG_DEBUG("Target point not found in any detected box"); + return false; + } + + return setInitialTracking(aligned_depth_image, *target_box, yaw); +} + +bool VisionDWA::setInitialTracking( + const Eigen::MatrixX &aligned_depth_image, + const Bbox2D &target_box_2d, const float yaw) { + if (!detector_) { + throw std::runtime_error( + "DepthDetector is not initialized with the camera intrinsics. Call " + "'VisionDWA::setCameraIntrinsics' first"); + } + if (track_velocity_) { + // Send current state to the detector + detector_->updateBoxes(aligned_depth_image, {target_box_2d}, currentState); + } else { + detector_->updateBoxes(aligned_depth_image, {target_box_2d}); + } + auto boxes_3d = detector_->get3dDetections(); + if (!boxes_3d) { + LOG_DEBUG("Failed to get 3D box from 2D target box"); + return false; + } + return tracker_->setInitialTracking(boxes_3d.value()[0], yaw); +} + +Trajectory2D +VisionDWA::getTrackingReferenceSegment(const TrackedPose2D &tracking_pose) { + int step = 0; + + Trajectory2D ref_traj(config_.prediction_horizon()); + Path::State simulated_state(0.0, 0.0, 0.0); + if (track_velocity_) { + // Global frame -> get actual state + simulated_state = Path::State(currentState); + } + auto simulated_track = TrackedPose2D(tracking_pose); + Eigen::Isometry3f tracked_state_tf = Eigen::Isometry3f::Identity(); + Velocity2D cmd; + + // Simulate following the tracked target for the period til + // prediction_horizon assuming the target moves with its same current + // velocity + while (step < config_.prediction_horizon()) { + ref_traj.path.add(step, + Path::Point(simulated_state.x, simulated_state.y, 0.0)); + this->setCurrentState(simulated_state); + // Get the pure tracking control command + // If step == 0, update the global error to have the errors for the initial state + cmd = this->getPureTrackingCtrl(simulated_track, (step == 0)); + simulated_state.update(cmd, config_.control_time_step()); + if (track_velocity_) { + simulated_track.update(config_.control_time_step()); + } else { + // Update the tracked state in the local frame after the robot have + // moved (apply inverse of robot velocity) + tracked_state_tf = getTransformation(simulated_state); + auto new_state = transformPosition( + Eigen::Vector3f(simulated_track.x(), simulated_track.y(), 0.0), + tracked_state_tf.inverse()); + simulated_track = TrackedPose2D(new_state.x(), new_state.y(), + simulated_track.z(), 0.0, 0.0, 0.0); + } + if (step < config_.prediction_horizon() - 1) { + ref_traj.velocities.add(step, cmd); + } + + step++; + } + this->setCurrentState(currentState); + + return ref_traj; +}; + +Trajectory2D VisionDWA::getTrackingReferenceSegmentDiffDrive( + const TrackedPose2D &tracking_pose) { + int step = 0; + + Trajectory2D ref_traj(config_.prediction_horizon()); + Path::State simulated_state(0.0, 0.0, 0.0); + if (track_velocity_) { + // Global frame -> get actual state + simulated_state = Path::State(currentState); + } + auto simulated_track = TrackedPose2D(tracking_pose); + Eigen::Isometry3f tracked_state_tf = Eigen::Isometry3f::Identity(); + Velocity2D cmd; + + // Simulate following the tracked target for the period til + // prediction_horizon assuming the target moves with its same current + // velocity + while (step < config_.prediction_horizon()) { + LOG_DEBUG("Step: ", step, "Simulated robot at", simulated_state.x, ",", + simulated_state.y, " simulated target at ", simulated_track.x(), + ",", simulated_track.y()); + this->setCurrentState(simulated_state); + cmd = this->getPureTrackingCtrl(simulated_track); + LOG_DEBUG("Got CMD: ", cmd.vx(), ",", cmd.omega()); + if (std::abs(cmd.vx()) >= config_.min_vel() && + std::abs(cmd.omega()) >= config_.min_vel()) { + // Rotate then Move + ref_traj.path.add(step, + Path::Point(simulated_state.x, simulated_state.y, 0.0)); + auto vel_rotate = Velocity2D(0.0, 0.0, cmd.omega()); + simulated_state.update(vel_rotate, config_.control_time_step()); + if (track_velocity_) { + simulated_track.update(config_.control_time_step()); + } else { + // Update the tracked state in the local frame after the robot have + // moved (apply inverse of robot velocity) + tracked_state_tf = getTransformation(simulated_state); + auto new_state = transformPosition( + Eigen::Vector3f(simulated_track.x(), simulated_track.y(), 0.0), + tracked_state_tf.inverse()); + simulated_track = TrackedPose2D(new_state.x(), new_state.y(), + simulated_track.z(), 0.0, 0.0, 0.0); + } + if (step < config_.prediction_horizon() - 1) { + ref_traj.velocities.add(step, vel_rotate); + } + step++; + if (step < config_.prediction_horizon() - 2) { + auto vel_move = Velocity2D(cmd.vx(), 0.0, 0.0); + ref_traj.path.add( + step, Path::Point(simulated_state.x, simulated_state.y, 0.0)); + simulated_state.update(vel_move, config_.control_time_step()); + if (track_velocity_) { + simulated_track.update(config_.control_time_step()); + } else { + // Update the tracked state in the local frame after the robot have + // moved (apply inverse of robot velocity) + tracked_state_tf = getTransformation(simulated_state); + auto new_state = transformPosition( + Eigen::Vector3f(simulated_track.x(), simulated_track.y(), 0.0), + tracked_state_tf.inverse()); + simulated_track = TrackedPose2D(new_state.x(), new_state.y(), + simulated_track.z(), 0.0, 0.0, 0.0); + } + ref_traj.velocities.add(step, vel_move); + step++; + } + } else { + ref_traj.path.add(step, + Path::Point(simulated_state.x, simulated_state.y, 0.0)); + simulated_state.update(cmd, config_.control_time_step()); + if (track_velocity_) { + simulated_track.update(config_.control_time_step()); + } else { + // Update the tracked state in the local frame after the robot have + // moved (apply inverse of robot velocity) + tracked_state_tf = getTransformation(simulated_state); + auto new_state = transformPosition( + Eigen::Vector3f(simulated_track.x(), simulated_track.y(), 0.0), + tracked_state_tf.inverse()); + simulated_track = TrackedPose2D(new_state.x(), new_state.y(), + simulated_track.z(), 0.0, 0.0, 0.0); + } + + if (step < config_.prediction_horizon() - 1) { + ref_traj.velocities.add(step, cmd); + } + step++; + } + } + this->setCurrentState(currentState); + + return ref_traj; +}; + +} // namespace Control +} // namespace Kompass diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_follower.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_follower.cpp deleted file mode 100644 index cd4ec847..00000000 --- a/src/kompass_cpp/kompass_cpp/src/controllers/vision_follower.cpp +++ /dev/null @@ -1,275 +0,0 @@ -#include "controllers/vision_follower.h" -#include "datatypes/control.h" -#include "utils/logger.h" -#include -#include - -namespace Kompass { -namespace Control { - -VisionFollower::VisionFollower(const ControlType robotCtrlType, - const ControlLimitsParams robotCtrlLimits, - const VisionFollowerConfig config) { - _ctrlType = robotCtrlType; - _ctrl_limits = robotCtrlLimits; - _config = config; - // Initialize time steps - int num_steps = _config.control_horizon(); - // Initialize control vectors - _out_vel = Velocities(num_steps); - _rotate_in_place = _ctrlType != ControlType::ACKERMANN; -} - -void VisionFollower::resetTarget(const TrackingData tracking) { - // empty the search command queue - std::queue> empty; - std::swap(_search_commands_queue, empty); - // Set the reference depth as the current depth if it is not provided in the - // config - if (tracking.depth > 0) { - if (_config.target_distance() < 0) { - _config.set_target_distance(tracking.depth); - } - } - // if depth is not provided set the target as the current tracking bounding - // box size - else { - double size = (tracking.size_xy[0] * tracking.size_xy[1]) / - (tracking.img_width * tracking.img_height); - LOG_DEBUG("Setting vision target reference distance to size: ", size); - _config.set_target_distance(size); - } -} - -void VisionFollower::generateSearchCommands(double total_rotation, - double search_radius, - int max_rotation_steps, - bool enable_pause) { - // Calculate rotation direction and magnitude - double rotation_sign = (total_rotation < 0.0) ? -1.0 : 1.0; - int rotation_steps = max_rotation_steps; - if (enable_pause) { - // Modify the total number of active rotation to include the pause steps - rotation_steps = (max_rotation_steps + _config.search_pause()) / - (_config.search_pause() + 1); - } - // Angular velocity to rotate 'total_rotation' in total time steps - // 'rotation_steps' with dt = control_time_step - double rotation_value = - (total_rotation / rotation_steps) / _config.control_time_step(); - rotation_value = - std::max(std::min(rotation_value, _ctrl_limits.omegaParams.maxOmega), - _config.min_vel()); - // Generate velocity commands - for (int i = 0; i <= rotation_steps; i = i + 1) { - if (_ctrlType != ControlType::ACKERMANN) { - // In-place rotation - _search_commands_queue.emplace( - std::array{0.0, 0.0, rotation_sign * rotation_value}); - } else { - // Angular velocity based on linear velocity and radius - double omega_ackermann = - rotation_sign * _ctrl_limits.velXParams.maxVel / search_radius; - // Non-holonomic circular motion - _search_commands_queue.emplace(std::array{ - _ctrl_limits.velXParams.maxVel, 0.0, omega_ackermann}); - } - if (enable_pause) { - // Add zero commands for search pause - for (int j = 0; j <= _config.search_pause(); j++) { - _search_commands_queue.emplace(std::array{0.0, 0.0, 0.0}); - } - } - } - return; -} - -void VisionFollower::getFindTargetCmds() { - // Generate new search commands if starting a new search or no commands are - // available - double last_direction = 1.0; - if (_last_tracking != nullptr) { - last_direction = - ((_last_tracking->center_xy[0] - _last_tracking->img_width / 2.0) > 0.0) - ? 1.0 - : -1.0; - _last_tracking = nullptr; - } - LOG_DEBUG("Generating new search commands in direction: ", last_direction); - - _search_commands_queue = std::queue>(); - int target_search_steps_max = static_cast( - _config.target_search_timeout() / _config.control_time_step()); - // rotate pi - generateSearchCommands(last_direction * M_PI, _config.target_search_radius(), - target_search_steps_max / 4, true); - // go back - generateSearchCommands(-last_direction * M_PI, _config.target_search_radius(), - target_search_steps_max / 8); - // rotate -pi - generateSearchCommands(-last_direction * M_PI, _config.target_search_radius(), - target_search_steps_max / 4, true); - // go back - generateSearchCommands(last_direction * M_PI, _config.target_search_radius(), - target_search_steps_max / 8); -} - -bool VisionFollower::run(const std::optional tracking) { - bool tracking_available = false; - // Check if tracking has a value - if (tracking.has_value()) { - // clear all - _recorded_wait_time = 0.0; - _recorded_search_time = 0.0; - // Access the TrackingData object - const auto &data = tracking.value(); - // ensure size_xy or depth have valid values - tracking_available = - ((data.size_xy[0] > 0 && data.size_xy[1] > 0) || data.depth > 0); - if (tracking_available) { - if (_last_tracking == nullptr) { - resetTarget(data); - } - _last_tracking = std::make_unique(data); - // Track the target - trackTarget(data); - return true; - } - } - // Tracking not available - if (_config.enable_search()) { - if (_recorded_search_time < _config.target_search_timeout()) { - if (_search_commands_queue.empty()) { - getFindTargetCmds(); - } - _search_command = _search_commands_queue.front(); - _search_commands_queue.pop(); - _recorded_search_time += _config.control_time_step(); - return true; - } else { - _recorded_search_time = 0.0; - // Failed to find target - return false; - } - } else { - if (_recorded_wait_time < _config.target_wait_timeout()) { - LOG_DEBUG("Target lost, waiting to get tracked target again ..."); - _last_tracking = nullptr; - // Do nothing and wait - _recorded_wait_time += _config.control_time_step(); - return true; - } else { - _recorded_wait_time = 0.0; - // Failed to get target after waiting - return false; - } - } -} - -void VisionFollower::trackTarget(const TrackingData &tracking) { - double size = (tracking.size_xy[0] * tracking.size_xy[1]) / - (tracking.img_width * tracking.img_height); - - double current_distance = tracking.depth > 0 ? tracking.depth : size; - double distance_error = _config.target_distance() - current_distance; - - double distance_tolerance = _config.tolerance() * _config.target_distance(); - - double error_y = (2 * tracking.center_xy[1] / tracking.img_height - 1.0); - double error_x = (2 * tracking.center_xy[0] / tracking.img_width - 1.0); - - LOG_DEBUG("Current distance: ", size, - ", Reference: ", _config.target_distance(), - "Distance_error=", distance_error, - ", Distance_tolerance=", distance_tolerance); - - // Initialize control vectors - std::fill(_out_vel.vx.begin(), _out_vel.vx.end(), 0.0); - std::fill(_out_vel.vy.begin(), _out_vel.vy.end(), 0.0); - std::fill(_out_vel.omega.begin(), _out_vel.omega.end(), 0.0); - - double simulated_depth = current_distance; - double dist_speed, omega, v; - for (size_t i = 0; i < _out_vel._length; ++i) { - // If all errors are within limits -> break - if (std::abs(distance_error) < distance_tolerance && - std::abs(error_y) < _config.tolerance() && - std::abs(error_x) < _config.tolerance()) { - break; - } - if (_rotate_in_place && i % 2 != 0) - continue; - - dist_speed = std::abs(distance_error) > distance_tolerance - ? distance_error * _ctrl_limits.velXParams.maxVel - : 0.0; - - // X center error : (2.0 * tracking.center_xy[0] / tracking.img_width - 1.0) - // in [-1, 1] Omega in [-alpha * omega_max, alpha * omega_max] - omega = -_config.alpha() * error_x * _ctrl_limits.omegaParams.maxOmega; - - // Y center error : (2.0 * tracking.center_xy[1] / tracking.img_height - // - 1.0) in [-1, 1] V in [-speed_max, speed_max] - v = _config.beta() * dist_speed; - - // Limit by the minimum allowed velocity to avoid sending meaningless low - // commands to the robot - omega = std::abs(omega) >= _config.min_vel() ? omega : 0.0; - v = std::abs(v) >= _config.min_vel() ? v : 0.0; - - simulated_depth += -v * _config.control_time_step(); - // Update distance error - distance_error = _config.target_distance() - simulated_depth; - - if (_rotate_in_place) { - if (std::abs(v) < _config.min_vel()) { - // Set vx_ctrl[i] and vx_ctrl[i+1] to 0.0 - _out_vel.set(i, 0.0, 0.0, omega); - if (i + 1 < _out_vel._length) { - _out_vel.set(i + 1, 0.0, 0.0, omega); - } - } else if (std::abs(omega) < _config.min_vel()) { - // Set vx_ctrl[i] and vx_ctrl[i+1] to 0.0 - _out_vel.set(i, v, 0.0, 0.0); - if (i + 1 < _out_vel._length) { - _out_vel.set(i + 1, v, 0.0, 0.0); - } - } else { - // Set vx_ctrl[i] to 0.0 - _out_vel.set(i, v, 0.0, 0.0); - // Set vx_ctrl[i+1] to max(v, 0.0) - if (i + 1 < _out_vel._length) { - _out_vel.set(i + 1, 0.0, 0.0, omega); - } - } - } else { - _out_vel.set(i, v, 0.0, omega); - } - } - - return; -} - -const Velocities VisionFollower::getCtrl() const { - if (_recorded_search_time <= 0.0 && _recorded_wait_time <= 0.0) { - return _out_vel; - } - // If search is on - else if (_recorded_search_time > 0.0) { - Velocities _search(1); - LOG_DEBUG( - "Number of search commands remaining: ", _search_commands_queue.size(), - "recorded search time: ", _recorded_search_time); - _search.set(0, _search_command[0], _search_command[1], _search_command[2]); - return _search; - } - // If search not active -> wait till timeout - else { - // send 0.0 to wait - Velocities _wait(1); - return _wait; - } -} - -} // namespace Control -} // namespace Kompass diff --git a/src/kompass_cpp/kompass_cpp/src/datatypes/path.cpp b/src/kompass_cpp/kompass_cpp/src/datatypes/path.cpp index c250a96c..000257bd 100644 --- a/src/kompass_cpp/kompass_cpp/src/datatypes/path.cpp +++ b/src/kompass_cpp/kompass_cpp/src/datatypes/path.cpp @@ -8,40 +8,116 @@ using namespace tk; namespace Path { -Path::Path(const std::vector &points) : points(points) { - for (const auto &point : points) { - X_.emplace_back(point.x()); - Y_.emplace_back(point.y()); - Z_.emplace_back(point.z()); +Path::Path(const std::vector &points, const size_t new_max_size) { + if (new_max_size < 2) { + throw std::invalid_argument( + "At least two points are required to create a path."); + } + if (points.size() > new_max_size) { + throw std::invalid_argument( + "Points size is larger than the allowed maximum size."); + } + resize(new_max_size); + current_size_ = points.size(); + for (size_t i = 0; i < points.size(); ++i) { + X_(i) = points[i].x(); + Y_(i) = points[i].y(); + Z_(i) = points[i].z(); } } -const std::vector& Path::getX() const { return X_; } +Path::Path(const Eigen::VectorXf &x_points, const Eigen::VectorXf &y_points, + const Eigen::VectorXf &z_points, const size_t new_max_size) { + if (x_points.size() != y_points.size() || + x_points.size() != z_points.size()) { + throw std::invalid_argument("X, Y and Z vectors must have the same size."); + } + if (new_max_size < 2) { + throw std::invalid_argument( + "At least two points are required to create a path."); + } + if (x_points.size() > new_max_size) { + throw std::invalid_argument( + "Points size is larger than the allowed maximum size."); + } + resize(new_max_size); + current_size_ = x_points.size(); + X_.head(current_size_) = x_points; + Y_.head(current_size_) = y_points; + Z_.head(current_size_) = z_points; +} -const std::vector& Path::getY() const { return Y_; } +const Eigen::VectorXf Path::getX() const { + return X_.segment(0, current_size_); +} -const std::vector& Path::getZ() const { return Z_; } +const Eigen::VectorXf Path::getY() const { + return Y_.segment(0, current_size_); +} -void Path::setMaxLength(double max_length) { - this->_max_path_length = max_length; +const Eigen::VectorXf Path::getZ() const { + return Z_.segment(0, current_size_); +} + +size_t Path::getSize() const { return current_size_; } + +void Path::setMaxLength(double max_length) { max_path_length_ = max_length; } + +void Path::resize(const size_t new_max_size) { + max_size_ = new_max_size; + X_.resize(max_size_); + Y_.resize(max_size_); + Z_.resize(max_size_); + if (current_size_ > max_size_) { + current_size_ = max_size_; + } } bool Path::endReached(State currentState, double minDist) { - Point endPoint = points.back(); + Point endPoint = getEnd(); double dist = sqrt(pow(endPoint.x() - currentState.x, 2) + pow(endPoint.y() - currentState.y, 2)); return dist <= minDist; } +size_t Path::getMaxSize() const { return max_size_; } + size_t Path::getMaxNumSegments() { return segments.size() - 1; } -Point Path::getEnd() const { return points.back(); } +Point Path::getEnd() const { return getIndex(current_size_ - 1); } + +Point Path::getStart() const { return getIndex(0); } + +Point Path::getIndex(const size_t index) const { + assert(index < current_size_ && "Index out of range"); + return Point(X_(index), Y_(index), Z_(index)); +} + +Path Path::getPart(const size_t start, const size_t end, + const size_t max_part_size) const { + if (start >= current_size_ || end >= current_size_ || start >= end) { + throw std::out_of_range("Invalid range for path part."); + } + auto part_size = std::max(max_part_size, end - start + 1); + Path part(X_.segment(start, end - start + 1), + Y_.segment(start, end - start + 1), + Z_.segment(start, end - start + 1), part_size); + return part; +} -Point Path::getStart() const { return points.front(); } +void Path::pushPoint(const Point &point) { + if (current_size_ >= max_size_) { + throw std::out_of_range("Path is full. Cannot add more points."); + } + X_(current_size_) = point.x(); + Y_(current_size_) = point.y(); + Z_(current_size_) = point.z(); + current_size_++; +} float Path::getEndOrientation() const { - const Point &p1 = points[points.size() - 2]; - const Point &p2 = points[points.size() - 1]; + const Point &p1 = getIndex(current_size_ - 2); + const Point &p2 = getIndex(current_size_ - 1); float dx = p2.x() - p1.x(); float dy = p2.y() - p1.y(); @@ -53,8 +129,8 @@ float Path::getEndOrientation() const { } float Path::getStartOrientation() const { - const Point &p1 = points[0]; - const Point &p2 = points[1]; + const Point &p1 = getIndex(0); + const Point &p2 = getIndex(1); float dx = p2.x() - p1.x(); float dy = p2.y() - p1.y(); @@ -68,12 +144,12 @@ float Path::getStartOrientation() const { float Path::getOrientation(const size_t index) const { Point p1; Point p2; - if (index < points.size()) { - p1 = points[index]; - p2 = points[index + 1]; + if (index < current_size_) { + p1 = getIndex(index); + p2 = getIndex(index + 1); } else { - p1 = points[index - 1]; - p2 = points[index]; + p1 = getIndex(index - 1); + p2 = getIndex(index); } float dx = p2.x() - p1.x(); @@ -92,13 +168,13 @@ float Path::distance(const Point &p1, const Point &p2) { // Function to compute the total path length float Path::totalPathLength() const { - if (points.empty()) { + if (current_size_ < 2) { return 0.0; } float totalLength = 0.0; - for (size_t i = 1; i < points.size(); ++i) { - totalLength += distance(points[i - 1], points[i]); + for (size_t i = 1; i < current_size_; ++i) { + totalLength += distance(getIndex(i - 1), getIndex(i)); } return totalLength; @@ -106,13 +182,13 @@ float Path::totalPathLength() const { Point Path::getPointAtLength(const double length) const { float totalLength = totalPathLength(); - if (length <= totalLength or points.size() > 2) { + if (length <= totalLength or current_size_ > 2) { float accumLength = 0.0; - float twoPointDist = distance(points[0], points[1]); - for (size_t i = 1; i < points.size(); ++i) { - accumLength += distance(points[i - 1], points[i]); + float twoPointDist = distance(getIndex(0), getIndex(1)); + for (size_t i = 1; i < current_size_; ++i) { + accumLength += distance(getIndex(i - 1), getIndex(i)); if (abs(accumLength - totalLength) < twoPointDist) { - return points[i - 1]; + return getIndex(i - 1); } } } @@ -122,44 +198,45 @@ Point Path::getPointAtLength(const double length) const { size_t Path::getNumberPointsInLength(double length) const { double totalLength = 0.0; - for (size_t i = 1; i < points.size(); ++i) { - totalLength += distance(points[i - 1], points[i]); + for (size_t i = 1; i < current_size_; ++i) { + totalLength += distance(getIndex(i - 1), getIndex(i)); if (totalLength >= length) { return i; } } - return points.size(); + return current_size_; } void Path::interpolate(double max_interpolation_point_dist, InterpolationType type) { - - this->_max_interpolation_dist = max_interpolation_point_dist; - // Set the maximum size for the points - this->max_size = static_cast(this->_max_path_length / - this->_max_interpolation_dist); - if (points.size() < 2) { + if (current_size_ < 2) { throw invalid_argument( "At least two points are required to perform interpolation."); } + // Get copies of X and Y vectors effective points + Eigen::VectorXf x = X_.segment(0, current_size_); + Eigen::VectorXf y = Y_.segment(0, current_size_); - // Get copies of X and Y vectors - std::vector x = getX(); - std::vector y = getY(); + this->max_interpolation_dist_ = max_interpolation_point_dist; + // Set the maximum size for the points + auto maxSize = static_cast(this->max_path_length_ / + this->max_interpolation_dist_); + resize(maxSize); + // Remaining iteration when interpolating the path (interpolation points + // between each two path points) + max_interpolation_iterations_ = + static_cast((maxSize - current_size_) / (current_size_)); - points.clear(); - X_.clear(); - Y_.clear(); - Z_.clear(); + Z_ = Eigen::VectorXf::Zero(max_size_); + current_size_ = 0; float dist, x_e, y_e; for (size_t i = 0; i < x.size() - 1; ++i) { // Add the first point - points.emplace_back(x[i], y[i], 0.0); - X_.emplace_back(x[i]); - Y_.emplace_back(y[i]); - Z_.emplace_back(0.0); + X_(current_size_) = x[i]; + Y_(current_size_) = y[i]; + current_size_++; std::vector x_points, y_points; // Add mid point to send 3 points for spline interpolation @@ -180,11 +257,11 @@ void Path::interpolate(double max_interpolation_point_dist, // Create the spline object and set the x and y values if (type == InterpolationType::LINEAR) { - _spline = new tk::spline(x_points, y_points, tk::spline::linear); + spline_ = new tk::spline(x_points, y_points, tk::spline::linear); } else if (type == InterpolationType::CUBIC_SPLINE) { - _spline = new tk::spline(x_points, y_points, tk::spline::cspline); + spline_ = new tk::spline(x_points, y_points, tk::spline::cspline); } else { - _spline = new tk::spline(x_points, y_points, tk::spline::cspline_hermite); + spline_ = new tk::spline(x_points, y_points, tk::spline::cspline_hermite); } x_e = x[i]; @@ -193,53 +270,53 @@ void Path::interpolate(double max_interpolation_point_dist, // Interpolate new points between i, i+1 while (dist > max_interpolation_point_dist and - j < max_interpolation_iterations) { + j < max_interpolation_iterations_) { x_e = x[i] + j * (x[i + 1] - x[i]) * max_interpolation_point_dist; - y_e = _spline->operator()(x_e); - points.emplace_back(x_e, y_e, 0.0); - X_.emplace_back(x_e); - Y_.emplace_back(y_e); - Z_.emplace_back(0.0); + y_e = spline_->operator()(x_e); + X_(current_size_) = x_e; + Y_(current_size_) = y_e; + current_size_++; dist = distance({x_e, y_e, 0.0}, {x[i + 1], y[i + 1], 0.0}); j++; } - if (points.size() > this->max_size) { + if (current_size_ > this->max_size_) { float remaining_len = distance({x[i + 1], y[i + 1], 0.0}, {x[-1], y[-1], 0.0}); - LOG_WARNING("Cannot interpolate more than ", this->max_size, + LOG_WARNING("Cannot interpolate more than ", this->max_size_, " path points -> " "Discarding all future points of length ", remaining_len, "m"); break; } } - if (points.size() < this->max_size) { + if (current_size_ < this->max_size_) { // Add last point - points.emplace_back(x.back(), y.back(), 0.0); - X_.emplace_back(x.back()); - Y_.emplace_back(y.back()); - Z_.emplace_back(0.0); + X_(current_size_) = x(x.size() - 1); + Y_(current_size_) = y(y.size() - 1); + current_size_++; } } // Segment the path by a given segment path length [m] void Path::segment(double pathSegmentLength) { - if (_max_interpolation_dist > 0.0) { + if (max_interpolation_dist_ > 0.0) { this->max_segment_size = - static_cast(pathSegmentLength / _max_interpolation_dist) + 1; + static_cast(pathSegmentLength / max_interpolation_dist_) + 1; } segments.clear(); double totalLength = totalPathLength(); - Path new_segment; if (pathSegmentLength >= totalLength) { - new_segment = Path(points); + // Add the whole path as a single segment + auto new_segment = *this; + new_segment.resize(this->max_segment_size); segments.push_back(new_segment); } else { - int segmentsNumber = max(totalLength / pathSegmentLength, 1.0); + int segmentsNumber = + max(static_cast(totalLength / pathSegmentLength), 1); if (segmentsNumber == 1) { - new_segment = Path(points); - new_segment.max_size = this->max_segment_size; + auto new_segment = *this; + new_segment.resize(this->max_segment_size); segments.push_back(new_segment); return; } @@ -250,27 +327,27 @@ void Path::segment(double pathSegmentLength) { // Segment using a number of segments void Path::segmentBySegmentNumber(int numSegments) { segments.clear(); - if (numSegments <= 0 || points.empty()) { + if (numSegments <= 0 || current_size_ <= 0) { throw std::invalid_argument( "Invalid number of segments or empty points vector."); } - this->max_segment_size = points.size() / numSegments; - int remainder = points.size() % numSegments; + this->max_segment_size = current_size_ / numSegments; + int remainder = current_size_ % numSegments; - auto it = points.begin(); - Path new_segment; + auto it_x = X_.begin(); + auto it_y = Y_.begin(); + auto it_z = Z_.begin(); for (int i = 0; i < numSegments; ++i) { std::vector segment_points; for (int j = 0; j < this->max_segment_size; ++j) { - segment_points.push_back(*it++); + segment_points.push_back({*it_x++, *it_y++, *it_z++}); } if (remainder > 0) { - segment_points.push_back(*it++); + segment_points.push_back({*it_x++, *it_y++, *it_z++}); --remainder; } - new_segment = Path(segment_points); - new_segment.max_size = this->max_segment_size; + auto new_segment = Path(segment_points, segment_points.size()); segments.push_back(new_segment); } } @@ -279,18 +356,17 @@ void Path::segmentBySegmentNumber(int numSegments) { void Path::segmentByPointsNumber(int segmentLength) { this->max_segment_size = segmentLength; segments.clear(); - if (segmentLength <= 0 || points.empty()) { + if (segmentLength <= 0 || current_size_ <= 0) { throw std::invalid_argument( "Invalid segment length or empty points vector."); } - Path new_segment; - for (size_t i = 0; i < points.size(); i += segmentLength) { + for (size_t i = 0; i < current_size_; i += segmentLength) { std::vector segment_points; - for (size_t j = i; j < i + segmentLength && j < points.size(); ++j) { - segment_points.push_back(points[j]); + for (size_t j = i; j < i + segmentLength && j < current_size_; ++j) { + segment_points.push_back(getIndex(j)); } - new_segment = Path(segment_points); - new_segment.max_size = this->max_segment_size; + auto new_segment = Path(segment_points, segment_points.size()); + new_segment.max_size_ = this->max_segment_size; segments.push_back(new_segment); } } diff --git a/src/kompass_cpp/kompass_cpp/src/planning/ompl.cpp b/src/kompass_cpp/kompass_cpp/src/planning/ompl.cpp new file mode 100644 index 00000000..4d3a66e5 --- /dev/null +++ b/src/kompass_cpp/kompass_cpp/src/planning/ompl.cpp @@ -0,0 +1,103 @@ +#include "planning/ompl.h" +#include "utils/logger.h" + +namespace Kompass { +namespace Planning { +OMPL2DGeometricPlanner::OMPL2DGeometricPlanner( + const CollisionChecker::ShapeType &robot_shape_type, + const std::vector &robot_dimensions, + const ompl::geometric::SimpleSetup &setup, const float map_resolution) { + setup_ = ompl::geometric::SimpleSetupPtr( + std::make_shared(setup)); + setup_->setStateValidityChecker([this](const ompl::base::State *state) { + return this->state_validity_checker(state); + }); + collision_checker_ = std::make_shared( + robot_shape_type, robot_dimensions, Eigen::Vector3f(0.0, 0.0, 0.0), + Eigen::Quaternionf(1.0, 0.0, 0.0, 0.0), map_resolution); +} + +OMPL2DGeometricPlanner::~OMPL2DGeometricPlanner() {} + +void OMPL2DGeometricPlanner::setupProblem( + double start_x, double start_y, double start_yaw, double goal_x, + double goal_y, double goal_yaw, + const std::vector &map_3d) { + setup_->clear(); + collision_checker_->update3DMap(map_3d); + ompl::base::ScopedState start( + setup_->getStateSpace()); + ompl::base::ScopedState goal( + setup_->getStateSpace()); + start->setX(start_x); + start->setY(start_y); + start->setYaw(start_yaw); + + goal->setX(goal_x); + goal->setY(goal_y); + goal->setYaw(goal_yaw); + + setup_->setStartAndGoalStates(start, goal); +} + +void OMPL2DGeometricPlanner::setSpaceBoundsFromMap(const float origin_x, + const float origin_y, + const int width, + const int height, + const float resolution) { + auto bounds = ompl::base::RealVectorBounds(2); + bounds.setLow(0, origin_x); + bounds.setLow(1, origin_y); + bounds.setHigh(0, origin_x + float(resolution * width)); + bounds.setHigh(1, origin_y + float(resolution * height)); + setup_->getStateSpace()->as()->setBounds(bounds); +} + +bool OMPL2DGeometricPlanner::solve(double planning_timeout) { + auto state = setup_->solve(planning_timeout); + if (state == ompl::base::PlannerStatus::APPROXIMATE_SOLUTION or + state == ompl::base::PlannerStatus::EXACT_SOLUTION) { + gotSolution_ = true; + setup_->simplifySolution(); + } else { + LOG_ERROR("OMPL planner failed to find path, status = ", state.asString()); + gotSolution_ = false; + } + return gotSolution_; +} + +std::optional +OMPL2DGeometricPlanner::getPath() const { + if (gotSolution_) { + return setup_->getSolutionPath(); + } + return std::nullopt; +} + +float OMPL2DGeometricPlanner::getCost() const { + auto solution = this->getPath(); + if (solution) { + auto optimization_objective = setup_->getOptimizationObjective(); + ompl::base::Cost cost = solution->cost(optimization_objective); + return float(cost.value()); + } + return 0.0f; +} + +bool OMPL2DGeometricPlanner::state_validity_checker( + const ompl::base::State *state) { + bool is_state_valid = setup_->getSpaceInformation()->satisfiesBounds(state); + auto se2_state = state->as(); + double x = se2_state->getX(); + double y = se2_state->getY(); + double yaw = se2_state->getYaw(); + + collision_checker_->updateState(x, y, yaw); + + bool has_collisions = collision_checker_->checkCollisions(); + + return is_state_valid and not has_collisions; +} +} // namespace Planning + +} // namespace Kompass diff --git a/src/kompass_cpp/kompass_cpp/src/utils/collision_check.cpp b/src/kompass_cpp/kompass_cpp/src/utils/collision_check.cpp index 2e327246..eae1d781 100644 --- a/src/kompass_cpp/kompass_cpp/src/utils/collision_check.cpp +++ b/src/kompass_cpp/kompass_cpp/src/utils/collision_check.cpp @@ -1,8 +1,6 @@ #include "utils/collision_check.h" #include "utils/logger.h" #include "utils/transformation.h" -#include -#include #include #include #include @@ -19,8 +17,8 @@ namespace Kompass { CollisionChecker::CollisionChecker( const ShapeType robot_shape_type, const std::vector &robot_dimensions, - const std::array &sensor_position_body, - const std::array &sensor_rotation_body, + const Eigen::Vector3f &sensor_position_body, + const Eigen::Quaternionf &sensor_rotation_body, const double octree_resolution) { collManager_ = std::make_unique(); @@ -29,7 +27,7 @@ CollisionChecker::CollisionChecker( octree_resolution_ = octree_resolution; octTree_ = std::make_shared(octree_resolution_); - fclTree_ = std::make_unique(octTree_); + fclTree_ = std::make_shared(octTree_); body->shapeType = robot_shape_type; @@ -63,8 +61,7 @@ CollisionChecker::CollisionChecker( // Init the sensor position w.r.t body sensor_tf_body_ = - getTransformation(Eigen::Quaternionf(sensor_rotation_body.data()), - Eigen::Vector3f(sensor_position_body.data())); + getTransformation(sensor_rotation_body, sensor_position_body); sensor_tf_world_ = sensor_tf_body_; } @@ -76,9 +73,11 @@ void CollisionChecker::resetOctreeResolution(const double resolution) { } } -void CollisionChecker::generateBoxesFromOctomap( - std::vector &boxes, fcl::OcTreef &tree) { +float CollisionChecker::getRadius() const { return robotRadius_; } +std::vector +CollisionChecker::generateBoxesFromOctomap(fcl::OcTreef &tree) { + std::vector boxes; // Turn OctTree nodes into boxes std::vector> boxes_ = tree.toBoxes(); @@ -104,20 +103,22 @@ void CollisionChecker::generateBoxesFromOctomap( sensor_tf_world_.translation() + Eigen::Vector3f(x, y, z)); // translation obj->computeAABB(); + // Uncomment for debug + // auto trans = obj->getTransform(); + // std::cout << "Adding box with translation " << trans.translation() << + // std::endl; boxes.push_back(obj); } LOG_DEBUG("Total Generated boxes above: ", boxes.size()); + return boxes; } void CollisionChecker::updateOctreePtr() { - // Create fcl::OcTree from octomap::OcTree - // fclTree_ = new fcl::OcTreef(octTree_); fclTree_.reset(new fcl::OcTreef(octTree_)); - - // Transform the tree into a set of boxes and generate collision objects in - // OctreeBoxes - generateBoxesFromOctomap(OctreeBoxes, *fclTree_); + OctreeCollObj_ = + std::make_unique(fclTree_, sensor_tf_world_); + OctreeCollObj_->computeAABB(); } void CollisionChecker::updateState(const Path::State current_state) { @@ -159,6 +160,28 @@ void CollisionChecker::updatePointCloud(const std::vector &cloud, convertPointCloudToOctomap(cloud); } +void CollisionChecker::update3DMap(const std::vector &points, + const bool global_frame) { + if (global_frame) { + sensor_tf_world_ = Eigen::Isometry3f::Identity(); + } else { + // Transform the sensor position to the world frame + sensor_tf_world_ = body->tf * sensor_tf_body_; + } + + // Clear old data + octTree_->clear(); + + octomapCloud_.clear(); + for (auto &point : points) { + octomapCloud_.push_back(point.x(), point.y(), point.z()); + } + + octTree_->insertPointCloud(octomapCloud_, octomap::point3d(0, 0, 0)); + + updateOctreePtr(); +} + void CollisionChecker::convertPointCloudToOctomap( const pcl::PointCloud::Ptr &cloud, const bool global_frame) { @@ -166,7 +189,7 @@ void CollisionChecker::convertPointCloudToOctomap( sensor_tf_world_ = Eigen::Isometry3f::Identity(); } else { // Transform the sensor position to the world frame - sensor_tf_world_ = sensor_tf_body_ * body->tf; + sensor_tf_world_ = body->tf * sensor_tf_body_; } // Clear old data @@ -186,13 +209,13 @@ void CollisionChecker::convertPointCloudToOctomap( const std::vector &cloud, const bool global_frame) { // Transform the sensor position to the world frame - // NOTE: Transformation will be applied to the points when generating the - // collision boxes + // NOTE: Transformation will be applied to the points when creating the + // collision object if (global_frame) { sensor_tf_world_ = Eigen::Isometry3f::Identity(); } else { // Transform the sensor position to the world frame - sensor_tf_world_ = sensor_tf_body_ * body->tf; + sensor_tf_world_ = body->tf * sensor_tf_body_; } // Clear old data @@ -213,9 +236,9 @@ void CollisionChecker::convertLaserScanToOctomap( double height) { // Transform the sensor position to the world frame - // NOTE: Transformation will be applied to the points when generating the - // collision boxes - sensor_tf_world_ = sensor_tf_body_ * body->tf; + // NOTE: Transformation will be applied to the points when creating the + // collision object + sensor_tf_world_ = body->tf * sensor_tf_body_; // Clear old data octTree_->clear(); @@ -230,7 +253,6 @@ void CollisionChecker::convertLaserScanToOctomap( x = ranges[i] * cos(angle); y = ranges[i] * sin(angle); z = height_in_sensor; - octomapCloud_.push_back(x, y, z); } @@ -244,9 +266,9 @@ void CollisionChecker::convertLaserScanToOctomap( double height) { // Transform the sensor position to the world frame - // NOTE: Transformation will be applied to the points when generating the - // collision boxes - sensor_tf_world_ = sensor_tf_body_ * body->tf; + // NOTE: Transformation will be applied to the points when creating the + // collision object + sensor_tf_world_ = body->tf * sensor_tf_body_; // Clear old data octTree_->clear(); @@ -274,7 +296,7 @@ bool CollisionChecker::checkCollisionsOctree() { collManager_->clear(); - collManager_->registerObjects(OctreeBoxes); + collManager_->registerObject(OctreeCollObj_.get()); collManager_->setup(); @@ -284,6 +306,7 @@ bool CollisionChecker::checkCollisionsOctree() { return collisionData.result.isCollision(); // NOTE: Code below for testing box by box + // auto OctreeBoxes = generateBoxesFromOctomap(*fclTree_); // for (auto boxObj : OctreeBoxes) { // collManager->clear(); @@ -324,7 +347,7 @@ float CollisionChecker::getMinDistance() { collManager_->clear(); - collManager_->registerObjects(OctreeBoxes); + collManager_->registerObject(OctreeCollObj_.get()); collManager_->setup(); @@ -392,7 +415,7 @@ bool CollisionChecker::checkCollisions(const Path::State current_state) { std::make_unique(); m_collManager->clear(); - m_collManager->registerObjects(OctreeBoxes); + m_collManager->registerObject(OctreeCollObj_.get()); m_collManager->setup(); m_collManager->collide(m_stateObjPtr.get(), &collisionData, fcl::DefaultCollisionFunction); diff --git a/src/kompass_cpp/kompass_cpp/src/utils/cost_evaluator.cpp b/src/kompass_cpp/kompass_cpp/src/utils/cost_evaluator.cpp index f412aba4..40bd8c08 100644 --- a/src/kompass_cpp/kompass_cpp/src/utils/cost_evaluator.cpp +++ b/src/kompass_cpp/kompass_cpp/src/utils/cost_evaluator.cpp @@ -2,16 +2,13 @@ #include "utils/cost_evaluator.h" #include "datatypes/path.h" #include "datatypes/trajectory.h" -#include -#include -#include -#include +#include #include namespace Kompass { namespace Control { -CostEvaluator::CostEvaluator(TrajectoryCostsWeights& costsWeights, +CostEvaluator::CostEvaluator(TrajectoryCostsWeights &costsWeights, ControlLimitsParams ctrLimits, size_t maxNumTrajectories, size_t numPointsPerTrajectory, @@ -23,37 +20,35 @@ CostEvaluator::CostEvaluator(TrajectoryCostsWeights& costsWeights, static_cast(ctrLimits.omegaParams.maxAcceleration)}; } -CostEvaluator::CostEvaluator(TrajectoryCostsWeights& costsWeights, - const std::array &sensor_position_body, - const std::array &sensor_rotation_body, +CostEvaluator::CostEvaluator(TrajectoryCostsWeights &costsWeights, + const Eigen::Vector3f &sensor_position_body, + const Eigen::Quaternionf &sensor_rotation_body, ControlLimitsParams ctrLimits, size_t maxNumTrajectories, size_t numPointsPerTrajectory, size_t maxRefPathSegmentSize) { sensor_tf_body_ = - getTransformation(Eigen::Quaternionf(sensor_rotation_body.data()), - Eigen::Vector3f(sensor_position_body.data())); + getTransformation(sensor_rotation_body, sensor_position_body); this->costWeights = std::make_unique(costsWeights); accLimits_ = {static_cast(ctrLimits.velXParams.maxAcceleration), static_cast(ctrLimits.velYParams.maxAcceleration), static_cast(ctrLimits.omegaParams.maxAcceleration)}; } -void CostEvaluator::updateCostWeights(TrajectoryCostsWeights& costsWeights) { +void CostEvaluator::updateCostWeights(TrajectoryCostsWeights &costsWeights) { this->costWeights = std::make_unique(costsWeights); } CostEvaluator::~CostEvaluator() { - //Clear custom cost pointers + // Clear custom cost pointers customTrajCostsPtrs_.clear(); }; TrajSearchResult CostEvaluator::getMinTrajectoryCost( const std::unique_ptr &trajs, - const Path::Path &reference_path, - const Path::Path &tracked_segment) { + const Path::Path *reference_path, const Path::Path &tracked_segment) { double weight; float total_cost; float ref_path_length; @@ -63,15 +58,17 @@ TrajSearchResult CostEvaluator::getMinTrajectoryCost( for (const auto &traj : *trajs) { total_cost = 0.0; - if ((ref_path_length = reference_path.totalPathLength()) > 0.0) { + if ((ref_path_length = reference_path->totalPathLength()) > 0.0) { if ((weight = costWeights->getParameter("goal_distance_weight")) > 0.0) { - float goalCost = goalCostFunc(traj, reference_path, ref_path_length); + float goalCost = + goalCostFunc(traj, reference_path->getEnd(), ref_path_length); total_cost += weight * goalCost; } if ((weight = costWeights->getParameter( "reference_path_distance_weight")) > 0.0) { - float refPathCost = pathCostFunc(traj, tracked_segment, tracked_segment.totalPathLength()); + float refPathCost = pathCostFunc(traj, tracked_segment, + tracked_segment.totalPathLength()); total_cost += weight * refPathCost; } } @@ -99,7 +96,7 @@ TrajSearchResult CostEvaluator::getMinTrajectoryCost( for (const auto &custom_cost : customTrajCostsPtrs_) { // custom cost functions takes in the trajectory and the reference path total_cost += - custom_cost->weight * custom_cost->evaluator_(traj, reference_path); + custom_cost->weight * custom_cost->evaluator_(traj, *reference_path); } if (total_cost < minCost) { @@ -123,8 +120,8 @@ float CostEvaluator::pathCostFunc(const Trajectory2D &trajectory, // infinity distError = DEFAULT_MIN_DIST; // Get minimum distance to the reference - for (size_t j = 0; j < tracked_segment.points.size(); ++j) { - dist = Path::Path::distance(tracked_segment.points[j], + for (size_t j = 0; j < tracked_segment.getSize(); ++j) { + dist = Path::Path::distance(tracked_segment.getIndex(j), trajectory.path.getIndex(i)); if (dist < distError) { distError = dist; @@ -146,11 +143,11 @@ float CostEvaluator::pathCostFunc(const Trajectory2D &trajectory, // Compute the cost of a trajectory based on distance to a given reference path float CostEvaluator::goalCostFunc(const Trajectory2D &trajectory, - const Path::Path &reference_path, + const Path::Point &reference_path_end_point, const float ref_path_length) { // end point distance normalized to range [0, 1] return Path::Path::distance(trajectory.path.getEnd(), - reference_path.getEnd()) / + reference_path_end_point) / ref_path_length; } diff --git a/src/kompass_cpp/kompass_cpp/src/utils/cost_evaluator_gpu.cpp b/src/kompass_cpp/kompass_cpp/src/utils/cost_evaluator_gpu.cpp index 6b6d8623..cd1a40e7 100644 --- a/src/kompass_cpp/kompass_cpp/src/utils/cost_evaluator_gpu.cpp +++ b/src/kompass_cpp/kompass_cpp/src/utils/cost_evaluator_gpu.cpp @@ -3,11 +3,8 @@ #include "datatypes/trajectory.h" #include "utils/cost_evaluator.h" #include "utils/logger.h" -#include -#include -#include -#include -#include +#include +#include #include #include @@ -33,16 +30,15 @@ CostEvaluator::CostEvaluator(TrajectoryCostsWeights &costsWeights, } CostEvaluator::CostEvaluator(TrajectoryCostsWeights &costsWeights, - const std::array &sensor_position_body, - const std::array &sensor_rotation_body, + const Eigen::Vector3f &sensor_position_body, + const Eigen::Quaternionf &sensor_rotation_body, ControlLimitsParams ctrLimits, size_t maxNumTrajectories, size_t numPointsPerTrajectory, size_t maxRefPathSegmentSize) { sensor_tf_body_ = - getTransformation(Eigen::Quaternionf(sensor_rotation_body.data()), - Eigen::Vector3f(sensor_position_body.data())); + getTransformation(sensor_rotation_body, sensor_position_body); this->costWeights = std::make_unique(costsWeights); accLimits_ = {static_cast(ctrLimits.velXParams.maxAcceleration), @@ -193,7 +189,7 @@ CostEvaluator::~CostEvaluator() { TrajSearchResult CostEvaluator::getMinTrajectoryCost( const std::unique_ptr &trajs, - const Path::Path &reference_path, const Path::Path &tracked_segment) { + const Path::Path *reference_path, const Path::Path &tracked_segment) { try { double weight; @@ -224,15 +220,17 @@ TrajSearchResult CostEvaluator::getMinTrajectoryCost( if ((costWeights->getParameter("reference_path_distance_weight") > 0.0 || costWeights->getParameter("goal_distance_weight") > 0.0) && - (ref_path_length = reference_path.totalPathLength()) > 0.0) { + (ref_path_length = reference_path->totalPathLength()) > 0.0) { if ((weight = costWeights->getParameter("goal_distance_weight")) > 0.0) { - events.push_back(goalCostFunc(trajs_size, reference_path.getEnd(), - ref_path_length, weight)); + auto last_point = reference_path->getEnd(); + m_deviceRefPathEnd = + sycl::vec(last_point.x(), last_point.y(), last_point.z()); + events.push_back(goalCostFunc(trajs_size, ref_path_length, weight)); } if ((weight = costWeights->getParameter( "reference_path_distance_weight")) > 0.0) { - size_t tracked_segment_size = tracked_segment.points.size(); + size_t tracked_segment_size = tracked_segment.getSize(); m_q.memcpy(m_devicePtrTrackedSegmentX, tracked_segment.getX().data(), sizeof(float) * tracked_segment_size) .wait(); @@ -279,7 +277,7 @@ TrajSearchResult CostEvaluator::getMinTrajectoryCost( // path m_devicePtrTempCosts[idx] += custom_cost->weight * - custom_cost->evaluator_(traj, reference_path); + custom_cost->evaluator_(traj, *reference_path); } idx += 1; } @@ -303,8 +301,7 @@ TrajSearchResult CostEvaluator::getMinTrajectoryCost( m_q.submit([&](sycl::handler &h) { h.depends_on(events); auto costs = m_devicePtrCosts; - auto minCost = m_minCost; - auto reduction = sycl::reduction(minCost, sycl::plus()); + auto reduction = sycl::reduction(m_minCost, sycl::plus()); // Kernel scope h.parallel_for( sycl::range<1>(trajs_size), reduction, @@ -427,7 +424,6 @@ sycl::event CostEvaluator::pathCostFunc(const size_t trajs_size, // Compute the cost of trajectory based on distance to the goal point sycl::event CostEvaluator::goalCostFunc(const size_t trajs_size, - const Path::Point &last_ref_point, const float ref_path_length, const double cost_weight) { // ----------------------------------------------------- @@ -447,7 +443,8 @@ sycl::event CostEvaluator::goalCostFunc(const size_t trajs_size, const float pathLength = ref_path_length; auto global_size = sycl::range<1>(trajs_size); // Last point of reference path - sycl::vec lastRefPoint = {last_ref_point.x(), last_ref_point.y()}; + sycl::vec lastRefPoint = {m_deviceRefPathEnd[0], + m_deviceRefPathEnd[1]}; // Kernel scope h.parallel_for( sycl::range<1>(global_size), [=](sycl::id<1> id) { diff --git a/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check.cpp b/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check.cpp index 265c7a04..817f0e66 100644 --- a/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check.cpp +++ b/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check.cpp @@ -13,9 +13,9 @@ namespace Kompass { CriticalZoneChecker::CriticalZoneChecker( const CollisionChecker::ShapeType robot_shape_type, const std::vector &robot_dimensions, - const std::array &sensor_position_body, - const std::array &sensor_rotation_body, - const float critical_angle, const float critical_distance) { + const Eigen::Vector3f &sensor_position_body, + const Eigen::Vector4f &sensor_rotation_body, const float critical_angle, + const float critical_distance, const float slowdown_distance) { // Construct a geometry object based on the robot shape if (robot_shape_type == CollisionChecker::ShapeType::CYLINDER) { robotHeight_ = robot_dimensions.at(1); @@ -31,8 +31,7 @@ CriticalZoneChecker::CriticalZoneChecker( // Init the sensor position w.r.t body sensor_tf_body_ = - getTransformation(Eigen::Quaternionf(sensor_rotation_body.data()), - Eigen::Vector3f(sensor_position_body.data())); + getTransformation(sensor_rotation_body, sensor_position_body); // Compute the critical zone angles min,max float angle_rad = critical_angle * M_PI / 180.0; angle_right_forward_ = angle_rad / 2; @@ -40,12 +39,19 @@ CriticalZoneChecker::CriticalZoneChecker( angle_right_backward_ = Angle::normalizeTo0Pi(M_PI + angle_right_forward_); angle_left_backward_ = Angle::normalizeTo0Pi(M_PI + angle_left_forward_); - LOG_DEBUG("Critical zone forward angles: [", angle_right_forward_, ", ", angle_left_forward_, "]"); + LOG_DEBUG("Critical zone forward angles: [", angle_right_forward_, ", ", + angle_left_forward_, "]"); LOG_DEBUG("Critical zone backward angles: [", angle_right_backward_, ", ", angle_left_backward_, "]"); // Set critical distance + if (slowdown_distance <= critical_distance) { + + throw std::invalid_argument( + "SlowDown distance must be greater than the Critical distance!"); + } critical_distance_ = critical_distance; + slowdown_distance_ = slowdown_distance; } void CriticalZoneChecker::preset(const std::vector &angles) { @@ -75,9 +81,9 @@ void CriticalZoneChecker::preset(const std::vector &angles) { preset_ = true; } -bool CriticalZoneChecker::check(const std::vector &ranges, - const std::vector &angles, - const bool forward) { +float CriticalZoneChecker::check(const std::vector &ranges, + const std::vector &angles, + const bool forward) { if (angles.size() != ranges.size()) { LOG_ERROR("Angles and ranges vectors must have the same size!"); return false; @@ -106,16 +112,19 @@ bool CriticalZoneChecker::check(const std::vector &ranges, // check if within the zone converted_range = std::sqrt(std::pow(cartesianPoint.y(), 2) + std::pow(cartesianPoint.x(), 2)); - - if (converted_range - robotRadius_ <= critical_distance_) { - return true; + float distance = converted_range - robotRadius_; + if (distance <= critical_distance_) { + return 0.0; + } else if (distance <= slowdown_distance_) { + return (distance - critical_distance_) / + (slowdown_distance_ - critical_distance_); } } else { preset_ = false; return check(ranges, angles, forward); } } - return false; + return 1.0; } } diff --git a/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check_gpu.cpp b/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check_gpu.cpp index adbea3cb..f5a57d4b 100644 --- a/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check_gpu.cpp +++ b/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check_gpu.cpp @@ -1,21 +1,17 @@ #include "utils/critical_zone_check_gpu.h" #include "utils/logger.h" -#include -#include #include namespace Kompass { -bool CriticalZoneCheckerGPU::check(const std::vector &ranges, - const std::vector &angles, - const bool forward) { +float CriticalZoneCheckerGPU::check(const std::vector &ranges, + const std::vector &angles, + const bool forward) { try { - m_q.fill(m_devicePtrOutput, false, m_scan_in_zone); + m_q.fill(m_devicePtrOutput, 1.0f, m_scan_in_zone); - m_q.memcpy(m_devicePtrAngles, angles.data(), sizeof(double) * m_scanSize) - .wait(); - m_q.memcpy(m_devicePtrRanges, ranges.data(), sizeof(double) * m_scanSize) - .wait(); + m_q.memcpy(m_devicePtrAngles, angles.data(), sizeof(double) * m_scanSize); + m_q.memcpy(m_devicePtrRanges, ranges.data(), sizeof(double) * m_scanSize); // command scope m_q.submit([&](sycl::handler &h) { @@ -34,7 +30,10 @@ bool CriticalZoneCheckerGPU::check(const std::vector &ranges, const auto devicePtrAngles = m_devicePtrAngles; const auto devicePtrOutput = m_devicePtrOutput; const auto criticalDistance = critical_distance_; + const auto slowdownDistance = slowdown_distance_; size_t *critical_indices; + float *result = m_result; + *result = 1.0f; sycl::range<1> global_size; if (forward) { critical_indices = m_devicePtrForward; @@ -60,23 +59,23 @@ bool CriticalZoneCheckerGPU::check(const std::vector &ranges, transformed_point[1] += y_transform[i] * point[i]; } - double converted_range = sycl::length(transformed_point); + float converted_range = sycl::length(transformed_point); if (converted_range - robot_radius <= criticalDistance) { - // point within the zone and range is low - devicePtrOutput[idx] = true; + devicePtrOutput[idx] = 0.0f; + } else if (converted_range - robot_radius <= slowdownDistance) { + devicePtrOutput[idx] = + (converted_range - robot_radius - criticalDistance) / + (slowdownDistance - criticalDistance); + } else { + devicePtrOutput[idx] = 1.0f; } - }); - }).wait(); - *m_result = false; - // Launch a kernel that reduces the array using a logical OR operation. - // If any element is true, the reduction will produce true. - m_q.submit([&](sycl::handler &h) { - auto reduction = sycl::reduction(m_result, sycl::logical_or()); - auto devicePtrOutput = m_devicePtrOutput; - h.parallel_for( - sycl::range<1>(m_scan_in_zone), reduction, - [=](sycl::id<1> idx, auto &r) { r.combine(devicePtrOutput[idx]); }); + sycl::atomic_ref + atomic_cost(*result); + atomic_cost.fetch_min(devicePtrOutput[idx]); + }); }); m_q.wait_and_throw(); diff --git a/src/kompass_cpp/kompass_cpp/src/utils/gpu_check.cpp b/src/kompass_cpp/kompass_cpp/src/utils/gpu_check.cpp new file mode 100644 index 00000000..85c742ed --- /dev/null +++ b/src/kompass_cpp/kompass_cpp/src/utils/gpu_check.cpp @@ -0,0 +1,22 @@ +#include + +#ifdef GPU +#include +#endif + +std::string getAvailableAccelerators() { +#ifdef GPU + std::string result; + auto platforms = sycl::platform::get_platforms(); + + for (const auto &platform : platforms) { + for (const auto &device : platform.get_devices()) { + result += device.get_info() + "\n"; + } + } + + return result; +#else + return ""; +#endif +} diff --git a/src/kompass_cpp/kompass_cpp/src/utils/kalman_filter.cpp b/src/kompass_cpp/kompass_cpp/src/utils/kalman_filter.cpp new file mode 100644 index 00000000..1a8bd67e --- /dev/null +++ b/src/kompass_cpp/kompass_cpp/src/utils/kalman_filter.cpp @@ -0,0 +1,114 @@ +#define EIGEN_DONT_PARALLELIZE // TODO: resolve error when enabling Eigen + // parallelization +#include "utils/kalman_filter.h" +#include "utils/logger.h" + +namespace Kompass { + +LinearSSKalmanFilter::LinearSSKalmanFilter(const size_t num_states, + const size_t num_inputs) { + state.resize(num_states, 1); + // State update eq: X_dot = A X + B U + Q + A.resize(num_states, num_states); + B.resize(num_states, num_inputs); + Q.resize(num_states, num_states); + + // Observation eq: Z = H X + R + H.resize(num_states, num_states); + R.resize(num_states, num_states); + + P = Eigen::MatrixXf::Identity(num_states, num_states); +} + +bool LinearSSKalmanFilter::setup(const Eigen::MatrixXf &A, + const Eigen::MatrixXf &B, + const Eigen::MatrixXf &Q, + const Eigen::MatrixXf &H, + const Eigen::MatrixXf &R) { + + if ((A.size() != this->A.size()) || (B.size() != this->B.size()) || + (Q.size() != this->Q.size()) || (H.size() != this->H.size()) || + (R.size() != this->R.size())) { + LOG_ERROR("Cannot setup the KalmanFilter. Matrix size error. Expected the " + "following sized: A=", + this->A.size(), ", B=", this->B.size(), ", H=", this->H.size(), + ", Q=", this->Q.size(), ", R=", this->R.size()); + return false; + } + this->A = A; + this->B = B; + this->H = H; + this->R = R; + this->Q = Q; + this->system_initialized = true; + return true; +} + +void LinearSSKalmanFilter::setInitialState( + const Eigen::VectorXf &initial_state) { + if (initial_state.size() != this->state.size()) { + LOG_ERROR("Cannot set initial state. Expected the " + "following sized: ", + this->state.size()); + throw std::length_error("Error Setting Initial State"); + } + this->state = initial_state; + this->state_initialized = true; +} + +void LinearSSKalmanFilter::setA(const Eigen::MatrixXf &A) { this->A = A; } + +void LinearSSKalmanFilter::estimate(const Eigen::MatrixXf &measurements, + const Eigen::MatrixXf &inputs, + const int numberSteps) { + Eigen::MatrixXf A_transpose = this->A.transpose(); + Eigen::MatrixXf B_inputs = this->B * inputs; + Eigen::MatrixXf predicted_state = this->state; + // predict a new state after number of steps + for (int i = 0; i < numberSteps; i++) { + predicted_state = this->A * predicted_state + B_inputs; + // Covariance Extrapolation Equation + this->P = this->A * this->P * A_transpose + this->Q; + } + + // Innovation Matrix + Eigen::MatrixXf S = this->R + this->H * this->P * this->H.transpose(); + + // Kalman Gain Matrix + Eigen::MatrixXf K = this->P * this->H.transpose() * S.inverse(); + + // Update the state + this->state = + predicted_state + K * (measurements - this->H * predicted_state); + + // Update the estimation uncertainty + this->P = (Eigen::MatrixXf::Identity(this->P.rows(), this->P.cols()) - + K * this->H) * + this->P; +} + +void LinearSSKalmanFilter::estimate(const Eigen::MatrixXf &measurements, + const int numberSteps) { + // estimate with zero inputs + auto size = this->B.cols(); + Eigen::MatrixXf inputs; + inputs.resize(size, 1); + inputs = Eigen::MatrixXf::Zero(size, 1); + this->estimate(measurements, inputs); +} + +double LinearSSKalmanFilter::getState(const size_t state_index) { + if (this->state_initialized) { + return this->state(state_index, 0); + } else { + return 0; + } +} + +std::optional LinearSSKalmanFilter::getState() { + if (this->state_initialized and this->system_initialized) { + return this->state; + } + return std::nullopt; +} +} // namespace Kompass diff --git a/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp b/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp index 09400f1f..19712a9f 100644 --- a/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp +++ b/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp @@ -5,6 +5,7 @@ #include #include +#include "datatypes/control.h" #include "datatypes/path.h" #include "datatypes/trajectory.h" #include "utils/logger.h" @@ -26,8 +27,8 @@ TrajectorySampler::TrajectorySampler( double predictionHorizon, double controlHorizon, int maxLinearSamples, int maxAngularSamples, const CollisionChecker::ShapeType robotShapeType, const std::vector robotDimensions, - const std::array &sensor_position_body, - const std::array &sensor_rotation_body, const double octreeRes, + const Eigen::Vector3f &sensor_position_body, + const Eigen::Quaternionf &sensor_rotation_body, const double octreeRes, const int maxNumThreads) { // Setup the collision checker collChecker = std::make_unique( @@ -61,8 +62,8 @@ TrajectorySampler::TrajectorySampler( TrajectorySamplerParameters config, ControlLimitsParams controlLimits, ControlType controlType, const CollisionChecker::ShapeType robotShapeType, const std::vector robotDimensions, - const std::array &sensor_position_body, - const std::array &sensor_rotation_body, const int maxNumThreads) { + const Eigen::Vector3f &sensor_position_body, + const Eigen::Quaternionf &sensor_rotation_body, const int maxNumThreads) { double octreeRes = config.getParameter("octree_map_resolution"); // Setup the collision checker collChecker = std::make_unique( @@ -97,6 +98,10 @@ void TrajectorySampler::setSampleDroppingMode(const bool drop_samples) { this->drop_samples_ = drop_samples; } +float TrajectorySampler::getRobotRadius() const { + return collChecker->getRadius(); +} + void TrajectorySampler::updateParams(TrajectorySamplerParameters config) { time_step_ = config.getParameter("time_step"); max_time_ = config.getParameter("prediction_horizon"); @@ -123,14 +128,7 @@ void TrajectorySampler::getAdmissibleTrajsFromVel( size_t last_free_index{numPointsPerTrajectory - 1}; for (size_t i = 0; i < (numPointsPerTrajectory - 1); ++i) { - - simulated_pose.x += (vel.vx() * cos(simulated_pose.yaw) - - vel.vy() * sin(simulated_pose.yaw)) * - time_step_; - simulated_pose.y += (vel.vx() * sin(simulated_pose.yaw) + - vel.vy() * cos(simulated_pose.yaw)) * - time_step_; - simulated_pose.yaw += vel.omega() * time_step_; + simulated_pose.update(vel, time_step_); // Update the position of the robot in the collision checker (updates the // robot collision object) No need to update the Octree (laserscan) @@ -199,13 +197,8 @@ void TrajectorySampler::getAdmissibleTrajsFromVelDiffDrive( simulated_pose.yaw += vel.omega() * time_step_; temp_vel = Velocity2D(0.0, 0.0, vel.omega(), vel.steer_ang()); } else { - simulated_pose.x += (vel.vx() * cos(simulated_pose.yaw) - - vel.vy() * sin(simulated_pose.yaw)) * - time_step_; - simulated_pose.y += (vel.vx() * sin(simulated_pose.yaw) + - vel.vy() * cos(simulated_pose.yaw)) * - time_step_; temp_vel = Velocity2D(vel.vx(), vel.vy(), 0.0, 0.0); + simulated_pose.update(temp_vel, time_step_); } // Update the position of the robot in the collision checker (updates the @@ -511,5 +504,78 @@ void TrajectorySampler::UpdateReachableVelocityRange( std::max((max_omega_ - min_omega_) / (ang_samples_max_ - 1), 0.001); } +void TrajectorySampler::updateState(const Path::State ¤t_state) { + collChecker->updateState(current_state); +} + +template <> +bool TrajectorySampler::checkStatesFeasibility( + const std::vector &states, const LaserScan &scan) { + // collChecker->updateState(states[0]); + collChecker->updateScan(scan.ranges, scan.angles); + for (auto state : states) { + collChecker->updateState(state); + // Update the PointCloud values + if (collChecker->checkCollisions()) { + return true; + } + } + return false; +} + +template <> +bool TrajectorySampler::checkStatesFeasibility>( + const std::vector &states, + const std::vector &cloud) { + // collChecker->updateState(states[0]); + collChecker->updatePointCloud(cloud); + for (auto state : states) { + collChecker->updateState(state); + // Update the PointCloud values + if (collChecker->checkCollisions()) { + return true; + } + } + return false; +} + +Trajectory2D +TrajectorySampler::generateSingleSampleFromVel(const Velocity2D &vel, + const Path::State &pose) { + Path::State simulated_pose = pose; + Trajectory2D trajectory(numPointsPerTrajectory); + trajectory.path.add(0, simulated_pose.x, simulated_pose.y); + if (ctrType == ControlType::DIFFERENTIAL_DRIVE) { + for (size_t i = 0; i < (numPointsPerTrajectory - 1); ++i) { + if (std::abs(vel.vx()) > MIN_VEL && std::abs(vel.omega()) > MIN_VEL) { + // Rotate then move + Velocity2D tempVel = vel; + tempVel.setVx(0.0); + simulated_pose.update(tempVel, time_step_); + trajectory.path.add(i + 1, simulated_pose.x, simulated_pose.y); + trajectory.velocities.add(i, vel); + + tempVel.setVx(vel.vx()); + tempVel.setOmega(0.0); + simulated_pose.update(tempVel, time_step_); + trajectory.path.add(i + 1, simulated_pose.x, simulated_pose.y); + trajectory.velocities.add(i, vel); + i++; + } + // Else apply directly + simulated_pose.update(vel, time_step_); + trajectory.path.add(i + 1, simulated_pose.x, simulated_pose.y); + trajectory.velocities.add(i, vel); + } + } else { + for (size_t i = 0; i < (numPointsPerTrajectory - 1); ++i) { + simulated_pose.update(vel, time_step_); + trajectory.path.add(i + 1, simulated_pose.x, simulated_pose.y); + trajectory.velocities.add(i, vel); + } + } + return trajectory; +} + }; // namespace Control } // namespace Kompass diff --git a/src/kompass_cpp/kompass_cpp/src/vision/depth_detector.cpp b/src/kompass_cpp/kompass_cpp/src/vision/depth_detector.cpp new file mode 100644 index 00000000..0a16d1e7 --- /dev/null +++ b/src/kompass_cpp/kompass_cpp/src/vision/depth_detector.cpp @@ -0,0 +1,164 @@ +#include "vision/depth_detector.h" +#include "datatypes/tracking.h" +#include "utils/logger.h" +#include "utils/transformation.h" +#include +#include +#include + +namespace Kompass { + +DepthDetector::DepthDetector(const Eigen::Vector2f &depth_range, + const Eigen::Vector3f &camera_in_body_translation, + const Eigen::Quaternionf &camera_in_body_rotation, + const Eigen::Vector2f &focal_length, + const Eigen::Vector2f &principal_point, + const float depth_conversion_factor) { + // Set camera tf + auto body_to_camera_tf = + getTransformation(camera_in_body_rotation, camera_in_body_translation); + DepthDetector(depth_range, body_to_camera_tf, focal_length, principal_point, + depth_conversion_factor); +} + +DepthDetector::DepthDetector( + const Eigen::Vector2f &depth_range, + const Eigen::Isometry3f &camera_in_body_tf, + const Eigen::Vector2f &focal_length, const Eigen::Vector2f &principal_point, + const float depth_conversion_factor) { // Range of interest for depth values + // in meters + minDepth_ = depth_range(0); + maxDepth_ = depth_range(1); + // Factor to convert depth image data to meters (in ROS2 its given in mm -> + // depthConversionFactor = 1e-3) + depthConversionFactor_ = depth_conversion_factor; + // Set camera tf + camera_in_body_tf_ = camera_in_body_tf; + + // Set camera intrinsic parameters + fx_ = focal_length.x(); + fy_ = focal_length.y(); + cx_ = principal_point.x(); + cy_ = principal_point.y(); + + body_in_world_tf_ = Eigen::Isometry3f::Identity(); +} + +std::optional> DepthDetector::get3dDetections() const { + if (boxes_) { + return *boxes_; + } + return std::nullopt; +} + +void DepthDetector::updateBoxes( + const Eigen::MatrixX aligned_depth_img, + const std::vector &detections, + const std::optional &robot_state) { + if (robot_state.has_value()) { + body_in_world_tf_ = getTransformation(robot_state.value()); + } + alignedDepthImg_ = aligned_depth_img; + boxes_ = std::make_unique>(); + for (auto box2d : detections) { + auto converted_box = convert2Dboxto3Dbox(box2d); + if (converted_box) { + boxes_->push_back(converted_box.value()); + } + } +} + +std::optional DepthDetector::convert2Dboxto3Dbox(const Bbox2D &box2d) { + Bbox3D box3d(box2d); + Eigen::Vector2i x_limits = box2d.getXLimits(); + Eigen::Vector2i y_limits = box2d.getYLimits(); + float depth_meters; + // All depth values in the 2D box within the range of interest + std::vector depth_values; + for (int row_idx = y_limits(0); row_idx <= y_limits(1); ++row_idx) { + for (int col_idx = x_limits(0); col_idx <= x_limits(1); ++col_idx) { + depth_meters = + alignedDepthImg_(row_idx, col_idx) * depthConversionFactor_; + if (depth_meters <= maxDepth_ && depth_meters >= minDepth_) { + depth_values.push_back(depth_meters); + } + } + } + if (depth_values.size() <= 1) { + LOG_WARNING("Could not get any depth values for 2D bounding box at ", + box2d.top_corner.x(), ", ", box2d.top_corner.y()); + return std::nullopt; + } + float medianDepth, madDepth; + calculateMAD(depth_values, medianDepth, madDepth); + + // Get min and max depth + float minimum_d = maxDepth_, maximum_d = minDepth_; + for (auto depth : depth_values) { + if ((depth < minimum_d) && (depth >= medianDepth - 1.5 * madDepth)) { + minimum_d = depth; + } + if ((depth > maximum_d) && (depth <= medianDepth + 1.5 * madDepth)) { + maximum_d = depth; + } + } + + // Convert from 2D box center in the pixel frame to the 3D box center in the + // camera frame + Eigen::Vector3f center_in_camera_frame, size_camera_frame; + + center_in_camera_frame(0) = + (box2d.top_corner.x() + 0.5 * box2d.size.x() - cx_) * medianDepth / + this->fx_; + center_in_camera_frame(1) = + (box2d.top_corner.y() + 0.5 * box2d.size.y() - cy_) * medianDepth / + this->fy_; + center_in_camera_frame(2) = medianDepth; + + LOG_DEBUG("Median depth = ", medianDepth, ", min=", minimum_d, + ", max=", maximum_d); + + // Size in meters + size_camera_frame(0) = box2d.size.x() * medianDepth / this->fx_; + size_camera_frame(1) = box2d.size.y() * medianDepth / this->fy_; + size_camera_frame(2) = maximum_d - minimum_d; + + Eigen::Isometry3f camera_in_world_tf = body_in_world_tf_ * camera_in_body_tf_; + // Register center in the world frame + box3d.center = camera_in_world_tf * center_in_camera_frame; + + LOG_DEBUG("Got detected box in 3D coordinates at :", box3d.center.x(), ", ", + box3d.center.y(), ", ", box3d.center.z()); + + // Transform size from camera frame to world frame + Eigen::Matrix3f abs_rotation = camera_in_world_tf.linear().cwiseAbs(); + box3d.size = abs_rotation * size_camera_frame; + + return box3d; +} + +float DepthDetector::getMedian(const std::vector &values) { + float median; + auto sorted_value = values; + std::sort(sorted_value.begin(), sorted_value.end()); + // number of items + auto num = sorted_value.size() - 1; + if (num % 2 == 0) { + median = (sorted_value[num / 2] + sorted_value[num / 2 + 1]) / 2; + } else { + median = sorted_value[(num + 1) / 2]; + } + return median; +} + +void DepthDetector::calculateMAD(const std::vector &depthValues, + float &median, float &mad) { + median = getMedian(depthValues); + + std::vector deviations; + for (size_t i = 0; i < depthValues.size(); ++i) { + deviations.push_back(std::abs(depthValues[i] - median)); + } + mad = getMedian(deviations); +} +} // namespace Kompass diff --git a/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp b/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp new file mode 100644 index 00000000..9e88dbd2 --- /dev/null +++ b/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp @@ -0,0 +1,259 @@ +#include "vision/tracker.h" +#include "datatypes/control.h" +#include "datatypes/tracking.h" +#include "utils/logger.h" +#include +#include + +namespace Kompass { + +FeatureBasedBboxTracker::FeatureBasedBboxTracker(const float &time_step, + const float &e_pos, + const float &e_vel, + const float &e_acc) { + + timeStep_ = time_step; + // Setup Kalman filter matrices + Eigen::MatrixXf A; + A.resize(StateSize, StateSize); + + A << 1, 0, 0, time_step, 0, 0, 0.5 * pow(time_step, 2), 0, 0, 0, 1, 0, 0, + time_step, 0, 0, 0.5 * pow(time_step, 2), 0, 0, 0, 1, 0, 0, time_step, 0, + 0, 0.5 * pow(time_step, 2), 0, 0, 0, 1, 0, 0, time_step, 0, 0, 0, 0, 0, 0, + 1, 0, 0, time_step, 0, 0, 0, 0, 0, 0, 1, 0, 0, time_step, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; + + Eigen::MatrixXf B = Eigen::MatrixXf::Zero(StateSize, 1); + Eigen::MatrixXf H = Eigen::MatrixXf::Identity(StateSize, StateSize); + Eigen::MatrixXf Err = Eigen::MatrixXf::Identity(StateSize, StateSize); + Err(0, 0) *= e_pos; + Err(1, 1) *= e_pos; + Err(2, 2) *= e_pos; + Err(3, 3) *= e_vel; + Err(4, 4) *= e_vel; + Err(5, 5) *= e_vel; + Err(6, 6) *= e_acc; + Err(7, 7) *= e_acc; + Err(8, 8) *= e_acc; + + stateKalmanFilter_ = std::make_unique(StateSize, 1); + stateKalmanFilter_->setup(A, B, Err, H, Err); +} + +bool FeatureBasedBboxTracker::setInitialTracking(const TrackedBbox3D &bBox) { + trackedBox_ = std::make_unique(bBox); + trackedLabel_ = bBox.box.label; + Eigen::VectorXf state_vec; + state_vec.resize(StateSize); + state_vec(0) = bBox.box.center[0]; // x + state_vec(1) = bBox.box.center[1]; // y + state_vec(2) = bBox.yaw(); // yaw + state_vec(3) = bBox.vel[0]; // vx + state_vec(4) = bBox.vel[1]; // vy + state_vec(5) = bBox.omega(); // omega + state_vec(6) = bBox.acc[0]; // ax + state_vec(7) = bBox.acc[1]; // ay + state_vec(8) = bBox.ang_acc(); // a_yaw + stateKalmanFilter_->setInitialState(state_vec); + return true; +} + +bool FeatureBasedBboxTracker::setInitialTracking(const Bbox3D &bBox, + const float yaw) { + LOG_DEBUG("Setting initial tracked box"); + trackedBox_ = std::make_unique(bBox); + trackedLabel_ = bBox.label; + Eigen::Matrix state_vec = + Eigen::Matrix::Zero(); + state_vec(0) = bBox.center.x(); + state_vec(1) = bBox.center.y(); + state_vec(2) = yaw; + stateKalmanFilter_->setInitialState(state_vec); + return true; +} + +bool FeatureBasedBboxTracker::setInitialTracking( + const int &pose_x_img, const int &pose_y_img, + const std::vector &detected_boxes, const float yaw) { + std::unique_ptr target_box; + // Find a detected box containing the point + for (auto box : detected_boxes) { + auto limits_x = box.getXLimitsImg(); + if (pose_x_img >= limits_x(0) and pose_x_img <= limits_x(1)) { + auto limits_y = box.getYLimitsImg(); + if (pose_y_img >= limits_y(0) and pose_y_img <= limits_y(1)) { + target_box = std::make_unique(box); + break; + } + } + } + if (!target_box) { + // given position was not found inside any detected box + return false; + } + return setInitialTracking(*target_box, yaw); +} + +bool FeatureBasedBboxTracker::trackerInitialized() const { + if (trackedBox_) { + return true; + } + return false; +} + +void FeatureBasedBboxTracker::updateTrackedBoxState(const int numberSteps) { + Eigen::MatrixXf measurement; + measurement.resize(StateSize, 1); + measurement(0) = trackedBox_->box.center.x(); + measurement(1) = trackedBox_->box.center.y(); + measurement(2) = trackedBox_->yaw(); + measurement(3) = trackedBox_->vel.x(); + measurement(4) = trackedBox_->vel.y(); + measurement(5) = trackedBox_->omega(); + measurement(6) = trackedBox_->acc.x(); + measurement(7) = trackedBox_->acc.y(); + measurement(8) = trackedBox_->ang_acc(); + stateKalmanFilter_->estimate(measurement, numberSteps); +} + +bool FeatureBasedBboxTracker::updateTracking( + const std::vector &detected_boxes) { + std::vector label_boxes; + for(auto box : detected_boxes) { + if(box.label == trackedLabel_) { + label_boxes.push_back(box); + } + } + if (label_boxes.empty()) { + LOG_DEBUG("No boxes with label ", trackedLabel_, " found in the detected boxes!"); + return false; + } + + float max_similarity_score = 0.0f; // Similarity score + Bbox3D * found_box; + float dt = label_boxes[0].timestamp - trackedBox_->box.timestamp; + + if(label_boxes.size() == 1) { + // Only one box detected, so it is the same + max_similarity_score = 1.0f; + found_box = &label_boxes[0]; + } + else{ + // Compute similarity score and find box + // Predicted the new location of the tracked box + auto predicted_tracked_box = trackedBox_->predictConstantAcc(dt); + + auto ref_box_features = extractFeatures(predicted_tracked_box); + + // Get the features of all the new detections + FeaturesVector detected_boxes_feature_vec; + size_t similar_box_idx = 0, count = 0; + + for (auto box : label_boxes) { + detected_boxes_feature_vec = extractFeatures(box); + FeaturesVector error_vec = detected_boxes_feature_vec - ref_box_features; + // Error vector normalization + for (int i = 0; i < error_vec.size(); ++i) { + if (std::abs(ref_box_features(i)) > 0.0) { + error_vec(i) = error_vec(i) / std::abs(ref_box_features(i)); + } + } + float similarity_score = std::exp(-std::pow(error_vec.norm(), 2)); + + if (similarity_score > max_similarity_score) { + max_similarity_score = similarity_score; + similar_box_idx = count; + } + count++; + } + found_box = &label_boxes[similar_box_idx]; + } + + LOG_DEBUG("Max similarity score = ", max_similarity_score); + if (max_similarity_score > minAcceptedSimilarityScore_) { + // Update raw tracking + float dt = found_box->timestamp - trackedBox_->box.timestamp; + int number_steps = std::max(static_cast(dt / timeStep_), 1); + + trackedBox_->updateFromNewDetection(*found_box); + + // Update state by applying kalman filter on the raw measurement + updateTrackedBoxState(number_steps); + LOG_DEBUG("Update after ", number_steps, + " timer steps, Box Now updated to: ", trackedBox_->box.center.x(), + ", ", trackedBox_->box.center.y(), ", v=", trackedBox_->v()); + return true; + } + LOG_DEBUG("Box not found in the detected boxes! Max similarity score = ", + max_similarity_score, ", min accepted = ", + minAcceptedSimilarityScore_); + return false; +} + +FeatureBasedBboxTracker::FeaturesVector +FeatureBasedBboxTracker::extractFeatures( + const TrackedBbox3D &bBox_tracked) const { + return extractFeatures(bBox_tracked.box); +} + +FeatureBasedBboxTracker::FeaturesVector +FeatureBasedBboxTracker::extractFeatures(const Bbox3D &bBox) const { + FeatureBasedBboxTracker::FeaturesVector features_vec = + Eigen::Vector::Zero(); + features_vec.segment<2>(0) = bBox.center.segment<2>(0); // indices 0, 1 + features_vec.segment<3>(2) = bBox.size; // indices 2, 3, 4 + features_vec(5) = bBox.pc_points.size(); + if (features_vec(5) > 0.0) { + // Compute point cloud points standard deviation + auto std_vec = this->computePointsStdDev(bBox.pc_points); + features_vec.segment<3>(6) = std_vec; // indices 6, 7, 8 + } + return features_vec; +} + +std::optional FeatureBasedBboxTracker::getRawTracking() const { + if (trackedBox_) { + return *trackedBox_; + } + return std::nullopt; +} + +std::optional +FeatureBasedBboxTracker::getTrackedState() const { + if (trackedBox_) { + return stateKalmanFilter_->getState(); + } + return std::nullopt; +} + +std::optional +FeatureBasedBboxTracker::getFilteredTrackedPose2D() const { + if (trackedBox_) { + auto state_vec = stateKalmanFilter_->getState().value(); + return Control::TrackedPose2D(state_vec(0), state_vec(1), state_vec(2), + state_vec(3), state_vec(4), state_vec(5)); + } + return std::nullopt; +} + +Eigen::Vector3f FeatureBasedBboxTracker::computePointsStdDev( + const std::vector &pc_points) const { + // compute the mean in each direction + auto size = std::max(int(pc_points.size() - 1), 1); + Eigen::Vector3f mean = Eigen::Vector3f::Zero(); + for (auto point : pc_points) { + mean += point; + } + mean /= size; + // Compute the variance + Eigen::Vector3f variance = Eigen::Vector3f::Zero(); + for (auto point : pc_points) { + auto diff = point - mean; + variance += diff.cwiseProduct(diff); + } + variance /= size; + // return the standard deviation + return variance.array().sqrt(); +} + +} // namespace Kompass diff --git a/src/kompass_cpp/tests/CMakeLists.txt b/src/kompass_cpp/tests/CMakeLists.txt index 5d0e0362..8e85866c 100644 --- a/src/kompass_cpp/tests/CMakeLists.txt +++ b/src/kompass_cpp/tests/CMakeLists.txt @@ -2,8 +2,11 @@ set(MODULE_NAME kompass) find_package(nlohmann_json 3.2.0 REQUIRED) +find_package(OpenCV REQUIRED) find_package(Boost COMPONENTS unit_test_framework REQUIRED) +include_directories( ${OpenCV_INCLUDE_DIRS} ) + # Specify the location of the Python script set(PYTHON_SCRIPT ${CMAKE_CURRENT_SOURCE_DIR}/trajectory_sampler_plt.py) # Create a symbolic link for the Python script @@ -34,6 +37,21 @@ add_executable(collisions_test collisions_test.cpp) target_link_libraries(collisions_test PRIVATE ${MODULE_NAME} Boost::unit_test_framework) add_test(collisions_tests collisions_test) +# Vision DWA test +add_executable(vision_dwa_test vision_dwa_test.cpp) +target_link_libraries(vision_dwa_test PRIVATE ${MODULE_NAME} Boost::unit_test_framework ${OpenCV_LIBS}) +add_test(vision_tests vision_dwa_test) + +# Set path to the test image +add_executable(vision_tracking_test vision_tracking_test.cpp) +target_link_libraries(vision_tracking_test PRIVATE ${MODULE_NAME} Boost::unit_test_framework nlohmann_json::nlohmann_json) +add_test(vision_tests vision_tracking_test) + +# Set path to the test image +add_executable(vision_detector_test vision_detector_test.cpp) +target_link_libraries(vision_detector_test PRIVATE ${MODULE_NAME} Boost::unit_test_framework ${OpenCV_LIBS}) +add_test(vision_tests vision_detector_test) + # find sycl find_package(AdaptiveCpp) diff --git a/src/kompass_cpp/tests/collisions_test.cpp b/src/kompass_cpp/tests/collisions_test.cpp index 0b1e4f34..25ac6e13 100644 --- a/src/kompass_cpp/tests/collisions_test.cpp +++ b/src/kompass_cpp/tests/collisions_test.cpp @@ -1,4 +1,5 @@ #include "test.h" +#include #define BOOST_TEST_MODULE KOMPASS TESTS #include "utils/angles.h" #include "utils/collision_check.h" @@ -63,8 +64,8 @@ BOOST_AUTO_TEST_CASE(test_FCL) { auto robotShapeType = CollisionChecker::ShapeType::BOX; std::vector robotDimensions{0.4, 0.4, 1.0}; - const std::array sensor_position_body{0.0, 0.0, 1.0}; - const std::array sensor_rotation_body{0, 0, 0, 1}; + const Eigen::Vector3f sensor_position_body{0.0, 0.0, 1.0}; + const Eigen::Quaternionf sensor_rotation_body{0, 0, 0, 1}; // Robot start state (pose) Path::State robotState(0.0, 0.0, 0.0, 0.0); @@ -78,30 +79,26 @@ BOOST_AUTO_TEST_CASE(test_FCL) { octreeRes); LOG_INFO("Testing collision checker using Laserscan data"); - + LOG_INFO("Running test with robot at ", robotState.x, ", ", robotState.y); + LOG_INFO("Scan Ranges ", scan_ranges[0], ", ", scan_ranges[1], ", ", + scan_ranges[2]); collChecker.updateState(robotState); bool res_false = collChecker.checkCollisions(scan_ranges, scan_angles); - LOG_INFO("Testing collision between: \nRobot at {x: ", robotState.x, - ", y: ", robotState.y, "}\n", - "and Laserscan with: ranges {1.0, 1.0, 1.0, 1.0} at angles {0, 0.1, " - "0.2, 3.14}, Collision: ", - res_false); BOOST_TEST(!res_false, "Collision Result should be FALSE Got: " << res_false); + std::cout << std::endl; robotState.x = 3.0; robotState.y = 5.0; - collChecker.updateState(robotState); - scan_ranges = {0.25, 0.5, 0.5}; + LOG_INFO("Running test with robot at ", robotState.x, ", ", robotState.y); + LOG_INFO("Scan Ranges ", scan_ranges[0], ", ", scan_ranges[1], ", ", + scan_ranges[2]); + collChecker.updateState(robotState); bool res_true = collChecker.checkCollisions(scan_ranges, scan_angles); - LOG_INFO("Testing collision between: \nRobot at {x: ", robotState.x, - ", y: ", robotState.y, "}\n", - "and Laserscan with: ranges {0.2, 0.5, 0.5} at angles {0, 0.1, " - "0.2, 3.14} -> Collision: ", - res_true); BOOST_TEST(res_true, "Collision Result should be TRUE got: " << res_true); + std::cout << std::endl; LOG_INFO("Testing collision between: \nRobot at {x: ", robotState.x, ", y: ", robotState.y, "}\n", "and Pointcloud"); @@ -123,8 +120,8 @@ BOOST_AUTO_TEST_CASE(test_critical_zone_check) { auto robotShapeType = CollisionChecker::ShapeType::BOX; std::vector robotDimensions{0.51, 0.27, 0.4}; - const std::array sensor_position_body{0.22, 0.0, 0.4}; - const std::array sensor_rotation_body{0, 0, 0.99, 0.0}; + const Eigen::Vector3f sensor_position_body{0.22, 0.0, 0.4}; + const Eigen::Vector4f sensor_rotation_body{0, 0, 0.99, 0.0}; // Robot laserscan value std::vector scan_angles; @@ -132,11 +129,13 @@ BOOST_AUTO_TEST_CASE(test_critical_zone_check) { initLaserscan(360, 10.0, scan_ranges, scan_angles); bool forward_motion = true; - float critical_angle = 120.0, critical_distance = 0.3; + float critical_angle = 160.0, critical_distance = 0.3, + slowdown_distance = 0.6; CriticalZoneChecker zoneChecker(robotShapeType, robotDimensions, sensor_position_body, sensor_rotation_body, - critical_angle, critical_distance); + critical_angle, critical_distance, + slowdown_distance); LOG_INFO("Testing Emergency Stop with CPU"); @@ -144,21 +143,22 @@ BOOST_AUTO_TEST_CASE(test_critical_zone_check) { setLaserscanAtAngle(0.0, 0.2, scan_ranges, scan_angles); setLaserscanAtAngle(0.1, 0.2, scan_ranges, scan_angles); setLaserscanAtAngle(-0.1, 0.2, scan_ranges, scan_angles); - bool result = zoneChecker.check(scan_ranges, scan_angles, forward_motion); - BOOST_TEST(!result, "Angles are behind and robot is moving forward -> " - "Critical zone result should be FALSE, returned " - << result); - if (!result) { + float result = zoneChecker.check(scan_ranges, scan_angles, forward_motion); + BOOST_TEST(result == 1.0, "Angles are behind and robot is moving forward -> " + "Critical zone result should be 1.0, returned " + << result); + if (result == 1.0) { LOG_INFO("Test1 PASSED: Angles are behind and robot is moving forward"); } // Set small ranges behind the robot initLaserscan(360, 10.0, scan_ranges, scan_angles); result = zoneChecker.check(scan_ranges, scan_angles, forward_motion); - BOOST_TEST(!result, "Angles are in front and far and robot is moving forward " - "-> Critical zone result should be FALSE, returned " - << result); - if (!result) { + BOOST_TEST(result == 1.0, + "Angles are in front and far and robot is moving forward " + "-> Critical zone result should be 1.0, returned " + << result); + if (result == 1.0) { LOG_INFO("Test2 PASSED: Angles are in front and robot is moving forward"); } @@ -167,21 +167,22 @@ BOOST_AUTO_TEST_CASE(test_critical_zone_check) { setLaserscanAtAngle(M_PI + 0.1, 0.2, scan_ranges, scan_angles); setLaserscanAtAngle(M_PI - 0.1, 0.2, scan_ranges, scan_angles); result = zoneChecker.check(scan_ranges, scan_angles, forward_motion); - BOOST_TEST(result, "Angles are in front and close and robot is moving " - "forward -> Critical zone result should be TRUE, returned " - << result); - if (result) { + BOOST_TEST(result == 0.0, + "Angles are in front and close and robot is moving " + "forward -> Critical zone result should be 0.0, returned " + << result); + if (result == 0.0) { LOG_INFO("Test3 PASSED: Angles are in front and close and robot is moving " "forward"); } forward_motion = false; result = zoneChecker.check(scan_ranges, scan_angles, forward_motion); - BOOST_TEST(!result, + BOOST_TEST(result == 1.0, "Angles are in front and close and robot is moving " - "backwards-> Critical zone result should be FALSE, returned " + "backwards-> Critical zone result should be 1.0, returned " << result); - if (!result) { + if (result == 1.0) { LOG_INFO("Test4 PASSED: Angles are in front and close and robot is " "moving backward"); } @@ -191,12 +192,58 @@ BOOST_AUTO_TEST_CASE(test_critical_zone_check) { setLaserscanAtAngle(0.1, 0.2, scan_ranges, scan_angles); setLaserscanAtAngle(-0.1, 0.2, scan_ranges, scan_angles); result = zoneChecker.check(scan_ranges, scan_angles, forward_motion); - BOOST_TEST(result, + BOOST_TEST(result == 0.0, "Angles are in back and close and robot is moving " - "backwards -> Critical zone result should be TRUE, returned " + "backwards -> Critical zone result should be 0.0, returned " << result); - if (result) { + if (result == 0.0) { LOG_INFO("Test5 PASSED: Angles are in back and close and robot is moving " "backwards"); } + + // Set slowdown ranges behind the robot + initLaserscan(360, 10.0, scan_ranges, scan_angles); + setLaserscanAtAngle(0.0, 1.0, scan_ranges, scan_angles); + result = zoneChecker.check(scan_ranges, scan_angles, forward_motion); + BOOST_TEST( + (result > 0.0 and result < 1.0), + "Angles are in back and in the slowdown zone and robot is moving " + "backwards -> Critical zone result should be between [0, 1], returned " + << result); + if (result > 0.0 and result < 1.0) { + LOG_INFO("Test6 PASSED: Angles are in back and in the slowdown zone and " + "robot is moving " + "backwards, slowdown factor = ", + result); + } + + forward_motion = true; + result = zoneChecker.check(scan_ranges, scan_angles, forward_motion); + BOOST_TEST(result == 1.0, + "Angles are in back and in the slowdown zone and robot is moving " + "forward -> Critical zone result should be between 1.0, returned " + << result); + if (result == 1.0) { + LOG_INFO("Test7 PASSED: Angles are in back and in the slowdown zone and " + "robot is moving " + "forward, slowdown factor = ", + result); + } + + // Set slowdown ranges infront of the robot + setLaserscanAtAngle(M_PI, 0.5, scan_ranges, scan_angles); + setLaserscanAtAngle(M_PI + 0.1, 0.4, scan_ranges, scan_angles); + setLaserscanAtAngle(M_PI - 0.1, 0.4, scan_ranges, scan_angles); + result = zoneChecker.check(scan_ranges, scan_angles, forward_motion); + BOOST_TEST( + (result > 0.0 and result < 1.0), + "Angles are in front and in the slowdown zone and robot is moving " + "forward -> Critical zone result should be between [0, 1], returned " + << result); + if (result > 0.0 and result < 1.0) { + LOG_INFO("Test8 PASSED: Angles are in front and in the slowdown zone and " + "robot is moving " + "forward, slowdown factor = ", + result); + } } diff --git a/src/kompass_cpp/tests/collisions_test_gpu.cpp b/src/kompass_cpp/tests/collisions_test_gpu.cpp index 507c582d..011424ff 100644 --- a/src/kompass_cpp/tests/collisions_test_gpu.cpp +++ b/src/kompass_cpp/tests/collisions_test_gpu.cpp @@ -1,4 +1,5 @@ #include "test.h" +#include #define BOOST_TEST_MODULE KOMPASS TESTS #include "collisions_test.cpp" #include "utils/collision_check.h" @@ -15,8 +16,8 @@ BOOST_AUTO_TEST_CASE(test_critical_zone_check_gpu) { auto robotShapeType = CollisionChecker::ShapeType::BOX; std::vector robotDimensions{0.51, 0.27, 0.4}; - const std::array sensor_position_body{0.22, 0.0, 0.4}; - const std::array sensor_rotation_body{0, 0, 0.99, 0.0}; + const Eigen::Vector3f sensor_position_body{0.22, 0.0, 0.4}; + const Eigen::Vector4f sensor_rotation_body{0, 0, 0.99, 0.0}; // Robot laserscan value std::vector scan_angles; @@ -24,11 +25,13 @@ BOOST_AUTO_TEST_CASE(test_critical_zone_check_gpu) { initLaserscan(360, 10.0, scan_ranges, scan_angles); bool forward_motion = true; - float critical_angle = 120.0, critical_distance = 0.2; + float critical_angle = 160.0, critical_distance = 0.3, + slowdown_distance = 0.6; - CriticalZoneCheckerGPU zoneChecker( - robotShapeType, robotDimensions, sensor_position_body, - sensor_rotation_body, critical_angle, critical_distance, scan_angles); + CriticalZoneCheckerGPU zoneChecker(robotShapeType, robotDimensions, + sensor_position_body, sensor_rotation_body, + critical_angle, critical_distance, + slowdown_distance, scan_angles); LOG_INFO("Testing Emergency Stop with GPU "); @@ -36,21 +39,22 @@ BOOST_AUTO_TEST_CASE(test_critical_zone_check_gpu) { setLaserscanAtAngle(0.0, 0.2, scan_ranges, scan_angles); setLaserscanAtAngle(0.1, 0.2, scan_ranges, scan_angles); setLaserscanAtAngle(-0.1, 0.2, scan_ranges, scan_angles); - bool result = zoneChecker.check(scan_ranges, scan_angles, forward_motion); - BOOST_TEST(!result, "Angles are behind and robot is moving forward -> " - "Critical zone result should be FALSE, returned " + float result = zoneChecker.check(scan_ranges, scan_angles, forward_motion); + BOOST_TEST(result == 1.0, "Angles are behind and robot is moving forward -> " + "Critical zone result should be 1.0, returned " << result); - if (!result) { + if (result == 1.0) { LOG_INFO("Test1 PASSED: Angles are behind and robot is moving forward"); } // Set small ranges behind the robot initLaserscan(360, 10.0, scan_ranges, scan_angles); result = zoneChecker.check(scan_ranges, scan_angles, forward_motion); - BOOST_TEST(!result, "Angles are in front and far and robot is moving forward " - "-> Critical zone result should be FALSE, returned " - << result); - if (!result) { + BOOST_TEST(result == 1.0, + "Angles are in front and far and robot is moving forward " + "-> Critical zone result should be 1.0, returned " + << result); + if (result == 1.0) { LOG_INFO("Test2 PASSED: Angles are in front and robot is moving forward"); } @@ -59,21 +63,22 @@ BOOST_AUTO_TEST_CASE(test_critical_zone_check_gpu) { setLaserscanAtAngle(M_PI + 0.1, 0.2, scan_ranges, scan_angles); setLaserscanAtAngle(M_PI - 0.1, 0.2, scan_ranges, scan_angles); result = zoneChecker.check(scan_ranges, scan_angles, forward_motion); - BOOST_TEST(result, "Angles are in front and close and robot is moving " - "forward -> Critical zone result should be TRUE, returned " - << result); - if (result) { + BOOST_TEST(result == 0.0, + "Angles are in front and close and robot is moving " + "forward -> Critical zone result should be 0.0, returned " + << result); + if (result == 0.0) { LOG_INFO("Test3 PASSED: Angles are in front and close and robot is moving " "forward"); } forward_motion = false; result = zoneChecker.check(scan_ranges, scan_angles, forward_motion); - BOOST_TEST(!result, + BOOST_TEST(result == 1.0, "Angles are in front and close and robot is moving " - "backwards-> Critical zone result should be FALSE, returned " + "backwards-> Critical zone result should be 1.0, returned " << result); - if (!result) { + if (result == 1.0) { LOG_INFO("Test4 PASSED: Angles are in front and close and robot is " "moving backward"); } @@ -83,12 +88,58 @@ BOOST_AUTO_TEST_CASE(test_critical_zone_check_gpu) { setLaserscanAtAngle(0.1, 0.2, scan_ranges, scan_angles); setLaserscanAtAngle(-0.1, 0.2, scan_ranges, scan_angles); result = zoneChecker.check(scan_ranges, scan_angles, forward_motion); - BOOST_TEST(result, + BOOST_TEST(result == 0.0, "Angles are in back and close and robot is moving " - "backwards -> Critical zone result should be TRUE, returned " + "backwards -> Critical zone result should be 0.0, returned " << result); - if (result) { + if (result == 0.0) { LOG_INFO("Test5 PASSED: Angles are in back and close and robot is moving " "backwards"); } + + // Set slowdown ranges behind the robot + forward_motion = false; + initLaserscan(360, 10.0, scan_ranges, scan_angles); + setLaserscanAtAngle(0.0, 1.0, scan_ranges, scan_angles); + result = zoneChecker.check(scan_ranges, scan_angles, forward_motion); + BOOST_TEST((result > 0.0f and result < 1.0f), + "Angles are in back and in the slowdown zone and robot is moving " + "backwards -> Critical zone result should be between [0, 1], returned " + << result); + if (result > 0.0f and result < 1.0f) { + LOG_INFO( + "Test6 PASSED: Angles are in back and in the slowdown zone and robot is moving " + "backwards, slowdown factor = ", result); + } + + forward_motion = true; + result = zoneChecker.check(scan_ranges, scan_angles, forward_motion); + BOOST_TEST( + result==1.0, + "Angles are in back and in the slowdown zone and robot is moving " + "forward -> Critical zone result should be between 1.0, returned " + << result); + if (result ==1.0) { + LOG_INFO("Test7 PASSED: Angles are in back and in the slowdown zone and " + "robot is moving " + "forward, slowdown factor = ", + result); + } + + // Set slowdown ranges infront of the robot + setLaserscanAtAngle(M_PI, 0.5, scan_ranges, scan_angles); + setLaserscanAtAngle(M_PI + 0.1, 0.4, scan_ranges, scan_angles); + setLaserscanAtAngle(M_PI - 0.1, 0.4, scan_ranges, scan_angles); + result = zoneChecker.check(scan_ranges, scan_angles, forward_motion); + BOOST_TEST( + (result > 0.0 and result < 1.0), + "Angles are in front and in the slowdown zone and robot is moving " + "forward -> Critical zone result should be between [0, 1], returned " + << result); + if (result > 0.0 and result < 1.0) { + LOG_INFO("Test8 PASSED: Angles are in front and in the slowdown zone and " + "robot is moving " + "forward, slowdown factor = ", + result); + } } diff --git a/src/kompass_cpp/tests/controller_test.cpp b/src/kompass_cpp/tests/controller_test.cpp index 43427d18..73feb953 100644 --- a/src/kompass_cpp/tests/controller_test.cpp +++ b/src/kompass_cpp/tests/controller_test.cpp @@ -34,7 +34,7 @@ BOOST_AUTO_TEST_CASE(test_DWA) { // Create a test path std::vector points{Path::Point(0.0, 0.0, 0.0), Path::Point(1.0, 0.0, 0.0), Path::Point(2.0, 0.0, 0.0)}; - Path::Path path(points); + Path::Path path(points, 500); // Sampling configuration double timeStep = 0.1; @@ -65,8 +65,8 @@ BOOST_AUTO_TEST_CASE(test_DWA) { auto robotShapeType = Kompass::CollisionChecker::ShapeType::CYLINDER; std::vector robotDimensions{0.1, 0.4}; // std::array sensorPositionWRTbody {0.0, 0.0, 1.0}; - const std::array sensor_position_body{0.0, 0.0, 0.0}; - const std::array sensor_rotation_body{0, 0, 0, 1}; + const Eigen::Vector3f sensor_position_body{0.0, 0.0, 0.0}; + const Eigen::Vector4f sensor_rotation_body{0, 0, 0, 1}; // Robot start state (pose) Path::State robotState(-0.51731912, 0.0, 0.0, 0.0); diff --git a/src/kompass_cpp/tests/cost_evaluator_test.cpp b/src/kompass_cpp/tests/cost_evaluator_test.cpp index 144f68bc..2fcd54be 100644 --- a/src/kompass_cpp/tests/cost_evaluator_test.cpp +++ b/src/kompass_cpp/tests/cost_evaluator_test.cpp @@ -128,7 +128,7 @@ Trajectory2D run_test(CostEvaluator &costEval, Path::Path &reference_path, } TrajSearchResult result = - costEval.getMinTrajectoryCost(samples, reference_path, reference_path.segments[current_segment_index]); + costEval.getMinTrajectoryCost(samples, &reference_path, reference_path.segments[current_segment_index]); BOOST_TEST(result.isTrajFound, "Minimum reference path cost trajectory is not found!"); @@ -175,15 +175,15 @@ struct TestConfig { ControlLimitsParams controlLimits; CollisionChecker::ShapeType robotShapeType; std::vector robotDimensions; - std::array sensor_position_body; - std::array sensor_rotation_body; + Eigen::Vector3f sensor_position_body; + Eigen::Quaternionf sensor_rotation_body; CostEvaluator::TrajectoryCostsWeights costWeights; CostEvaluator costEval; TestConfig() : points{Path::Point(0.0, 0.0, 0.0), Path::Point(1.0, 0.0, 0.0), Path::Point(2.0, 0.0, 0.0)}, - reference_path(points), max_path_length(10.0), + reference_path(points, 500), max_path_length(10.0), max_interpolation_point_dist(0.01), current_segment_index(0), timeStep(0.1), predictionHorizon(1.0), maxNumThreads(10), x_params(1, 3, 5), y_params(1, 3, 5), angular_params(3.14, 3, 5, 8), diff --git a/src/kompass_cpp/tests/json_export.h b/src/kompass_cpp/tests/json_export.h index 01a06f7a..c77ce115 100644 --- a/src/kompass_cpp/tests/json_export.h +++ b/src/kompass_cpp/tests/json_export.h @@ -25,7 +25,7 @@ inline void from_json(const json &j, Path::Point &p) { // Convert Path to JSON inline void to_json(json &j, const Path::Path &p) { j["points"] = json::array(); // Initialize as a JSON array - for (const auto &point : p.points) { + for (const auto &point : p) { j["points"].push_back( json{{"x", point.x()}, {"y", point.y()}}); // Serialize each Point } @@ -42,13 +42,14 @@ inline void to_json(json &j, const Control::TrajectoryPath &p) { // Convert JSON to Path inline void from_json(const json &j, Path::Path &p) { - p.points.clear(); // Clear existing points + std::vector points; for (const auto &item : j.at("points")) { Path::Point point; point.x() = item.at("x"); point.y() = item.at("y"); - p.points.push_back(point); // Deserialize each Point + points.push_back(point); // Deserialize each Point } + p = Path::Path(points, points.size()); } // Convert Velocity to JSON @@ -92,9 +93,8 @@ inline void to_json(json &j, const Control::TrajectorySamples2D &samples, } // Save trajectories to a JSON file -inline void saveTrajectoriesToJson( - const Control::TrajectorySamples2D &trajectories, - const std::string &filename) { +void saveTrajectoriesToJson(const Control::TrajectorySamples2D &trajectories, + const std::string &filename) { json j; to_json(j, trajectories); std::ofstream file(filename); @@ -106,6 +106,22 @@ inline void saveTrajectoriesToJson( } } +void saveTrajectoryToJson(const Control::Trajectory2D &trajectory, + const std::string &filename) { + json j; + j["paths"] = json::array(); // Initialize as a JSON array + json j_p; + to_json(j_p, trajectory.path); + j["paths"].push_back(j_p); // Serialize each Point + std::ofstream file(filename); + if (file.is_open()) { + file << j.dump(4); // Pretty print with 4 spaces indentation + file.close(); + } else { + std::cerr << "Unable to open file: " << filename << std::endl; + } +} + // Save one path to a JSON file inline void savePathToJson(const Path::Path &path, const std::string &filename) { json j; diff --git a/src/kompass_cpp/tests/trajectory_sampler_plt.py b/src/kompass_cpp/tests/trajectory_sampler_plt.py index a80027a6..06815fc2 100755 --- a/src/kompass_cpp/tests/trajectory_sampler_plt.py +++ b/src/kompass_cpp/tests/trajectory_sampler_plt.py @@ -1,5 +1,4 @@ import json -import matplotlib.pyplot as plt import argparse @@ -29,12 +28,21 @@ def read_path_from_json(filename: str): print(f"Read file error: {e}") -def plot_samples(trajectories, reference, figure_name): - # Plot reference - ref_path = reference["points"] - x_coords = [point["x"] for point in ref_path] - y_coords = [point["y"] for point in ref_path] - plt.plot(x_coords, y_coords, "--b") +def plot_samples(figure_name, trajectories, reference=None): + try: + import matplotlib.pyplot as plt + except ImportError: + print( + "Matplotlib is not installed. Test figures will not be generated. To generate figures run 'pip install matplotlib'" + ) + return + + if reference: + # Plot reference + ref_path = reference["points"] + x_coords = [point["x"] for point in ref_path] + y_coords = [point["y"] for point in ref_path] + plt.plot(x_coords, y_coords, "--b", linewidth=7.0, label="reference") for traj in trajectories: path = traj["points"] @@ -63,7 +71,7 @@ def main(): parser.add_argument( "--reference", type=str, - required=True, + required=False, help="Reference path JSON file name in the current directory", ) @@ -75,9 +83,13 @@ def main(): reference_file = args.reference trajectories = read_trajectories_from_json(f"{samples_file}.json") - reference = read_path_from_json(f"{reference_file}.json") + + reference = ( + read_path_from_json(f"{reference_file}.json") if reference_file else None + ) + # print(trajectories) - plot_samples(trajectories, reference, samples_file) + plot_samples(samples_file, trajectories, reference) if __name__ == "__main__": diff --git a/src/kompass_cpp/tests/trajectory_sampler_test.cpp b/src/kompass_cpp/tests/trajectory_sampler_test.cpp index 70cc2a5d..62ff806f 100644 --- a/src/kompass_cpp/tests/trajectory_sampler_test.cpp +++ b/src/kompass_cpp/tests/trajectory_sampler_test.cpp @@ -6,10 +6,9 @@ #include "test.h" #include "utils/logger.h" #include "utils/trajectory_sampler.h" +#include #include // for program_location #include -#include -#include #include #include @@ -24,7 +23,7 @@ void testTrajSampler() { std::vector points{Path::Point(0.0, 0.0, 0.0), Path::Point(1.0, 0.0, 0.0), Path::Point(2.0, 0.0, 0.0)}; - Path::Path raw_path(points); + Path::Path raw_path(points, 500); // Generic follower to use for raw path interpolation and segmentation Control::Follower *follower = new Control::Follower(); @@ -61,8 +60,8 @@ void testTrajSampler() { auto robotShapeType = Kompass::CollisionChecker::ShapeType::BOX; std::vector robotDimensions{0.3, 0.3, 1.0}; // std::array sensorPositionWRTbody {0.0, 0.0, 1.0}; - const std::array sensor_position_body{0.0, 0.0, 0.5}; - const std::array sensor_rotation_body{0, 0, 0, 1}; + const Eigen::Vector3f sensor_position_body{0.0, 0.0, 0.5}; + const Eigen::Quaternionf sensor_rotation_body{0, 0, 0, 1}; // Robot start state (pose) Path::State robotState(0.0, 0.0, 0.0, 0.0); diff --git a/src/kompass_cpp/tests/vision_detector_test.cpp b/src/kompass_cpp/tests/vision_detector_test.cpp new file mode 100644 index 00000000..fbc356ba --- /dev/null +++ b/src/kompass_cpp/tests/vision_detector_test.cpp @@ -0,0 +1,167 @@ +#include "datatypes/tracking.h" +#include "test.h" +#include "utils/logger.h" +#include "utils/transformation.h" +#include "vision/depth_detector.h" +#include +#include +#include +#include +#define BOOST_TEST_MODULE KOMPASS TESTS +#include // for program_location +#include +#include +#include + +using namespace Kompass; + +struct DepthDetectorTestConfig { + std::unique_ptr detector; + Path::State current_state = {1.0, 1.0, 0.0}; + std::vector detected_boxes; + std::string pltFileName = "DepthDetectorTest"; + Eigen::Vector2f focal_length = {911.71, 910.288}; + Eigen::Vector2f principal_point = {643.06, 366.72}; + Eigen::Vector2f depth_range = {0.001, 5.0}; // 5cm to 5 meters + float depth_conv_factor = 1e-3; // convert from mm to m + Eigen::Isometry3f camera_body_tf; + Eigen::MatrixX depth_image; + cv::Mat cv_img; + std::vector detections; + + DepthDetectorTestConfig(const std::string image_filename, const Bbox2D &box) { + // Body to camera tf from robot of test pictures + auto link_in_body = + getTransformation(Eigen::Quaternionf{0.0f, 0.1987f, 0.0f, 0.98f}, + Eigen::Vector3f{0.32f, 0.0209f, 0.3f}); + + auto cam_in_link = + getTransformation(Eigen::Quaternionf{0.01f, -0.00131f, 0.002f, 0.9999f}, + Eigen::Vector3f{0.0f, 0.0105f, 0.0f}); + + auto cam_opt_in_cam = + getTransformation(Eigen::Quaternionf{-0.5f, 0.5f, -0.5f, 0.5f}, + Eigen::Vector3f{0.0f, 0.0105f, 0.0f}); + + Eigen::Isometry3f cam_in_body = link_in_body * cam_in_link * cam_opt_in_cam; + + detector = + std::make_unique(depth_range, cam_in_body, focal_length, + principal_point, depth_conv_factor); + + cv_img = cv::imread(image_filename, cv::IMREAD_GRAYSCALE); + + if (cv_img.empty()) { + LOG_ERROR("Could not open or find the image"); + } + + // Create an Eigen matrix of type int from the OpenCV Mat + depth_image = Eigen::MatrixX(cv_img.rows, cv_img.cols); + for (int i = 0; i < cv_img.rows; ++i) { + for (int j = 0; j < cv_img.cols; ++j) { + depth_image(i, j) = cv_img.at(i, j); + } + } + detections.push_back(box); + }; + + std::vector run(const std::string outputFilename, + const bool local_frame = true) { + if(local_frame){ + detector->updateBoxes(depth_image, detections); + } + else{ + detector->updateBoxes(depth_image, detections, current_state); + } + + auto boxes3D = detector->get3dDetections(); + if (boxes3D) { + auto res = boxes3D.value(); + + // Draw the bounding boxes on the image + cv::Scalar color(0, 0, 0); // Green color for the rectangles + int thickness = 2; // Thickness of the rectangle lines + + for (const auto &box : res) { + LOG_INFO("Got detected box in 3D world coordinates at :", + box.center.x(), ", ", box.center.y(), ", ", box.center.z()); + LOG_INFO("Box size :", box.size.x(), ", ", box.size.y(), ", ", + box.size.z()); + cv::Point topLeft(box.center_img_frame.x() - box.size_img_frame.x() / 2, + box.center_img_frame.y() - + box.size_img_frame.y() / 2); + cv::Point bottomRight( + box.center_img_frame.x() + box.size_img_frame.x() / 2, + box.center_img_frame.y() + box.size_img_frame.y() / 2); + cv::rectangle(cv_img, topLeft, bottomRight, color, thickness); + } + + // Save the modified image + cv::imwrite(outputFilename, cv_img); + + if (!cv::imwrite(outputFilename, cv_img)) { + LOG_ERROR("Could not save the image"); + } else { + LOG_INFO("Image saved to ", outputFilename); + } + BOOST_TEST(res.size() == 1, "Got different size for 3D and 2D boxes"); + BOOST_TEST(res[0].size_img_frame.x() == detections[0].size.x(), + "Error parsing box, size x in pixel frame is not conserved"); + BOOST_TEST(res[0].size_img_frame.y() == detections[0].size.y(), + "Error parsing box, size y in pixel frame is not conserved"); + return res; + } + throw std::runtime_error("Could not find 3D boxes"); + } +}; + +BOOST_AUTO_TEST_CASE(test_Depth_Detector_person_image) { + // Create timer + Timer time; + std::string filename = + "/home/ahr/kompass/uvmap_code/resources/depth_image.tif"; + Bbox2D box({535, 0}, {520, 420}); + auto config = DepthDetectorTestConfig(filename, box); + std::string outputFilename = + "/home/ahr/kompass/uvmap_code/resources/image_output.jpg"; + auto boxes = config.run(outputFilename); +} + + +BOOST_AUTO_TEST_CASE(test_Depth_Detector_bag_image_local_frame) { + LOG_INFO("Testing and generating 3D boxes in local robot frame"); + // Create timer + Timer time; + std::string filename = + "/home/ahr/kompass/uvmap_code/resources/bag_image_depth.tif"; + Bbox2D box({410, 0}, {410, 390}); + auto config = DepthDetectorTestConfig(filename, box); + std::string outputFilename = + "/home/ahr/kompass/uvmap_code/resources/bag_depth_output.jpg"; + auto boxes = config.run(outputFilename, true); + float dist = + std::sqrt(std::pow((boxes[0].center.x()), 2) + + std::pow((boxes[0].center.y()), 2)); + const float approx_actual_dist = 1.8; + BOOST_TEST(std::abs(dist - approx_actual_dist) <= 0.1, + "3D box distance is not equal to approximate measured distance"); +} + +BOOST_AUTO_TEST_CASE(test_Depth_Detector_bag_image_global_frame) { + LOG_INFO("Testing and generating 3D boxes in global world frame"); + // Create timer + Timer time; + std::string filename = + "/home/ahr/kompass/uvmap_code/resources/bag_image_depth.tif"; + Bbox2D box({410, 0}, {410, 390}); + auto config = DepthDetectorTestConfig(filename, box); + std::string outputFilename = + "/home/ahr/kompass/uvmap_code/resources/bag_depth_output.jpg"; + auto boxes = config.run(outputFilename, false); + float dist = + std::sqrt(std::pow((boxes[0].center.x() - config.current_state.x), 2) + + std::pow((boxes[0].center.y() - config.current_state.y), 2)); + const float approx_actual_dist = 1.8; + BOOST_TEST(std::abs(dist - approx_actual_dist) <= 0.1, + "3D box distance is not equal to approximate measured distance"); +} diff --git a/src/kompass_cpp/tests/vision_dwa_test.cpp b/src/kompass_cpp/tests/vision_dwa_test.cpp new file mode 100644 index 00000000..30996b5e --- /dev/null +++ b/src/kompass_cpp/tests/vision_dwa_test.cpp @@ -0,0 +1,414 @@ +#include "controllers/vision_dwa.h" +#include "datatypes/control.h" +#include "datatypes/path.h" +#include "datatypes/tracking.h" +#include "datatypes/trajectory.h" +#include "test.h" +#include "utils/cost_evaluator.h" +#include "utils/logger.h" +#include "utils/transformation.h" +#include +#include +#define BOOST_TEST_MODULE KOMPASS TESTS +#include "json_export.h" +#include // for program_location +#include +#include +#include +#include + +using namespace Kompass; + +struct VisionDWATestConfig { + bool use_local_frame; + + // Sampling configuration + double timeStep; + int predictionHorizon; + int controlHorizon; + int maxLinearSamples; + int maxAngularSamples; + int maxNumThreads; + + // Detected Boxes + std::vector detected_boxes; + int num_test_boxes = 3; + Eigen::Vector2i ref_point_img{150, 150}; + float boxes_ori = 0.0f; + + // Octomap resolution + double octreeRes; + + // Cost weights + Control::CostEvaluator::TrajectoryCostsWeights costWeights; + + // Robot configuration + Control::LinearVelocityControlParams x_params; + Control::LinearVelocityControlParams y_params; + Control::AngularVelocityControlParams angular_params; + Control::ControlLimitsParams controlLimits; + Control::ControlType controlType; + Kompass::CollisionChecker::ShapeType robotShapeType; + std::vector robotDimensions; + Eigen::Vector3f prox_sensor_position_body; + Eigen::Vector4f prox_sensor_rotation_body; + + // Depth detector + Eigen::Vector2f focal_length = {911.71, 910.288}; + Eigen::Vector2f principal_point = {643.06, 366.72}; + Eigen::Vector2f depth_range = {0.001, 5.0}; // 5cm to 5 meters + float depth_conv_factor = 1e-3; // convert from mm to m + Eigen::Isometry3f camera_body_tf; + + // Robot pointcloud values (global frame) + std::vector cloud; + + // Robot start state (pose) + Path::State robotState; + + // Tracked target with respect to the robot + Control::Velocity2D tracked_vel; + Control::TrackedPose2D tracked_pose; + // VisionDWA configuration object + std::unique_ptr controller; + + // Constructor to initialize the struct + VisionDWATestConfig(const std::vector sensor_points, + const bool use_local_frame = true, + const double timeStep = 0.1, + const int predictionHorizon = 6, + const int controlHorizon = 2, + const int maxLinearSamples = 20, + const int maxAngularSamples = 20, + const float maxVel = 2.0, const float maxOmega = 4.0, + const int maxNumThreads = 1, + const double reference_path_distance_weight = 1.0, + const double goal_distance_weight = 1.0, + const double obstacles_distance_weight = 0.0) + : use_local_frame(use_local_frame), timeStep(timeStep), predictionHorizon(predictionHorizon), + controlHorizon(controlHorizon), maxLinearSamples(maxLinearSamples), + maxAngularSamples(maxAngularSamples), maxNumThreads(maxNumThreads), + octreeRes(0.1), x_params(maxVel, 5.0, 10.0), y_params(1, 3, 5), + angular_params(3.14, maxOmega, 3.0, 3.0), + controlLimits(x_params, y_params, angular_params), + controlType(Control::ControlType::ACKERMANN), + robotShapeType(Kompass::CollisionChecker::ShapeType::CYLINDER), + robotDimensions{0.1, 0.4}, prox_sensor_position_body{0.0, 0.0, 0.0}, + prox_sensor_rotation_body{0, 0, 0, 1}, cloud(sensor_points), + robotState(-0.5, 0.0, 0.0, 0.0), tracked_vel(0.1, 0.0, 0.3), + tracked_pose(0.0, 0.0, 0.0, tracked_vel) { + // Initialize cost weights + costWeights.setParameter("reference_path_distance_weight", + reference_path_distance_weight); + costWeights.setParameter("goal_distance_weight", goal_distance_weight); + costWeights.setParameter("obstacles_distance_weight", + obstacles_distance_weight); + costWeights.setParameter("smoothness_weight", 0.0); + costWeights.setParameter("jerk_weight", 0.0); + auto config = Control::VisionDWA::VisionDWAConfig(); + config.setParameter("control_time_step", timeStep); + config.setParameter("control_horizon", controlHorizon); + config.setParameter("prediction_horizon", predictionHorizon); + config.setParameter("use_local_coordinates", use_local_frame); + + // For depth config + // Body to camera tf from robot of test pictures + auto body_to_link_tf = + getTransformation(Eigen::Quaternionf{0.0f, 0.1987f, 0.0f, 0.98f}, + Eigen::Vector3f{0.32f, 0.0209f, 0.3f}); + + auto link_to_cam_tf = + getTransformation(Eigen::Quaternionf{0.01f, -0.00131f, 0.002f, 0.9999f}, + Eigen::Vector3f{0.0f, 0.0105f, 0.0f}); + + auto cam_to_cam_opt_tf = + getTransformation(Eigen::Quaternionf{-0.5f, 0.5f, -0.5f, 0.5f}, + Eigen::Vector3f{0.0f, 0.0105f, 0.0f}); + + Eigen::Isometry3f body_to_cam_tf = + body_to_link_tf * link_to_cam_tf * cam_to_cam_opt_tf; + + Eigen::Vector3f translation = body_to_cam_tf.translation(); + Eigen::Quaternionf rotation_quat = + Eigen::Quaternionf(body_to_cam_tf.rotation()); + Eigen::Vector4f rotation = {rotation_quat.w(), rotation_quat.x(), + rotation_quat.y(), rotation_quat.z()}; + + controller = std::make_unique( + controlType, controlLimits, maxLinearSamples, maxAngularSamples, + robotShapeType, robotDimensions, prox_sensor_position_body, + prox_sensor_rotation_body, translation, rotation, octreeRes, + costWeights, maxNumThreads, config); + + // Initialize the detected boxes + Bbox3D new_box; + new_box.size = {0.5f, 0.5f, 1.0f}; + detected_boxes.resize(num_test_boxes); + for (int i = 0; i < num_test_boxes; ++i) { + auto new_box_shift = + Eigen::Vector3f({float(0.7 * i), float(0.7 * i), 0.0f}); + auto img_frame_shift = Eigen::Vector2i({float(50 * i), float(50 * i)}); + new_box.center = new_box_shift; + new_box.center_img_frame = img_frame_shift + ref_point_img; + new_box.size_img_frame = {25, 25}; + new_box.timestamp = 0.0f; + detected_boxes[i] = new_box; + } + } + + void moveDetectedBoxes() { + // Update the detected boxes using the velocity command + Eigen::Vector3f target_ref_vel = {float(tracked_vel.vx() * cos(boxes_ori)), + float(tracked_vel.vx() * sin(boxes_ori)), + 0.0f}; + boxes_ori += tracked_vel.omega() * timeStep; + for (auto &box : detected_boxes) { + box.center += target_ref_vel * timeStep; + box.timestamp += timeStep; + } + }; + + bool test_one_cmd_depth(const std::string image_file_path, + const std::vector &detections, + const Eigen::Vector2i &clicked_point, + std::vector cloud) { + // robot velocity + Control::Velocity2D cmd; + // Get image + cv::Mat cv_img = cv::imread(image_file_path, cv::IMREAD_UNCHANGED); + + if (cv_img.empty()) { + LOG_ERROR("Could not open or find the image"); + } + + // Create an Eigen matrix of type int from the OpenCV Mat + auto depth_image = Eigen::MatrixX(cv_img.rows, cv_img.cols); + for (int i = 0; i < cv_img.rows; ++i) { + for (int j = 0; j < cv_img.cols; ++j) { + depth_image(i, j) = cv_img.at(i, j); + } + } + + controller->setCameraIntrinsics(focal_length.x(), focal_length.y(), + principal_point.x(), principal_point.y()); + + controller->setCurrentState(robotState); + auto found_target = controller->setInitialTracking( + clicked_point(0), clicked_point(1), depth_image, detections); + if (!found_target) { + LOG_WARNING("Point not found on image"); + return false; + } else { + LOG_INFO("Point found on image ..."); + } + + auto res = controller->getTrackingCtrl(depth_image, detections, cmd, cloud); + if (res.isTrajFound) { + LOG_INFO("Got control (vx, vy, omega) = (", + res.trajectory.velocities.vx[0], ", ", + res.trajectory.velocities.vy[0], ", ", + res.trajectory.velocities.omega[0], ")"); + } + return res.isTrajFound; + } + + bool run_test(const int numPointsPerTrajectory, std::string pltFileName, + const bool simulate_obstacle = false) { + Control::TrajectorySamples2D samples(2, numPointsPerTrajectory); + Control::TrajectoryVelocities2D simulated_velocities( + numPointsPerTrajectory); + Control::TrajectoryPath robot_path(numPointsPerTrajectory), + tracked_path(numPointsPerTrajectory); + Control::Velocity2D cmd; + Control::TrajSearchResult result; + + controller->setCurrentState(robotState); + controller->setInitialTracking(ref_point_img(0), ref_point_img(1), + detected_boxes); + + int step = 0; + while (step < numPointsPerTrajectory) { + Path::Point point(robotState.x, robotState.y, 0.0); + robot_path.add(step, point); + tracked_path.add(step, {tracked_pose.x(), tracked_pose.y(), 0.0}); + controller->setCurrentState(robotState); + LOG_INFO("Target center at: ", tracked_pose.x(), ", ", tracked_pose.y(), + ", ", tracked_pose.yaw()); + LOG_INFO("Robot at: ", point.x(), ", ", point.y()); + + std::vector seen_boxes; + if (use_local_frame) { + // Transform boxes to local coordinates + std::vector boxes_local_coordinates; + Eigen::Isometry3f world_in_robot_tf = + getTransformation(robotState).inverse(); + Eigen::Matrix3f abs_rotation = world_in_robot_tf.linear().cwiseAbs(); + for (auto &box : detected_boxes) { + // Transform the box to the robot frame + Bbox3D box_local(box); + box_local.center = world_in_robot_tf * box.center; + box_local.size = abs_rotation * box.size; + boxes_local_coordinates.push_back(box_local); + } + seen_boxes = boxes_local_coordinates; + } else { + seen_boxes = detected_boxes; + } + + // Simulate lost of target around obstacle + if (simulate_obstacle and robotState.y > 0.2 and robotState.y < 0.4) { + LOG_INFO("Sending EMPTY BOXES NOW!!!!!"); + seen_boxes = {}; + } + controller->setCurrentState(robotState); + result = controller->getTrackingCtrl(seen_boxes, cmd, cloud); + + if (result.isTrajFound) { + LOG_INFO(FMAG("STEP: "), step, + FMAG(", Found best trajectory with cost: "), result.trajCost); + if (controller->getLinearVelocityCmdX() > + controlLimits.velXParams.maxVel) { + LOG_ERROR(BOLD(FRED("Vx is larger than max vel: ")), KRED, + controller->getLinearVelocityCmdX(), RST, + BOLD(FRED(", Vx: ")), KRED, controlLimits.velXParams.maxVel, + RST); + return false; + } + auto velocities = result.trajectory.velocities; + int numSteps = 0; + for (auto vel : velocities) { + if (numSteps >= controlHorizon) { + break; + } + numSteps++; + cmd = vel; + LOG_DEBUG(BOLD(FBLU("Robot at: {x: ")), KBLU, robotState.x, RST, + BOLD(FBLU(", y: ")), KBLU, robotState.y, RST, + BOLD(FBLU(", yaw: ")), KBLU, robotState.yaw, RST, + BOLD(FBLU("}"))); + + LOG_DEBUG(BOLD(FGRN("Found Control: {Vx: ")), KGRN, vel.vx(), RST, + BOLD(FGRN(", Vy: ")), KGRN, vel.vy(), RST, + BOLD(FGRN(", Omega: ")), KGRN, vel.omega(), RST, + BOLD(FGRN("}"))); + + robotState.update(vel, timeStep); + } + + } else { + LOG_ERROR(BOLD(FRED("DWA Planner failed with robot at: {x: ")), KRED, + robotState.x, RST, BOLD(FRED(", y: ")), KRED, robotState.y, + RST, BOLD(FRED(", yaw: ")), KRED, robotState.yaw, RST, + BOLD(FRED("}"))); + break; + } + + tracked_pose.update(timeStep); + + moveDetectedBoxes(); + + step++; + } + samples.push_back(simulated_velocities, robot_path); + samples.push_back(simulated_velocities, tracked_path); + + // Plot the trajectories (Save to json then run python script for plotting) + boost::filesystem::path executablePath = boost::dll::program_location(); + std::string file_location = executablePath.parent_path().string(); + + std::string trajectories_filename = file_location + "/" + pltFileName; + + saveTrajectoriesToJson(samples, trajectories_filename + ".json"); + // savePathToJson(reference_segment, ref_path_filename + ".json"); + + std::string command = "python3 " + file_location + + "/trajectory_sampler_plt.py --samples \"" + + trajectories_filename + "\""; + + // Execute the Python script + int res = system(command.c_str()); + if (res != 0) + throw std::system_error(res, std::generic_category(), + "Python script failed with error code"); + return true; + } +}; + +BOOST_AUTO_TEST_CASE(Test_VisionDWA_Obstacle_Free_local) { + // Create timer + Timer time; + + // Robot pointcloud values (global frame) + std::vector cloud = {{10.0, 10.0, 0.1}}; + bool use_local_frame = true; + + VisionDWATestConfig testConfig(cloud, use_local_frame); + + int numPointsPerTrajectory = 100; + + bool test_passed = testConfig.run_test( + numPointsPerTrajectory, + std::string("vision_follower_with_tracker_local_frame")); + BOOST_TEST(test_passed, + "VisionDWA Failed To Find Control in local frame mode"); +} + +BOOST_AUTO_TEST_CASE(Test_VisionDWA_Obstacle_Free_global_frame) { + // Create timer + Timer time; + + // Robot pointcloud values (global frame) + std::vector cloud = {{10.0, 10.0, 0.1}}; + bool use_local_frame = false; + + VisionDWATestConfig testConfig(cloud, use_local_frame); + + int numPointsPerTrajectory = 100; + + bool test_passed = testConfig.run_test( + numPointsPerTrajectory, + std::string("vision_follower_with_tracker_global_frame"), + use_local_frame); + BOOST_TEST(test_passed, + "VisionDWA Failed To Find Control in global frame mode"); +} + +// BOOST_AUTO_TEST_CASE(test_VisionDWA_with_tracker_and_obstacle) { +// // Create timer +// Timer time; + +// // Robot pointcloud values (global frame) +// std::vector cloud = {{0.3, 0.27, 0.1}}; + +// VisionDWATestConfig testConfig(cloud); + +// int numPointsPerTrajectory = 100; + +// bool test_passed = testConfig.run_test( +// numPointsPerTrajectory, +// std::string("vision_follower_with_tracker_and_obstacle"), true); +// BOOST_TEST(test_passed, "VisionDWA Failed To Find Control"); +// } + +BOOST_AUTO_TEST_CASE(test_VisionDWA_with_depth_image) { + // Create timer + Timer time; + + std::string filename = + "/home/ahr/kompass/kompass-navigation/tests/resources/control/" + "bag_image_depth.tif"; + Bbox2D box({410, 0}, {410, 390}); + std::vector detections_2d{box}; + + auto initial_point = Eigen::Vector2i{610, 200}; + + // Robot pointcloud values (global frame) + std::vector cloud = {{10.3, 10.5, 0.2}}; + + VisionDWATestConfig testConfig(cloud); + + auto test_passed = testConfig.test_one_cmd_depth(filename, detections_2d, + initial_point, cloud); + + BOOST_TEST(test_passed, "VisionDWA Failed To Find Control Using Depth Image"); +} diff --git a/src/kompass_cpp/tests/vision_tracking_test.cpp b/src/kompass_cpp/tests/vision_tracking_test.cpp new file mode 100644 index 00000000..615ae0d5 --- /dev/null +++ b/src/kompass_cpp/tests/vision_tracking_test.cpp @@ -0,0 +1,151 @@ +#include "datatypes/control.h" +#include "datatypes/path.h" +#include "datatypes/tracking.h" +#include "datatypes/trajectory.h" +#include "json_export.h" +#include "test.h" +#include "utils/logger.h" +#include "vision/tracker.h" +#define BOOST_TEST_MODULE KOMPASS TESTS +#include // for program_location +#include +#include +#include +#include + +using namespace Kompass; + +struct VisionTrackingTestConfig { + float time_step; + int maxSteps; + Eigen::Vector3f target_ref_vel; + std::unique_ptr tracker; + Control::TrajectoryPath reference_path, measured_path, tracked_path; + Path::State reference_state; + std::vector detected_boxes; + std::string pltFileName = "vision_tracker_test"; + std::mt19937 gen; + std::normal_distribution noise; + + VisionTrackingTestConfig(const int &maxSteps = 50, + const float &time_step = 0.1, + const float &e_pos = 0.1, const float &e_vel = 0.1, + const float &e_acc = 0.1, + const Eigen::Vector3f target_pose = {0.0f, 0.0f, 0.0f}, + const Eigen::Vector3f target_box_size = {0.5f, 0.5f, + 1.0f}, + const int num_test_boxes = 3, const float target_v = 0.2, const float target_omega = 0.3) + : time_step(time_step), maxSteps(maxSteps) { + tracker = std::make_unique(time_step, e_pos, e_vel, + e_acc); + Bbox3D tracked_box; + tracked_box.center = target_pose; + tracked_box.size = target_box_size; + tracked_box.timestamp = 0.0f; + tracker->setInitialTracking(tracked_box); + reference_path = Control::TrajectoryPath(maxSteps); + tracked_path = Control::TrajectoryPath(maxSteps); + measured_path = Control::TrajectoryPath(maxSteps); + reference_state = Path::State(target_pose.x(), target_pose.y()); + detected_boxes.resize(num_test_boxes); + detected_boxes[0] = tracked_box; + Bbox3D new_box; + new_box.size = target_box_size; + for(int i=1; i < num_test_boxes; ++i){ + auto new_box_shift = Eigen::Vector3f({float(0.7 * i), float(0.7 * i), 0.0f}); + new_box.center = new_box_shift + target_pose; + new_box.timestamp = 0.0f; + detected_boxes[i] = new_box; + } + + target_ref_vel = {target_v * cos(target_omega), target_v * sin(target_omega), 0.0}; + + // Random noise generator + std::random_device rd; // Obtain a random number from hardware + gen = std::mt19937(rd()); + // Define the normal distribution with mean and standard deviation + noise = std::normal_distribution(0.0f, 0.01f); + }; + + void moveDetectedBoxes(const int step){ + // Update the detected boxes using the velocity command with additional measurement noise + for(auto &box: detected_boxes){ + Eigen::Vector3f noise_vector(noise(gen), noise(gen), noise(gen)); + box.center += target_ref_vel * time_step + noise_vector; + box.timestamp += time_step; + } + // Update the reference state using same velocity command (real) + reference_state.x += target_ref_vel.x() * time_step; + reference_state.y += target_ref_vel.y() * time_step; + Path::Point point(reference_state.x, reference_state.y, 0.0); + reference_path.add( + step, Path::Point(reference_state.x, reference_state.y, 0.0)); + }; +}; + + +BOOST_AUTO_TEST_CASE(test_Vision_Tracker) { + // Create timer + Timer time; + VisionTrackingTestConfig config = VisionTrackingTestConfig(); + Control::TrajectorySamples2D samples(3, config.maxSteps); + Control::TrajectoryVelocities2D simulated_velocities(config.maxSteps); + + int step = 0; + float tracking_error = 0.0; + while (step < config.maxSteps) { + // Sed detected boxes and ask tracker to update + config.tracker->updateTracking(config.detected_boxes); + auto measured_track = config.tracker->getRawTracking(); + auto tracked_state = config.tracker->getTrackedState(); + if(tracked_state){ + auto mat = tracked_state->col(0); + LOG_INFO("Real target location at", config.reference_state.x, ", ", config.reference_state.y); + LOG_INFO("Found tracked target at", mat(0), ", ", mat(1)); + tracking_error += + Path::Path::distance(mat.segment(0, 3), Path::Point(config.reference_state.x, + config.reference_state.y, 0.0)); + config.tracked_path.add(step, mat.segment(0, 3)); + config.measured_path.add(step, Path::Point(measured_track->box.center.x(), + measured_track->box.center.y(), + 0.0)); + if (step < config.maxSteps - 1) + simulated_velocities.add(step, Control::Velocity2D()); + } + else{ + LOG_ERROR("Lost tracked target at step: ", step); + break; + } + // Move the detected boxes one step using the reference velocity + config.moveDetectedBoxes(step); + step++; + } + + // End Tracking error + tracking_error /= config.maxSteps; + LOG_INFO("Average Error Used Noisy Measurements = ", tracking_error); + BOOST_TEST(tracking_error <= 0.1 , "Tracking error is larger than 10%"); + + samples.push_back(simulated_velocities, config.reference_path); + samples.push_back(simulated_velocities, config.measured_path); + samples.push_back(simulated_velocities, config.tracked_path); + + // Plot the trajectories (Save to json then run python script for plotting) + boost::filesystem::path executablePath = boost::dll::program_location(); + std::string file_location = executablePath.parent_path().string(); + + std::string trajectories_filename = file_location + "/" + config.pltFileName; + + saveTrajectoriesToJson(samples, trajectories_filename + ".json"); + // savePathToJson(reference_segment, ref_path_filename + ".json"); + + std::string command = "python3 " + file_location + + "/trajectory_sampler_plt.py --samples \"" + + trajectories_filename + "\""; + + // Execute the Python script + int res = system(command.c_str()); + if (res != 0) + throw std::system_error(res, std::generic_category(), + "Python script failed with error code"); +} diff --git a/tests/resources/control/bag_image_depth.tif b/tests/resources/control/bag_image_depth.tif new file mode 100644 index 00000000..d48f681a Binary files /dev/null and b/tests/resources/control/bag_image_depth.tif differ diff --git a/tests/test_controllers.py b/tests/test_controllers.py index 1b2b507e..5b4e3262 100644 --- a/tests/test_controllers.py +++ b/tests/test_controllers.py @@ -1,9 +1,9 @@ import json import logging import os +import cv2 from typing import Union, List -import matplotlib.pyplot as plt import numpy as np import pytest from attrs import define, field, Factory @@ -12,6 +12,7 @@ from kompass_cpp.types import PathInterpolationType, Path as PathCpp from kompass_core.datatypes.laserscan import LaserScanData +from kompass_core.datatypes import Bbox2D from kompass_core.control import ( DVZ, DWAConfig, @@ -19,6 +20,10 @@ DWA, StanleyConfig, Stanley, + VisionRGBDFollower, + VisionRGBDFollowerConfig, + VisionRGBFollower, + VisionRGBFollowerConfig, ) from kompass_core.models import ( AngularCtrlLimits, @@ -86,6 +91,13 @@ def plot_path( figure_tag: str, ): """Plot Test Results""" + try: + import matplotlib.pyplot as plt + except ImportError: + logger.warning( + "Matplotlib is required for visualization. Figures will not be generated. To generate test figures, install it using 'pip install matplotlib'." + ) + return # Extract x and y coordinates from the Path message x_coords = [pose.pose.position.x for pose in path.poses] y_coords = [pose.pose.position.y for pose in path.poses] @@ -177,11 +189,8 @@ def run_control( # Interpolated path for visualization interpolated_path = controller.interpolated_path() - interpolation_x = [] - interpolation_y = [] - for point in interpolated_path.points: - interpolation_x.append(point[0]) - interpolation_y.append(point[1]) + interpolation_x = interpolated_path.x() + interpolation_y = interpolated_path.y() i = 0 x_robot = [] @@ -275,15 +284,22 @@ def test_path_interpolation(plot: bool = False): x_ref = [pose.pose.position.x for pose in ref_path.poses] y_ref = [pose.pose.position.y for pose in ref_path.poses] - x_inter_lin = [point[0] for point in linear_interpolation.points] - y_inter_lin = [point[1] for point in linear_interpolation.points] + x_inter_lin = linear_interpolation.x() + y_inter_lin = linear_interpolation.y() - x_inter_her = [point[0] for point in hermite_spline_interpolation.points] - y_inter_her = [point[1] for point in hermite_spline_interpolation.points] + x_inter_her = hermite_spline_interpolation.x() + y_inter_her = hermite_spline_interpolation.y() - x_inter_cub = [point[0] for point in cubic_spline_interpolation.points] - y_inter_cub = [point[1] for point in cubic_spline_interpolation.points] + x_inter_cub = cubic_spline_interpolation.x() + y_inter_cub = cubic_spline_interpolation.y() + try: + import matplotlib.pyplot as plt + except ImportError: + logger.warning( + "Matplotlib is required for visualization. Figures will not be generated. To generate test figures, install it using 'pip install matplotlib'." + ) + return # Plot the path plt.figure() plt.plot( @@ -330,9 +346,9 @@ def path_length(path: Union[Path, PathCpp]) -> float: ) length += np.sqrt(d_x**2 + d_y**2) elif isinstance(path, PathCpp): - for idx in range(len(path.points) - 1): - d_x = path.points[idx + 1][0] - path.points[idx][0] - d_y = path.points[idx + 1][1] - path.points[idx][1] + for idx in range(path.size() - 1): + d_x = path.getIndex(idx + 1)[0] - path.getIndex(idx)[0] + d_y = path.getIndex(idx + 1)[1] - path.getIndex(idx)[1] length += np.sqrt(d_x**2 + d_y**2) return length @@ -408,8 +424,8 @@ def test_dwa(plot: bool = False, figure_name: str = "dwa", figure_tag: str = "dw max_angular_samples=4, octree_resolution=0.1, costs_weights=cost_weights, - prediction_horizon=1.0, - control_horizon=0.2, + prediction_horizon=10, + control_horizon=2, control_time_step=control_time_step, max_num_threads=1, ) @@ -429,6 +445,148 @@ def test_dwa(plot: bool = False, figure_name: str = "dwa", figure_tag: str = "dw assert reached_end is True +def test_vision_dwa_with_depth_img(): + """Run VisionRGBDFollower pytest and assert reaching end""" + global global_path, my_robot, robot_ctr_limits, control_time_step + + from kompass_core import set_logging_level + + set_logging_level("DEBUG") + + my_robot.state.x = -0.5 + + image_file_path = f"{control_resources}/bag_image_depth.tif" + # clicked point on image to track target + clicked_point = [610, 200] + # Detected 2D box + box = Bbox2D(top_left_corner=np.array([410, 0]), size=np.array([410, 390])) + detections = [box] + + point_cloud = [np.array([10.3, 10.5, 0.2], dtype=np.float32)] + + control_horizon = 2 + prediction_horizon = 10 + + focal_length = [911.71, 910.288] + principal_point = [643.06, 366.72] + + cost_weights = TrajectoryCostsWeights( + reference_path_distance_weight=1.0, + goal_distance_weight=0.0, + smoothness_weight=0.0, + jerk_weight=0.0, + obstacles_distance_weight=0.0, + ) + config = VisionRGBDFollowerConfig( + control_time_step=control_time_step, + control_horizon=control_horizon, + prediction_horizon=prediction_horizon, + speed_gain=1.0, + rotation_gain=1.0, + costs_weights=cost_weights, + max_linear_samples=20, + max_angular_samples=20, + octree_resolution=0.1, + max_num_threads=1, + min_depth=0.001, + max_depth=5.0, + depth_conversion_factor=0.001, + camera_position_to_robot=np.array([0.32, 0.02089, 0.2999]), + camera_rotation_to_robot=np.array([0.385, -0.5846, 0.595, -0.395]), + ) + + controller = VisionRGBDFollower( + robot=my_robot, + ctrl_limits=robot_ctr_limits, + config=config, + camera_focal_length=focal_length, + camera_principal_point=principal_point, + ) + + # Get image + cv_img = cv2.imread(image_file_path, cv2.IMREAD_UNCHANGED) + + if cv_img is None: + logging.error("Could not open or find the image") + return + + # Create a NumPy array from the OpenCV Mat + depth_image = np.array(cv_img, dtype=np.ushort, order="F") + + found_target = controller.set_initial_tracking_image( + my_robot.state, clicked_point[0], clicked_point[1], detections, depth_image + ) + + if not found_target: + print("Point not found on image") + return + else: + print("Point found on image ...") + + res = controller.loop_step( + current_state=my_robot.state, + detections_2d=detections, + depth_image=depth_image, + point_cloud=point_cloud, + ) + if not res: + print("No control found") + + assert res + + velocities = controller.control_till_horizon + (vx, vy, omega) = velocities.vx[0], velocities.vy[0], velocities.omega[0] + print(f"Found Control {vx}, {vy}, {omega}") + + +def test_vision_rgb_follower(): + """Run VisionRGBFollower pytest and assert reaching end""" + global global_path, my_robot, robot_ctr_limits, control_time_step + + from kompass_core import set_logging_level + + set_logging_level("DEBUG") + + my_robot.state.x = -0.5 + + box = Bbox2D(top_left_corner=np.array([410, 0]), size=np.array([410, 390])) + box.set_img_size(np.array([640, 480], dtype=np.int32)) + detections = [box] + + config = VisionRGBFollowerConfig( + control_time_step=control_time_step, speed_gain=1.0, rotation_gain=1.0 + ) + + controller = VisionRGBFollower( + robot=my_robot, + ctrl_limits=robot_ctr_limits, + config=config, + ) + + found_target = controller.set_initial_tracking_2d_target(box) + + if not found_target: + print("Point not found on image") + return + else: + print("Point found on image ...") + + res = controller.loop_step( + detections_2d=detections, + ) + if not res: + print("No control found") + + assert res + + (vx, vy, omega) = ( + controller.linear_x_control, + controller.linear_y_control, + controller.angular_control, + ) + print(f"Found Control {vx}, {vy}, {omega}") + + def test_dwa_debug(): global global_path, my_robot, robot_ctr_limits, control_time_step @@ -444,8 +602,8 @@ def test_dwa_debug(): max_angular_samples=21, octree_resolution=0.1, costs_weights=cost_weights, - prediction_horizon=1.0, - control_horizon=0.2, + prediction_horizon=10, + control_horizon=2, control_time_step=control_time_step, max_num_threads=1, ) @@ -477,6 +635,14 @@ def test_dwa_debug(): dwa.planner.debug_velocity_search(current_velocity, sensor_data, False) (paths_x, paths_y) = dwa.planner.get_debugging_samples() + + try: + import matplotlib.pyplot as plt + except ImportError: + logger.warning( + "Matplotlib is required for visualization. Figures will not be generated. To generate test figures, install it using 'pip install matplotlib'." + ) + return _, ax = plt.subplots() # Add the two laserscan obstacles obstacle_1 = plt.Circle((-0.117319, 0), 0.1, color="black", label="obstacle") @@ -515,7 +681,7 @@ def run_before_and_after_tests(): robot_ctr_limits = RobotCtrlLimits( vx_limits=LinearCtrlLimits(max_vel=1.0, max_acc=5.0, max_decel=10.0), omega_limits=AngularCtrlLimits( - max_vel=3.0, max_acc=5.0, max_decel=10.0, max_steer=np.pi + max_vel=4.0, max_acc=3.0, max_decel=3.0, max_steer=np.pi ), ) @@ -541,7 +707,7 @@ def main(): 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 + max_vel=4.0, max_acc=3.0, max_decel=3.0, max_steer=np.pi ), ) @@ -568,6 +734,14 @@ def main(): print("RUNNING DWA CONTROLLER TEST") test_dwa(plot=True, figure_name="dwa", figure_tag="DWA Controller Test Results") + ## TESTING VISION RGBD Follower (VISION DWA) ## + print("RUNNING VISION DWA CONTROLLER TEST") + test_vision_dwa_with_depth_img() + + ## TESTING VISION RGB Follower ## + print("RUNNING VISION RGB FOLLOWER TEST") + test_vision_rgb_follower() + if __name__ == "__main__": main() diff --git a/tests/test_fcl.py b/tests/test_fcl.py index eeaeab95..d27885c4 100644 --- a/tests/test_fcl.py +++ b/tests/test_fcl.py @@ -344,7 +344,7 @@ def test_fcl(save_results: bool = False): "collision_manager_1_data": rdata.result.is_collision, } results["pointcloud_with_manager"] = res_dict - os.makedirs('logs', exist_ok=True) + os.makedirs("logs", exist_ok=True) with open("logs/fcl_result.json", "w") as f: json.dump(results, f) diff --git a/tests/test_laserscan_emergency_stop.py b/tests/test_laserscan_emergency_stop.py index 751bba0d..c0047e35 100644 --- a/tests/test_laserscan_emergency_stop.py +++ b/tests/test_laserscan_emergency_stop.py @@ -3,7 +3,6 @@ import numpy as np import logging import pytest -import matplotlib.pyplot as plt from kompass_core.datatypes import LaserScanData from kompass_core.utils.geometry import get_laserscan_transformed_polar_coordinates from kompass_core.utils.emergency_stop import EmergencyChecker @@ -49,7 +48,6 @@ def test_laserscan_polar_tf(laser_scan_data: LaserScanData, plot: bool = False): # 90 Deg rotation around z translation = [0.0, 0.0, 0.173] rotation = [0.0, 0.0, 0.7071068, 0.7071068] - # rotation = [0.0, 0.0, 0.0, 1.0] transformed_scan = get_laserscan_transformed_polar_coordinates( angle_min=laser_scan_data.angle_min, @@ -60,20 +58,28 @@ def test_laserscan_polar_tf(laser_scan_data: LaserScanData, plot: bool = False): translation=translation, rotation=rotation, ) - if plot: - fig, ax = plt.subplots(subplot_kw={"projection": "polar"}) - ax.plot( - laser_scan_data.angles, laser_scan_data.ranges, label="Original LaserScan" - ) - ax.plot( - transformed_scan.angles, - transformed_scan.ranges, - label="Transformed LaserScan", - ) - fig.legend() - dir_name = os.path.dirname(os.path.abspath(__file__)) - plt.savefig(os.path.join(dir_name, "laserscan_tf_test.png")) + try: + import matplotlib.pyplot as plt + + fig, ax = plt.subplots(subplot_kw={"projection": "polar"}) + ax.plot( + laser_scan_data.angles, + laser_scan_data.ranges, + label="Original LaserScan", + ) + ax.plot( + transformed_scan.angles, + transformed_scan.ranges, + label="Transformed LaserScan", + ) + fig.legend() + dir_name = os.path.dirname(os.path.abspath(__file__)) + plt.savefig(os.path.join(dir_name, "laserscan_tf_test.png")) + except ImportError: + logging.warning( + "Matplotlib is required for visualization. Figures will not be generated. To generate test figures, install it using 'pip install matplotlib'." + ) old_range = laser_scan_data.ranges[ laser_scan_data.angles == laser_scan_data.angle_min @@ -108,7 +114,17 @@ def test_laserscan_partial_data(laser_scan_data: LaserScanData, plot: bool = Fal right_angle=right_angle, left_angle=left_angle ) + assert len(partial_angles) <= laser_scan_data.angles.size + assert len(partial_ranges) <= laser_scan_data.ranges.size + if plot: + try: + import matplotlib.pyplot as plt + except ImportError: + logging.warning( + "Matplotlib is required for visualization. Figures will not be generated. To generate test figures, install it using 'pip install matplotlib'." + ) + return fig, ax = plt.subplots(subplot_kw={"projection": "polar"}) ax.plot( laser_scan_data.angles, laser_scan_data.ranges, label="Original LaserScan" @@ -118,9 +134,6 @@ def test_laserscan_partial_data(laser_scan_data: LaserScanData, plot: bool = Fal dir_name = os.path.dirname(os.path.abspath(__file__)) plt.savefig(os.path.join(dir_name, "laserscan_partial_test.png")) - assert len(partial_angles) <= laser_scan_data.angles.size - assert len(partial_ranges) <= laser_scan_data.ranges.size - @pytest.mark.parametrize("use_gpu", [False, True]) def test_emergency_stop(laser_scan_data: LaserScanData, use_gpu): @@ -136,6 +149,7 @@ def test_emergency_stop(laser_scan_data: LaserScanData, use_gpu): geometry_params=np.array([robot_radius, 0.4]), ) emergency_distance = 0.5 + slowdown_distance = 1.0 emergency_angle = 90.0 large_range = 10.0 @@ -144,9 +158,10 @@ def test_emergency_stop(laser_scan_data: LaserScanData, use_gpu): emergency_stop = EmergencyChecker( robot=robot, emergency_distance=emergency_distance, + slowdown_distance=slowdown_distance, emergency_angle=emergency_angle, - sensor_position_robot=[0.0, 0.0, 0.173], - sensor_rotation_robot=[0.0, 0.0, 0.0, 1.0], + sensor_position_robot=np.array([0.0, 0.0, 0.173], dtype=np.float32), + sensor_rotation_robot=np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float32), use_gpu=use_gpu, ) angles_size = np.arange( @@ -156,12 +171,12 @@ def test_emergency_stop(laser_scan_data: LaserScanData, use_gpu): ).shape[0] laser_scan_data.ranges = np.array([large_range] * angles_size) - assert not emergency_stop.run(scan=laser_scan_data, forward=True) + assert emergency_stop.run(scan=laser_scan_data, forward=True) == 1.0 # Add an obstacle in the critical zone in front of the robot laser_scan_data.ranges[0] = emergency_value - assert emergency_stop.run(scan=laser_scan_data, forward=True) - assert not emergency_stop.run(scan=laser_scan_data, forward=False) + assert emergency_stop.run(scan=laser_scan_data, forward=True) == 0.0 + assert emergency_stop.run(scan=laser_scan_data, forward=False) == 1.0 if __name__ == "__main__": diff --git a/tests/test_ompl.py b/tests/test_ompl.py index a51a995f..caf8e6c4 100644 --- a/tests/test_ompl.py +++ b/tests/test_ompl.py @@ -4,7 +4,6 @@ import timeit from typing import Dict import numpy as np -import pandas as pd import omplpy as ompl from kompass_core.models import Robot, RobotGeometry, RobotType @@ -19,10 +18,6 @@ SOLUTION_TOLERANCE_TIME = 0.3 SOLUTION_TOLERANCE_LENGTH = 0.1 -REF_RESULTS = pd.read_csv( - os.path.join(ompl_resources, "test_results_geometric_ref.csv") -) - def generate_all_geometric_planners_configs(): """ @@ -41,9 +36,7 @@ def generate_all_geometric_planners_configs(): print("-----------------------------\n") -def ompl_solve_once( - ompl_planner: OMPLGeometric, map_data: Dict, map_numpy: np.ndarray -): +def ompl_solve_once(ompl_planner: OMPLGeometric, map_data: Dict, map_numpy: np.ndarray): """ Setup and solve OMPL planning problem with given map and map metadata @@ -115,23 +108,29 @@ def load_map_meta(map_file: str) -> Dict: raise Exception(f"Failed to load map metadata: {str(e)}") from e -def ompl_geometric_testing(test_repetitions: int = 1): +def ompl_geometric_testing(test_repetitions: int = 10): """ Test all OMPL geometric planners :param test_repetitions: _description_, defaults to 7 :type test_repetitions: int, optional """ - ompl_df = pd.DataFrame( - columns=( - "method", - "solved", - "solution_time", - "solution_len", - "simplification_time", - "time_convert_2_ros", + try: + import pandas as pd + + ompl_df = pd.DataFrame( + columns=( + "method", + "solved", + "solution_time", + "solution_len", + ) ) - ) + except Exception: + logger.error( + "Pandas is not installed. Result CSV will not be generated. Please install pandas if you wish to generate a CSV of the results." + ) + ompl_df = None robot = Robot( robot_type=RobotType.ACKERMANN, @@ -150,7 +149,6 @@ def ompl_geometric_testing(test_repetitions: int = 1): for planner_id in ompl_planner.available_planners.keys(): solution_time: float = 0.0 - simplify_time: float = 0.0 sol_len: float = 0.0 if planner_id in ["ompl.geometric.AITstar", "ompl.geometric.LazyLBTRRT"]: continue @@ -172,39 +170,37 @@ def ompl_geometric_testing(test_repetitions: int = 1): solution_time += end_time - start_time - solved = True + solved = path is not None if path: - start_time = timeit.default_timer() - - path = ompl_planner.simplify_solution() - - end_time = timeit.default_timer() - - simplify_time += end_time - start_time - - solved = True - - sol_len += ompl_planner.solution.length() - else: - solved = False + sol_len += path.length() except Exception as e: logging.error(f"{e}") - ompl_df.loc[len(ompl_df)] = { - "method": planner_id, - "solved": solved, - "solution_time": solution_time / test_repetitions, - "solution_len": sol_len / test_repetitions, - "simplification_time": simplify_time / test_repetitions, - } - - os.makedirs("logs", exist_ok=True) - ompl_df.to_csv("logs/ompl_test_results.csv", index=False) + if ompl_df is not None: + ompl_df.loc[len(ompl_df)] = { + "method": planner_id, + "solved": solved, + "solution_time": solution_time / test_repetitions, + "solution_len": sol_len / test_repetitions, + } + if ompl_df is not None: + os.makedirs("logs", exist_ok=True) + ompl_df.to_csv("logs/ompl_test_results.csv", index=False) return ompl_df def ompl_test_all(): + try: + import pandas as pd + except ImportError: + logger.error( + "Pandas is not installed. Result CSV will not be generated. lease install it to run OMPL test." + ) + + REF_RESULTS = pd.read_csv( + os.path.join(ompl_resources, "test_results_geometric_ref.csv") + ) logging.info("Running all OMPL planners tests") results_df = ompl_geometric_testing(test_repetitions=20) logging.info("Done all OMPL planners tests")