Metadata-Version: 2.4
Name: fastsim-navigation
Version: 0.1.2
Summary: Native C++17 Hybrid A* SE(2) navigation for Python
License-Expression: MIT
Classifier: Development Status :: 3 - Alpha
Classifier: Intended Audience :: Developers
Classifier: Intended Audience :: Science/Research
Classifier: Operating System :: POSIX :: Linux
Classifier: Programming Language :: C++
Classifier: Programming Language :: Python :: 3
Classifier: Programming Language :: Python :: 3 :: Only
Classifier: Topic :: Scientific/Engineering :: Artificial Intelligence
Project-URL: Documentation, https://pypi.org/project/fastsim-navigation/
Requires-Python: >=3.9
Requires-Dist: numpy>=1.24
Description-Content-Type: text/markdown

# FastSim C++ SE(2) Navigation

这是从 FastSim 中独立出来的、与仿真器无关的 C++17 Hybrid A* 导航库。核心库不依赖 ROS、Nav2、Isaac、USD 或 Python 运行时，可直接嵌入机器人程序；Python 绑定是可选模块。

功能包括：

- Dubins / Reeds-Shepp 运动学和解析扩展
- 前进、倒车、方向切换以及可选原地旋转
- 圆形、多边形和多多边形机器人足迹
- `uint8` 代价地图、未知区域策略和代价地图膨胀
- 障碍物启发式缓存、路径平滑、重采样和曲率校验
- 超时、节点上限、线程安全取消和地图/目标/机器人版本透传
- 完整路径、每点运动方向、状态、消息和性能诊断返回
- C++、Python、CLI 三种调用方式
- CSV / PGM 地图输入，CSV / JSON 路径输出，SVG 路径绘图

## Python 安装

CPython 3.11、Linux x86-64、glibc 2.35 及以上可直接安装预编译 wheel。OMPL、ODE、CCD 和所需 Boost 运行库已包含在 wheel 中：

```bash
pip install fastsim-navigation
```

安装后使用：

```python
import fastsim_navigation as se2
```

其他 Python 或操作系统版本会尝试从源码构建，需要预先安装 CMake、C++17 编译器、Eigen3、OMPL 和 pybind11。

## 构建

依赖：CMake 3.20+、C++17 编译器、Eigen3、OMPL 1.5+；构建 Python 模块时还需要 pybind11。

```bash
cmake -S . -B build \
  -DCMAKE_BUILD_TYPE=Release \
  -DFASTSIM_OMPL_ROOT=/path/to/ompl/prefix \
  -Dpybind11_DIR="$(python -m pybind11 --cmakedir)"
cmake --build build -j
ctest --test-dir build --output-on-failure
```

如果 OMPL 安装在系统搜索路径，可省略 `FASTSIM_OMPL_ROOT`。不需要某些目标时可关闭：

```bash
cmake -S . -B build \
  -DFASTSIM_SE2_BUILD_PYTHON=OFF \
  -DFASTSIM_SE2_BUILD_BENCHMARKS=OFF \
  -DFASTSIM_SE2_BUILD_EXAMPLES=OFF
```

安装后，下游 CMake 工程可使用：

```cmake
find_package(fastsim_se2_planner CONFIG REQUIRED)
target_link_libraries(my_robot PRIVATE fastsim::fastsim_se2_planner)
```

## C++ 接口

统一入口头文件是 `fastsim/se2/planner.hpp`：

```cpp
#include <fastsim/se2/planner.hpp>

using namespace fastsim::se2;

std::vector<std::uint8_t> cells(width * height, 0);  // row-major, y=0 first
Costmap map(width, height, resolution, origin_x, origin_y, std::move(cells));

PlanRequest request;
request.start = {1.0, 1.0, 0.0};           // meters, meters, radians
request.goal = {7.0, 7.0, 0.0};
request.footprint = CircleFootprint{0.20};
request.map_revision = 10;
request.goal_revision = 20;
request.robot_revision = 30;

PlannerConfig config;
config.minimum_turning_radius = 0.40;
config.allow_reverse = true;
config.max_planning_time_seconds = 2.0;

HybridAStar planner(map);                  // reuse to retain heuristic cache
CancellationToken token;
PlannerResult result = planner.plan(request, config, &token);

if (result.status == PlannerStatus::SUCCESS) {
  // result.path: Pose2D{x, y, yaw}
  // result.directions: +1 forward, -1 reverse, 0 rotation
  savePathCsv(result, "path.csv");
  saveResultJson(result, "result.json");
  savePlanSvg(map, request, result, "plan.svg");
}
```

