# Artefacts ツールキットのナビゲーションヘルパー

Site [llms.txt](/ja/llms.txt)

---

Artefacts ツールキットのナビゲーションヘルパーは、自動ナビゲーションテストのための便利な関数と CLI ツールを提供します。RViz 上で直接ルートを記録し、その 2D マップからシミュレータ用のワールドを生成し、テストでルートを読み込んで走行させることができます。

インポート方法：

```python
from artefacts_toolkit.navigation import <function>
```

> [!NOTE]
> * ナビゲーションヘルパーは現在 Nav2 に強く依存しており、ROS 2 Humble（Python 3.10）と Jazzy（Python 3.12）でテストされています。`ros-<distro>-nav2-simple-commander` と `ros-<distro>-tf2-ros` が必要です。
> * ルートは、ツールキットに含まれる `artefacts-route-recorder` の `artefacts-route` コマンドで記録します。[ルートの記録](#recording-a-route)を参照してください。

## ルートの記録 {#recording-a-route}

ルートは、記録時の 2D Pose Estimate（`initial_pose`）、ウェイポイント、および記録に使用したマップを保持する YAML ファイルです。マップの `yaml` と `pgm` の両方のファイルが必要です。テストを実行するディレクトリから記録してください：

```bash
artefacts-route record [save/to/route.yaml] --map <map/to/load.yaml>
```

マップを読み込んだ Nav2 の `map_server` と RViz が起動します。**2D Pose Estimate** で開始位置を設定し、**Publish Point** でウェイポイントをクリックしてください。RViz を閉じるか、ターミナルで Enter を押すとルートが `routes/<date>_<time>.yaml` に保存されます。特定の場所に保存するにはパス（`[save/to/route.yaml]`）を渡してください。

| **オプション**       | **説明**                                                                                          |
| ------------------- | ------------------------------------------------------------------------------------------------ |
| `FILE`              | ルートの保存先（英数字、`-`、`_` が使用可能）。デフォルト：`./routes/<date>_<time>.yaml`             |
| `--map MAP_YAML`    | このマップで `map_server` と RViz を起動し、マップの参照を記録する                                     |
| `--map-ref MAP_YAML`| 何も起動せずに、このマップのパスとハッシュを記録する（RViz とマップがすでに起動している場合）            |
| `--force`           | `FILE` が存在する場合に上書きする                                                                  |

その他のサブコマンド：

* `artefacts-route list`：保存されたルートを一覧表示します。
* `artefacts-route show`：ルートとその問題点を表示します。
* `artefacts-route replay`：ルートを RViz のマーカーとしてパブリッシュします。

## 関数

起動ファイル用ヘルパー：
* [`load_route`](#load_route)
* [`route_launch_args`](#route_launch_args)
* [`route_start_pose`](#route_start_pose)
* [`check_map_match`](#check_map_match)
* [`map_to_world`](#map_to_world)

テスト用ヘルパー：
* [`follow_route`](#follow_route)
* [`assert_route_completed`](#assert_route_completed)
* [`assert_each_waypoint_reached`](#assert_each_waypoint_reached)
* [`assert_final_pose`](#assert_final_pose)
* [`assert_min_clearance`](#assert_min_clearance)
* [`assert_max_recoveries`](#assert_max_recoveries)

## 関数リファレンス

### `load_route`

`artefacts-route record` で記録したルートを読み込みます。

```python
load_route(
    route=None
)
```

#### パラメータ

| **パラメータ** | **型**          | **説明**                                                                          | **デフォルト** |
| ------------- | --------------- | -------------------------------------------------------------------------------- | ------------- |
| `route`       | `str` または `Path` | ルートファイルへのパス。`None` の場合、`./routes/` から最後に記録されたルートを読み込む | `None`        |

#### 戻り値

`Route`：読み込まれたルート。`name`、`frame_id`、`initial_pose`、`waypoints`、`map` 属性を持ちます。

> [!NOTE]
> * ルートが存在しない、またはファイルを読み込めない場合は `UserError` が発生します。

#### 例

```python
from artefacts_toolkit.navigation import load_route

route = load_route()                     # 最後に記録されたルート
route = load_route("routes/lab.yaml")    # 特定のルート
```

---

### `route_launch_args`

ルートの開始位置にロボットをスポーンさせる起動引数を返します。

```python
route_launch_args(
    route,
    x=None,
    y=None,
    yaw=None,
    names=("x_pose", "y_pose", "yaw"),
    map_arg="map"
)
```

#### パラメータ

| **パラメータ** | **型**          | **説明**                                                                       | **デフォルト**                  |
| ------------- | --------------- | ----------------------------------------------------------------------------- | ------------------------------ |
| `route`       | `Route`         | [`load_route`](#load_route) で読み込んだルート                                   | 必須                           |
| `x`           | `float`         | 記録された開始位置の代わりに、この x にロボットをスポーンさせる                      | `None`                         |
| `y`           | `float`         | 記録された開始位置の代わりに、この y にロボットをスポーンさせる                      | `None`                         |
| `yaw`         | `float`         | 記録された開始位置の代わりに、この向き（ラジアン）でロボットをスポーンさせる           | `None`                         |
| `names`       | `Sequence[str]` | 起動ファイルで宣言しているスポーン位置の引数名（デフォルトと異なる場合）              | `("x_pose", "y_pose", "yaw")`  |
| `map_arg`     | `str` または `None` | マップの起動引数名。`None` の場合はマップを含めない                             | `"map"`                        |

#### 戻り値

`dict[str, str]`：例：`{"x_pose": "0.59", "y_pose": "-1.45", "yaw": "1.5708", "map": "/maps/lab.yaml"}`

#### 例

```python
from artefacts_toolkit.navigation import load_route, route_launch_args

route = load_route()
args = route_launch_args(route)
IncludeLaunchDescription(my_sim_launch, launch_arguments=args.items())
```

---

### `route_start_pose`

ルートの開始位置（記録時の 2D Pose Estimate）を返します。各要素を個別に上書きすることもできます。

```python
route_start_pose(
    route,
    x=None,
    y=None,
    yaw=None
)
```

#### パラメータ

| **パラメータ** | **型**   | **説明**                              | **デフォルト** |
| ------------- | -------- | ------------------------------------- | ------------- |
| `route`       | `Route`  | ルート                                 | 必須          |
| `x`           | `float`  | 開始位置の x を上書きする               | `None`        |
| `y`           | `float`  | 開始位置の y を上書きする               | `None`        |
| `yaw`         | `float`  | 開始位置の yaw（ラジアン）を上書きする    | `None`        |

#### 戻り値

`Pose2D`：`x`、`y`、`yaw` 属性を持つ開始位置。

> [!NOTE]
> * ルートが 2D Pose Estimate なしで記録された場合、`x`、`y`、`yaw` をすべて指定する必要があります。指定しない場合は `UserError` が発生します。

---

### `check_map_match`

`map_yaml` がルートの記録に使用したマップと異なる場合に `UserError` を発生させます。誤ったマップでルートを走行させないために役立ちます。

```python
check_map_match(
    route,
    map_yaml,
    ignore_mismatch=False
)
```

#### パラメータ

| **パラメータ**     | **型**          | **説明**                                            | **デフォルト** |
| ----------------- | --------------- | -------------------------------------------------- | ------------- |
| `route`           | `Route`         | ルート                                              | 必須          |
| `map_yaml`        | `str` または `Path` | 起動ファイルが読み込むマップ                         | 必須          |
| `ignore_mismatch` | `bool`          | マップが異なる場合、エラーの代わりに警告にする          | `False`       |

#### 戻り値

`list[str]`：警告。ルートに比較対象のマップハッシュがない場合や、不一致を無視した場合など。

> [!NOTE]
> * 起動ファイルがルートからマップを取得しない場合にのみ必要です（[`route_launch_args`](#route_launch_args) を参照）。

---

### `map_to_world`

Nav2 のマップ（`yaml` と `pgm`。実機ロボットで保存したものなど）から押し出したシミュレータ用ワールドを書き出します。Isaac Sim、Newton、MuJoCo、Gazebo に対応しています。
ワールドの生成は比較的軽量（ミリ秒単位）なので、毎回実行時に生成できます。

```python
map_to_world(
    map_yaml,
    out=None,
    format=None,
    wall_height=1.0,
    fill_gaps=True,
    crop=None
)
```

#### パラメータ

| **パラメータ** | **型**                | **説明**                                                                                                          | **デフォルト**               |
| ------------- | --------------------- | ---------------------------------------------------------------------------------------------------------------- | --------------------------- |
| `map_yaml`    | `str` または `Path`     | Nav2 のマップ yaml（`pgm` も同じ場所に必要です）                                                                    | 必須                        |
| `out`         | `str` または `Path`     | ワールドの書き出し先（必要な場合）                                                                                  | `worlds/<map name>.<ext>`   |
| `format`      | `str`                 | `"usda"`（Isaac Sim、Newton）、`"mjcf"`（MuJoCo、Newton）、または `"sdf"`（Gazebo）。`out` が指定されている場合はその拡張子から決まる | `"usda"`                    |
| `wall_height` | `float`               | 壁の高さ（メートル）                                                                                              | `1.0`                       |
| `fill_gaps`   | `bool`                | 押し出す前に、占有セル間の 2 セル以下の隙間（スキャンノイズ）を埋める                                                   | `True`                      |
| `crop`        | `tuple[Route, float]` | `(route, metres)`：ルートに沿ってロボットが通る経路から指定メートル以内の壁のみを残す。大きなマップでの物理演算を削減する    | `None`                      |

#### 戻り値

`str`：書き出されたワールドファイルの絶対パス。シミュレータに渡します。

> [!NOTE]
> * `usda` と `mjcf` のワールドは地面と壁のみを含み、ロボットとセンサーはシミュレータ側のものを使用します。`sdf` は完全な Gazebo ワールドです。
> * ワールドの生成はミリ秒単位で完了するため、起動ファイル内で実行時に生成し、ファイルは使い捨てとして扱ってください。
> * このワールドは走行用であり、見栄えの良い環境を作るためのものではありません。

#### 例

以下の起動ファイルは、最後に記録されたルートを読み込み、そのマップから Gazebo ワールドを押し出し、ルートの開始位置にロボットを配置した状態で Nav2 の TurtleBot3 デモをそのワールドに含めます：

```python
import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration

from artefacts_toolkit.navigation import load_route, map_to_world, route_launch_args


def include_demo(context):
    route = load_route()  # 最後に記録されたルート
    headless = LaunchConfiguration("headless").perform(context)

    # x_pose、y_pose、yaw、map。デモが使用する名前のまま。
    args = route_launch_args(route)
    args["headless"] = headless
    args["use_rviz"] = str(headless.lower() not in ("true", "1", "yes"))
    # ワールド：ルートのマップを壁として押し出したもの（ミリ秒単位）。
    # これによりシミュレーションがルートの記録に使用したマップと一致する。
    args["world"] = map_to_world(route.map.file, format="sdf")

    demo = os.path.join(
        get_package_share_directory("nav2_bringup"), "launch", "tb3_simulation_launch.py")
    return [IncludeLaunchDescription(
        PythonLaunchDescriptionSource(demo), launch_arguments=args.items())]


def generate_launch_description():
    return LaunchDescription([
        DeclareLaunchArgument(
            "headless", default_value="True", description="no Gazebo GUI and no RViz"),
        OpaqueFunction(function=include_demo),
    ])
```

---

### `follow_route`

起動中の Nav2 スタックでルートを走行させ、[`RouteResult`](#routeresult) を返します。
テストファイル内で使用します。

```python
follow_route(
    route=None,
    timeout_s=300.0,
    startup_timeout_s=60.0,
    label=None,
    results_dir=None,
    set_initial_pose=True,
    base_frame="base_link",
    scan_topic="scan"
)
```

#### パラメータ

| **パラメータ**       | **型**              | **説明**                                                                                         | **デフォルト**   |
| ------------------- | ------------------- | ----------------------------------------------------------------------------------------------- | --------------- |
| `route`             | `str` または `Path`   | 走行させるルートへのパス（[`load_route`](#load_route) と同様）。`None` の場合は最後に記録されたルートを走行させる | `None`          |
| `timeout_s`         | `float` または `None` | この秒数を過ぎるとゴールがキャンセルされ、結果は `"timeout"` になる。`None` の場合は無期限に待機する      | `300.0`         |
| `startup_timeout_s` | `float`             | Nav2 がアクティブになるまで待機する秒数。超過すると `EnvError` が発生する                             | `60.0`          |
| `label`             | `str`               | 結果とそのファイル名に付加するラベル                                                               | `None`          |
| `results_dir`       | `str` または `Path`   | 結果を YAML ファイルとして書き出すディレクトリ                                                      | `None`          |
| `set_initial_pose`  | `bool`              | 走行前に、ロボットがルートの開始位置にいることを AMCL に伝える。すでに自己位置推定済みのロボットでは `False` | `True`          |
| `base_frame`        | `str`               | 最終位置誤差の計算に使用するロボットのフレーム                                                       | `"base_link"`   |
| `scan_topic`        | `str` または `None`   | 障害物との距離を計測する `LaserScan` トピック。`None` の場合は計測しない                              | `"scan"`        |

#### 戻り値

[`RouteResult`](#routeresult)：走行の結果。失敗した走行も例外ではなく結果として返されます。

> [!NOTE]
> * Nav2 がすでに起動している必要があります。`follow_route` は順に、`amcl` と `bt_navigator` がアクティブになるのを待ち、ルートの初期位置をパブリッシュして AMCL が受け取るのを待ち、ウェイポイントを 1 つの `FollowWaypoints` ゴールとして送信し、その完了を待ち、最終位置誤差のために tf からロボットの位置を読み取ります。
> * 障害物との距離はロボットの外縁ではなく、LiDAR から計測されます。
> * 不正なルートの場合は `UserError`、ROS がソースされていない、Nav2 の Python API がない、またはスタックが見つからない場合は `EnvError` が発生します。
> * `results_dir` に環境変数 `ARTEFACTS_SCENARIO_UPLOAD_DIR` を設定すると、シナリオ終了時に結果の YAML が Artefacts ダッシュボードにアップロードされます。

#### 例

以下のテストは、[`map_to_world`](#map_to_world) の例の起動ファイルをフィクスチャで実行し、最後に記録されたルートを走行させ、その結果に対してアサーションを行います：

```python
import os
import signal
import subprocess
import time
from pathlib import Path

import pytest

from artefacts_toolkit.navigation import (
    assert_each_waypoint_reached, assert_final_pose, assert_max_recoveries,
    assert_min_clearance, follow_route)

HEADLESS = os.environ.get("HEADLESS", "false").lower() not in ("0", "false", "no")
RESULTS_DIR = os.environ.get("ARTEFACTS_SCENARIO_UPLOAD_DIR", "results")
LAUNCH_FILE = Path(__file__).with_name("nav2_sim.launch.py")


@pytest.fixture
def sim(tmp_path):
    """TurtleBot3 デモ。テストごとに起動し、終了後に停止する。"""
    command = ["ros2", "launch", str(LAUNCH_FILE), "headless:={}".format(HEADLESS)]
    with (tmp_path / "sim.log").open("w") as out:
        proc = subprocess.Popen(command, stdout=out, stderr=subprocess.STDOUT,
                                start_new_session=True)
    time.sleep(15)  # Gazebo と Nav2 の起動を待つ
    try:
        yield
    finally:
        os.killpg(proc.pid, signal.SIGINT)  # ros2 launch がプロセスを順番に停止する
        proc.wait(timeout=30)


def test_route_is_completed(sim):
    result = follow_route(timeout_s=300, results_dir=RESULTS_DIR)  # 最後に記録されたルート
    assert_each_waypoint_reached(result, tolerance_m=0.5)
    assert_final_pose(result, max_pos_error_m=0.5)
    assert_min_clearance(result, 0.15)  # LiDAR からの距離。TurtleBot3 の LiDAR は中央付近にある
    assert_max_recoveries(result, 2)
```

---

> [!NOTE]
> `assert_*` ヘルパーは [`RouteResult`](#routeresult) をチェックします。これは [`follow_route`](#follow_route) のみが生成します。ツールキット自身がルートを走行させたことを前提としているため、別の方法でロボットを走行させている場合は使用できません。
>
> 各ヘルパーはアサーションに失敗すると `AssertionError` を発生させます。

### `assert_route_completed`

Nav2 がルートを完走し、ウェイポイントをスキップしなかった場合を除き、`AssertionError` を発生させます。

```python
assert_route_completed(
    result
)
```

#### パラメータ

| **パラメータ** | **型**        | **説明**                                      | **デフォルト** |
| ------------- | ------------- | -------------------------------------------- | ------------- |
| `result`      | `RouteResult` | [`follow_route`](#follow_route) の結果         | 必須          |

---

### `assert_each_waypoint_reached`

すべてのウェイポイントに到達し、`tolerance_m` が指定されている場合はその範囲内に到達した場合を除き、`AssertionError` を発生させます。

```python
assert_each_waypoint_reached(
    result,
    tolerance_m=None
)
```

#### パラメータ

| **パラメータ** | **型**        | **説明**                                              | **デフォルト** |
| ------------- | ------------- | ---------------------------------------------------- | ------------- |
| `result`      | `RouteResult` | [`follow_route`](#follow_route) の結果                 | 必須          |
| `tolerance_m` | `float`       | 各ウェイポイントにロボットがどれだけ近づく必要があるか      | `None`        |

> [!NOTE]
> * Nav2 の `xy_goal_tolerance` より緩い許容値では、ほぼ失敗しません。

---

### `assert_final_pose`

ロボットが最後のウェイポイントの十分近くで停止した場合を除き、`AssertionError` を発生させます。

```python
assert_final_pose(
    result,
    max_pos_error_m=0.5,
    max_yaw_error_rad=None
)
```

#### パラメータ

| **パラメータ**       | **型**        | **説明**                                      | **デフォルト** |
| ------------------- | ------------- | -------------------------------------------- | ------------- |
| `result`            | `RouteResult` | [`follow_route`](#follow_route) の結果         | 必須          |
| `max_pos_error_m`   | `float`       | 最後のウェイポイントからの最大距離                | `0.5`         |
| `max_yaw_error_rad` | `float`       | 最後のウェイポイントからの最大 yaw 誤差           | `None`        |

---

### `assert_min_clearance`

いずれかのレーザー測定値が `min_m` より近かった場合に `AssertionError` を発生させます。

```python
assert_min_clearance(
    result,
    min_m
)
```

#### パラメータ

| **パラメータ** | **型**        | **説明**                                      | **デフォルト** |
| ------------- | ------------- | -------------------------------------------- | ------------- |
| `result`      | `RouteResult` | [`follow_route`](#follow_route) の結果         | 必須          |
| `min_m`       | `float`       | 障害物までの最小許容距離（メートル）              | 必須          |

> [!NOTE]
> * LiDAR から計測されるため、LiDAR からロボットの外縁までの距離を加算してください。

---

### `assert_max_recoveries`

Nav2 が `max_recoveries` を超える回数のリカバリー動作（回転、後退、待機など）を実行した場合に `AssertionError` を発生させます。

```python
assert_max_recoveries(
    result,
    max_recoveries=0
)
```

#### パラメータ

| **パラメータ**    | **型**        | **説明**                                      | **デフォルト** |
| ---------------- | ------------- | -------------------------------------------- | ------------- |
| `result`         | `RouteResult` | [`follow_route`](#follow_route) の結果         | 必須          |
| `max_recoveries` | `int`         | 許容するリカバリー動作の最大回数                 | `0`           |

## `RouteResult`

1 回の [`follow_route`](#follow_route) の走行結果です。

| **属性**              | **型**                  | **説明**                                                        |
| --------------------- | ----------------------- | --------------------------------------------------------------- |
| `succeeded`           | `bool`                  | Nav2 がルートを完走し、ウェイポイントをスキップしなかった             |
| `outcome`             | `str`                   | `"succeeded"`、`"failed"`、`"canceled"`、または `"timeout"`       |
| `error`               | `str` または `None`       | outcome が `"succeeded"` でない場合のエラーメッセージ               |
| `duration_s`          | `float`                 | ルート送信から終了までの実時間（秒）                                |
| `waypoints_total`     | `int`                   | ルート内のウェイポイント数                                         |
| `missed_waypoints`    | `list[int]`             | Nav2 がスキップしたウェイポイントのインデックス                      |
| `waypoints`           | `list[WaypointResult]`  | 各ウェイポイントの結果（下記参照）                                  |
| `final_pos_error_m`   | `float` または `None`     | 終了時のロボットから最後のウェイポイントまでの距離（tf から取得）       |
| `final_yaw_error_rad` | `float` または `None`     | 終了時の最後のウェイポイントとの yaw 誤差                            |
| `recoveries`          | `int`                   | ルート全体で Nav2 が実行したリカバリー動作の回数                      |
| `min_clearance_m`     | `float` または `None`     | ルート全体での最も近いレーザー測定値                                |
| `warnings`            | `list[str]`             | 計測できなかった項目とその理由                                      |

`result.waypoints` 内の各 `WaypointResult` は以下を持ちます：

| **属性**             | **型**              | **説明**                                            |
| -------------------- | ------------------- | -------------------------------------------------- |
| `index`              | `int`               | ルート内のウェイポイントのインデックス                  |
| `reached`            | `bool`              | Nav2 が到達したかどうか                               |
| `time_s`             | `float` または `None` | ルート開始から到達までの秒数                           |
| `closest_approach_m` | `float` または `None` | 向かう途中でロボットがウェイポイントに最も近づいた距離     |
| `recoveries`         | `int`               | 向かう途中で Nav2 が実行したリカバリー動作の回数          |
| `min_clearance_m`    | `float` または `None` | 向かう途中での最も近いレーザー測定値                     |

### メソッド

* `result.metrics()`：走行の数値をフラットな `dict` で返します（`duration_s`、`waypoints_reached`、`waypoints_total`、`final_pos_error_m`、`final_yaw_error_rad`、`recoveries`、`min_clearance_m`）。Artefacts の `metrics.json` にそのまま書き出せます。
* `result.save(results_dir)`：結果を `<results_dir>/<route>_<YYYYmmdd-HHMMSS>[_<label>].yaml` として書き出し、そのパスを返します。
* `result.summary()`：走行結果の 1 行の説明。

### 例

```python
import json

result = follow_route(timeout_s=300)
assert result.succeeded, result.summary()

with open("output/metrics.json", "w") as f:
    json.dump(result.metrics(), f)
```