多边形和多多边形足迹：

```cpp
request.footprint = PolygonFootprint{{
    {-0.30, -0.20}, {0.30, -0.20}, {0.30, 0.20}, {-0.30, 0.20}}};

request.footprint = MultiPolygonFootprint{{front_polygon, rear_polygon}};
```

公开接口按职责分布在 `include/fastsim/se2/`：

| 接口 | 用途 |
|---|---|
| `Costmap` | 数组代价地图、世界/栅格坐标转换 |
| `PlanRequest` / `PlannerConfig` | 起终点、足迹、版本和全部规划参数 |
| `HybridAStar::plan` | 规划和取消 |
| `PlannerResult` | 状态、路径、方向、诊断和版本 |
| `CollisionChecker` | 单姿态/整条路径碰撞检查 |
| `inflateCostmap` | 代价地图膨胀 |
| `KinematicStateSpace` | Dubins / Reeds-Shepp 距离与插值 |
| `PathSmoother` | 平滑、重采样、最小转弯半径检查 |
| `loadCostmapCsv/Pgm` | 文件地图输入 |
| `savePathCsv/saveResultJson` | 路径和完整结果输出 |
| `renderPlanSvg/savePlanSvg` | 内存字符串或文件绘图 |

栅格代价值约定：`0` 是自由区域，`1..252` 是软代价，`253/254` 是障碍物，`255` 是未知区域。地图数组为行优先，第一行是 `y=0`。PGM 按常规图像顶行优先读取，并自动翻转为世界坐标的 `y=0` 行；像素值按最大灰度缩放到 `0..255`，不做黑白反转。

## CLI：文件输入、路径返回和画图

```bash
./build/fastsim-se2-plan \
  --map examples/map.csv --resolution 0.1 \
  --origin 0 0 \
  --start 0.25 0.25 0 --goal 1.75 1.75 0 \
  --radius 0.08 \
  --max-time 2 \
  --path-csv path.csv \
  --result-json result.json \
  --svg plan.svg
```

使用 `--polygon footprint.csv` 替换圆形足迹；重复传入该参数即可创建多多边形足迹。`--help` 列出全部 CLI 参数。即使规划失败，JSON 和 SVG 仍会输出，便于检查状态、起终点和地图。

## Python 接口

通过 pip 安装后，可以直接调用与 C++ 对应的绑定：

```python
import numpy as np
import fastsim_navigation as se2

cells = np.zeros((80, 80), dtype=np.uint8)
costmap = se2.Costmap(cells, 0.1, 0.0, 0.0)
planner = se2.HybridAStar(costmap)

request = se2.PlanRequest()
request.start = se2.Pose2D(1.0, 1.0, 0.0)
request.goal = se2.Pose2D(7.0, 7.0, 0.0)
request.footprint = se2.CircleFootprint(0.2)

config = se2.PlannerConfig()
result = planner.plan(request, config)
poses = result.path_array          # shape: (N, 3), float64
directions = result.directions_array  # shape: (N,), int8
se2.save_result_json(result, "result.json")
se2.save_plan_svg(costmap, request, result, "plan.svg")
```

Python 还提供 `cells_array`、CSV/PGM 读取、碰撞检查、膨胀、运动学插值、平滑、取消、状态名称和 SVG 字符串返回接口。

## 验证和示例

```bash
ctest --test-dir build --output-on-failure
./build/se2_planner_example
PYTHONPATH=build python -c "import _se2_planner; print(_se2_planner.__doc__)"
```

示例会在当前目录生成 `example_path.csv`、`example_result.json` 和 `example_plan.svg`。第三方依赖与设计参考见 `THIRD_PARTY_NOTICES.md`。
