navigation-manager 0.3.1__tar.gz

navigation_manager-0.3.1/PKG-INFO

@@ -0,0 +1,39 @@
+Metadata-Version: 2.4
+Name: navigation-manager
+Version: 0.3.1
+Summary: Navigation manager for the JUIZ 2D navigation stack: goal -> plan -> follow -> arrive over HTTP, with a browser UI
+License: MIT OR Apache-2.0
+Project-URL: Homepage, https://github.com/OpenJUIZ/navigation_manager
+Requires-Python: >=3.9
+Description-Content-Type: text/markdown
+Requires-Dist: PyYAML>=5.1
+
+# navigation-manager
+
+Navigation manager for the [JUIZ](https://github.com/OpenJUIZ/juiz) 2D navigation stack
+([mobile_sim](https://github.com/OpenJUIZ/mobile_sim), [localization_amcl](https://github.com/OpenJUIZ/localization_amcl),
+[planner_dijkstra](https://github.com/OpenJUIZ/planner_dijkstra), [planner_dwa](https://github.com/OpenJUIZ/planner_dwa) and
+navigation_hub from [navigation_manager](https://github.com/OpenJUIZ/navigation_manager)).
+It runs the goal → plan → follow → arrive state machine over HTTP and serves a browser UI on http://localhost:8005/.
+
+## Start the whole stack
+
+Download the five `.juizapp` packages from the Releases of each repository, then:
+
+```
+pip install juiz-cli==0.2.4 navigation-manager
+navigation-manager stack mobile_sim-0.3.3.juizapp localization_amcl-0.1.0.juizapp \
+    planner_dijkstra-0.1.0.juizapp planner_dwa-0.1.0.juizapp navigation_hub-0.3.1.juizapp [--map map.yaml]
+```
+
+`navigation-manager stack` starts the apps in the order the stack needs (navigation_hub as the root of one tree),
+then the manager itself. `--map` distributes a map YAML (ROS map_server format, e.g. one made with SLAM) instead of the
+mobile_sim map. Ctrl-C stops everything. Works on Linux, macOS and Windows.
+
+To run only the manager against apps that are already running: `navigation-manager [--map map.yaml]`.
+
+See the [repository](https://github.com/OpenJUIZ/navigation_manager) for details (in Japanese).
+
+## License
+
+MIT OR Apache-2.0

navigation_manager-0.3.1/README.md

@@ -0,0 +1,29 @@
+# navigation-manager
+
+Navigation manager for the [JUIZ](https://github.com/OpenJUIZ/juiz) 2D navigation stack
+([mobile_sim](https://github.com/OpenJUIZ/mobile_sim), [localization_amcl](https://github.com/OpenJUIZ/localization_amcl),
+[planner_dijkstra](https://github.com/OpenJUIZ/planner_dijkstra), [planner_dwa](https://github.com/OpenJUIZ/planner_dwa) and
+navigation_hub from [navigation_manager](https://github.com/OpenJUIZ/navigation_manager)).
+It runs the goal → plan → follow → arrive state machine over HTTP and serves a browser UI on http://localhost:8005/.
+
+## Start the whole stack
+
+Download the five `.juizapp` packages from the Releases of each repository, then:
+
+```
+pip install juiz-cli==0.2.4 navigation-manager
+navigation-manager stack mobile_sim-0.3.3.juizapp localization_amcl-0.1.0.juizapp \
+    planner_dijkstra-0.1.0.juizapp planner_dwa-0.1.0.juizapp navigation_hub-0.3.1.juizapp [--map map.yaml]
+```
+
+`navigation-manager stack` starts the apps in the order the stack needs (navigation_hub as the root of one tree),
+then the manager itself. `--map` distributes a map YAML (ROS map_server format, e.g. one made with SLAM) instead of the
+mobile_sim map. Ctrl-C stops everything. Works on Linux, macOS and Windows.
+
+To run only the manager against apps that are already running: `navigation-manager [--map map.yaml]`.
+
+See the [repository](https://github.com/OpenJUIZ/navigation_manager) for details (in Japanese).
+
+## License
+
+MIT OR Apache-2.0

navigation_manager-0.3.1/navigation_manager/__main__.py

@@ -0,0 +1,97 @@
+"""navigation_manager を起動する。
+
+    navigation-manager [--port 8005] [--map map.yaml] [--sim URL] [--amcl URL] [--planner URL] [--dwa URL] [--hub URL]
+    navigation-manager stack <.juizapp>... [--map map.yaml]     # 一式をまとめて起動する(navigation-manager stack --help)
+
+先にナビゲーション一式(mobile_sim・localization_amcl・planner_dijkstra・planner_dwa・navigation_hub)を
+起動しておく(navigation-manager stack はそれも含めて全部起動する)。ブラウザで http://localhost:8005/ を開く。
+起動時に mobile_sim の地図を planner_dijkstra・localization_amcl に配る。--map で地図の YAML(SLAM で作ったものなど)を渡すと、
+mobile_sim の地図の代わりにそれを配り、画面にもその地図を描く。
+"""
+from __future__ import annotations
+
+import argparse
+import logging
+import sys
+import time
+
+from .apps import Apps, Urls, ready
+from .manager import Manager
+from .maps import MapError
+from .server import serve
+from .state import Params
+
+
+def main(argv=None) -> None:
+    argv = sys.argv[1:] if argv is None else list(argv)
+    if argv and argv[0] == "stack":
+        from .stack import main as stack_main
+        stack_main(argv[1:])
+        return
+    d = Urls()
+    p = Params()
+    ap = argparse.ArgumentParser(prog="navigation-manager", description=__doc__,
+                                 formatter_class=argparse.RawDescriptionHelpFormatter)
+    ap.add_argument("--host", default="0.0.0.0")
+    ap.add_argument("--port", type=int, default=8005)
+    ap.add_argument("--sim", default=d.sim, help="mobile_sim の URL")
+    ap.add_argument("--amcl", default=d.amcl, help="localization_amcl の URL")
+    ap.add_argument("--planner", default=d.planner, help="planner_dijkstra の URL")
+    ap.add_argument("--dwa", default=d.dwa, help="planner_dwa の URL")
+    ap.add_argument("--hub", default=None,
+                    help="navigation_hub の URL。渡すとそれも応答するまで待つ(Docker で一式と同時に起動するとき)")
+    ap.add_argument("--rate", type=float, default=5.0, help="周期処理の周波数 [Hz]")
+    ap.add_argument("--xy-goal-tolerance", type=float, default=p.xy_goal_tolerance)
+    ap.add_argument("--yaw-goal-tolerance", type=float, default=p.yaw_goal_tolerance)
+    ap.add_argument("--max-planning-retries", type=int, default=p.max_planning_retries)
+    ap.add_argument("--initial-pose-from-sim", action="store_true",
+                    help="起動時に mobile_sim の真の姿勢を localization_amcl の初期位置にする")
+    ap.add_argument("--map", metavar="YAML",
+                    help="起動時に配る地図の YAML(ROS の map_server の形。image は YAML から見た相対パス)。省くと mobile_sim の地図")
+    ap.add_argument("--no-distribute-map", action="store_true",
+                    help="起動時に mobile_sim の地図を planner_dijkstra・localization_amcl に渡さない(各アプリの地図のまま)")
+    ap.add_argument("--wait", type=float, default=30.0, help="各アプリが応答するまで待つ秒数")
+    ap.add_argument("-v", "--verbose", action="store_true")
+    a = ap.parse_args(argv)
+    if a.map and a.no_distribute_map:
+        ap.error("--map and --no-distribute-map cannot be used together")
+    logging.basicConfig(level=logging.DEBUG if a.verbose else logging.INFO,
+                        format="%(asctime)s %(levelname)s %(name)s: %(message)s")
+    log = logging.getLogger("navigation_manager")
+
+    urls = Urls(a.sim, a.amcl, a.planner, a.dwa)
+    end = time.time() + a.wait
+    waits = [(name, getattr(urls, name)) for name in ("sim", "amcl", "planner", "dwa")]
+    if a.hub:
+        waits.append(("hub", a.hub))
+    for name, url in waits:
+        while not ready(url):
+            if time.time() > end:
+                raise SystemExit(f"{name} is not running at {url}")
+            time.sleep(0.5)
+    manager = Manager(Apps(urls),
+                      Params(a.xy_goal_tolerance, a.yaw_goal_tolerance, a.max_planning_retries), a.rate)
+    if a.map:
+        try:
+            manager.load_map(a.map)
+        except MapError as e:
+            raise SystemExit(f"--map: {e}")
+    elif not a.no_distribute_map:
+        manager.distribute_map()
+    if a.initial_pose_from_sim:
+        p0 = manager.set_initial_pose()
+        log.info("initial pose from mobile_sim: %s", p0)
+    manager.start()
+    server = serve(manager, a.host, a.port)
+    log.info("navigation_manager on http://localhost:%d/", a.port)
+    try:
+        server.serve_forever()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        server.server_close()
+        manager.shutdown()
+
+
+if __name__ == "__main__":
+    main()

navigation_manager-0.3.1/navigation_manager/apps.py

@@ -0,0 +1,172 @@
+"""ナビゲーション一式の各アプリを juiz の HTTP API で呼ぶ。
+
+| アプリ | 既定の URL | 使う ContainerProcess |
+| --- | --- | --- |
+| mobile_sim | http://localhost:8000 | sim0: get_pose0・get_map0・get_map_info0・get_state0 |
+| localization_amcl | http://localhost:8002 | amcl0: get_pose0・set_initial_pose0・set_map0・get_state0(粒子) |
+| planner_dijkstra | http://localhost:8003 | planner0: set_goal0(計画して経路を返す)・set_map0 |
+| planner_dwa | http://localhost:8004 | dwa0: set_path0・cancel0・get_status0・get_state0(局所計画・障害物) |
+
+ContainerProcess の ID は `<URL>/container_process/<name>::<type_name>:<container>`。
+HTTP の call は戻り値を返すだけで、トピックには出さない(必要な受け渡しは manager が自分で呼ぶ)。
+"""
+from __future__ import annotations
+
+import json
+import time
+import uuid
+import urllib.parse
+import urllib.request
+from dataclasses import dataclass
+from typing import Any, Optional, Tuple
+
+from .state import Pose
+
+
+class AppError(Exception):
+    pass
+
+
+@dataclass
+class Urls:
+    sim: str = "http://localhost:8000"
+    amcl: str = "http://localhost:8002"
+    planner: str = "http://localhost:8003"
+    dwa: str = "http://localhost:8004"
+
+
+def _request(url: str, body: Any = None, method: str = "GET", timeout: float = 5.0) -> Tuple[str, bytes]:
+    data = None if body is None else json.dumps(body).encode()
+    req = urllib.request.Request(url, data=data, method=method, headers={"Content-Type": "application/json"})
+    try:
+        with urllib.request.urlopen(req, timeout=timeout) as r:
+            return r.headers.get("Content-Type", ""), r.read()
+    except urllib.error.HTTPError as e:
+        raise AppError(f"{url}: HTTP {e.code} {e.read().decode(errors='replace')[:300]}") from e
+    except OSError as e:
+        raise AppError(f"{url}: {e}") from e
+
+
+def call(base: str, container: str, name: str, type_name: str, args: Optional[dict] = None, raw: bool = False):
+    ident = f"{base}/container_process/{name}::{type_name}:{container}"
+    url = f"{base}/api/container_process/call?identifier=" + urllib.parse.quote(ident, safe="")
+    ctype, body = _request(url, args or {}, "PATCH")
+    if raw:
+        return ctype, body
+    return json.loads(body) if body else None
+
+
+def call_multipart(base: str, container: str, name: str, type_name: str, parts: dict):
+    """画像の引数を持つ Process を multipart の PUT で呼ぶ。bytes の値は image/png、ほかは JSON の部分にする"""
+    ident = f"{base}/container_process/{name}::{type_name}:{container}"
+    url = f"{base}/api/container_process/call?identifier=" + urllib.parse.quote(ident, safe="")
+    boundary = uuid.uuid4().hex
+    chunks = []
+    for k, v in parts.items():
+        data, ctype = (v, "image/png") if isinstance(v, bytes) else (json.dumps(v).encode(), "application/json")
+        chunks.append(f'--{boundary}\r\nContent-Disposition: form-data; name="{k}"; filename="{k}"\r\n'
+                      f"Content-Type: {ctype}\r\n\r\n".encode() + data + b"\r\n")
+    body = b"".join(chunks) + f"--{boundary}--\r\n".encode()
+    req = urllib.request.Request(url, data=body, method="PUT",
+                                 headers={"Content-Type": f"multipart/form-data; boundary={boundary}"})
+    try:
+        with urllib.request.urlopen(req, timeout=10.0) as r:
+            out = r.read()
+    except urllib.error.HTTPError as e:
+        raise AppError(f"{url}: HTTP {e.code} {e.read().decode(errors='replace')[:300]}") from e
+    except OSError as e:
+        raise AppError(f"{url}: {e}") from e
+    return json.loads(out) if out else None
+
+
+def map_parts(png: bytes, info: dict) -> dict:
+    """mobile_sim の get_map・get_map_info を、planner_set_map・amcl_set_map の引数にする(両方同じ形)"""
+    return {
+        "image": png,
+        "resolution": float(info["resolution"]),
+        "origin": list(info["origin"]),
+        "negate": bool(info.get("negate", False)),
+        "occupied_thresh": float(info.get("occupied_thresh", 0.65)),
+        "free_thresh": float(info.get("free_thresh", 0.196)),
+    }
+
+
+def ready(base: str) -> bool:
+    try:
+        _request(f"{base}/api/container/list", timeout=1.0)
+        return True
+    except AppError:
+        return False
+
+
+def pose_from_value(v: Any) -> Optional[Pose]:
+    """Pose2DStamped(`{header, pose: {position: {x, y}, theta}}`)・Pose2D・`{x, y, theta}` を読む"""
+    if not isinstance(v, dict):
+        return None
+    p = v.get("pose", v)
+    if isinstance(p, dict) and "position" in p:
+        return Pose(float(p["position"]["x"]), float(p["position"]["y"]), float(p.get("theta", 0.0)))
+    if isinstance(p, dict) and "x" in p and "y" in p:
+        return Pose(float(p["x"]), float(p["y"]), float(p.get("theta", 0.0)))
+    return None
+
+
+def pose_stamped(p: Pose, frame_id: str = "map") -> dict:
+    t = time.time()
+    return {
+        "header": {"stamp": {"sec": int(t), "nanosec": int((t - int(t)) * 1e9)}, "frame_id": frame_id},
+        "pose": {"position": {"x": p.x, "y": p.y}, "theta": p.theta},
+    }
+
+
+class Apps:
+    def __init__(self, urls: Urls):
+        self.urls = urls
+
+    # mobile_sim
+    def true_pose(self) -> Optional[Pose]:
+        return pose_from_value(call(self.urls.sim, "sim0", "get_pose0", "mobile_sim_get_pose", {"index": 0}))
+
+    def map_png(self) -> Tuple[str, bytes]:
+        return call(self.urls.sim, "sim0", "get_map0", "mobile_sim_get_map", raw=True)
+
+    def map_info(self) -> dict:
+        return call(self.urls.sim, "sim0", "get_map_info0", "mobile_sim_get_map_info")
+
+    def sim_state(self) -> dict:
+        return call(self.urls.sim, "sim0", "get_state0", "mobile_sim_get_state")
+
+    # localization_amcl
+    def estimated_pose(self) -> Optional[Pose]:
+        return pose_from_value(call(self.urls.amcl, "amcl0", "get_pose0", "amcl_get_pose"))
+
+    def set_initial_pose(self, p: Pose) -> None:
+        call(self.urls.amcl, "amcl0", "set_initial_pose0", "amcl_set_initial_pose",
+             {"pose": pose_stamped(p), "cov_xx": -1.0, "cov_yy": -1.0, "cov_aa": -1.0})
+
+    def set_amcl_map(self, parts: dict) -> None:
+        call_multipart(self.urls.amcl, "amcl0", "set_map0", "amcl_set_map", parts)
+
+    def amcl_state(self) -> dict:
+        return call(self.urls.amcl, "amcl0", "get_state0", "amcl_get_state")
+
+    # planner_dijkstra
+    def set_planner_map(self, parts: dict) -> None:
+        call_multipart(self.urls.planner, "planner0", "set_map0", "planner_set_map", parts)
+
+    def plan(self, goal: Pose) -> dict:
+        """ゴールを渡して計画させる。始点はトピック nav/amcl_pose で planner に届いている推定姿勢"""
+        return call(self.urls.planner, "planner0", "set_goal0", "planner_set_goal", {"goal": pose_stamped(goal)})
+
+    # planner_dwa
+    def follow(self, path: dict) -> None:
+        call(self.urls.dwa, "dwa0", "set_path0", "dwa_set_path", {"path": path})
+
+    def stop(self) -> None:
+        call(self.urls.dwa, "dwa0", "cancel0", "dwa_cancel")
+
+    def local_status(self) -> dict:
+        return call(self.urls.dwa, "dwa0", "get_status0", "dwa_get_status")
+
+    def local_state(self) -> dict:
+        return call(self.urls.dwa, "dwa0", "get_state0", "dwa_get_state")

navigation_manager-0.3.1/navigation_manager/e2e.py

@@ -0,0 +1,110 @@
+"""一式が動いているところに、ゴールを出して着くまでと、キャンセルで止まることを確かめる(navigation-manager stack --test、CI の e2e)。
+
+navigation_manager がゴールで SUCCEEDED になり、mobile_sim の真の姿勢がゴールの近くにあること、表示(粒子・局所計画・障害物)、
+キャンセルで止まることを見る。失敗したら E2EError を出す。
+"""
+import json
+import math
+import time
+import urllib.parse
+import urllib.request
+
+MANAGER = "http://127.0.0.1:8005"
+SIM = "http://127.0.0.1:8000"
+
+
+class E2EError(Exception):
+    pass
+
+
+def fail(message):
+    raise E2EError(message)
+
+
+def api(base, path, query=None, body=None, method="GET", timeout=5):
+    url = f"{base}/api/{path}"
+    if query:
+        url += "?" + urllib.parse.urlencode(query)
+    data = None if body is None else json.dumps(body).encode()
+    req = urllib.request.Request(url, data=data, method=method, headers={"Content-Type": "application/json"})
+    with urllib.request.urlopen(req, timeout=timeout) as r:
+        return json.loads(r.read())
+
+
+def call(base, name, ptype, container, args=None):
+    ident = f"{base}/container_process/{name}::{ptype}:{container}"
+    return api(base, "container_process/call", {"identifier": ident}, args or {}, "PATCH")
+
+
+def status():
+    return api(MANAGER, "status")
+
+
+def run_test(goal, timeout, map_source="mobile_sim"):
+    """ゴールを出して、navigation_manager が SUCCEEDED になり、mobile_sim の真の姿勢がゴールの近くにあることを確かめる"""
+    # 自己位置推定が落ち着くまで待つ(amcl_pose が navigation_manager に届くこと)
+    end = time.time() + 30
+    while status().get("pose") is None:
+        if time.time() > end:
+            fail("amcl_pose did not reach navigation_manager")
+        time.sleep(0.5)
+    print("amcl_pose reached navigation_manager:", status()["pose"], flush=True)
+    if status().get("map_source") != map_source:
+        fail(f"the {map_source} map was not distributed: {status()}")
+    info = api(MANAGER, "map_info")
+    print(f"map: {map_source} ({info['width']}x{info['height']}, {info['resolution']} m/px, origin {info['origin']})",
+          flush=True)
+    v = api(MANAGER, "view")
+    print(f"view: {len(v['particles'])} particles", flush=True)
+    if not v["particles"]:
+        fail("the view has no particles")
+
+    x, y, theta = goal
+    api(MANAGER, "goal", body={"x": x, "y": y, "theta": theta}, method="POST")
+    print(f"goal ({x}, {y}, {theta})", flush=True)
+    end = time.time() + timeout
+    last = None
+    seen = set()
+    while time.time() < end:
+        s = status()
+        seen.add(s["state"])
+        if (s["state"], s["message"]) != last:
+            print(f"  {s['state']}: {s['message']} (distance {s['goal_distance']})", flush=True)
+            last = (s["state"], s["message"])
+        if s["state"] in ("succeeded", "failed"):
+            break
+        time.sleep(0.5)
+    if s["state"] != "succeeded":
+        fail(f"navigation did not succeed: {s}")
+    if "following" not in seen:
+        fail(f"never followed a path: {seen}")
+    truth = call(SIM, "get_pose0", "mobile_sim_get_pose", "sim0", {"index": 0})["pose"]
+    d = math.hypot(truth["position"]["x"] - x, truth["position"]["y"] - y)
+    print(f"true pose {truth} ({d:.2f} m from the goal)", flush=True)
+    if d > 0.5:
+        fail(f"the robot is {d:.2f} m from the goal")
+
+    # 2 つ目のゴールを出して、追っている途中でキャンセルすると止まる
+    api(MANAGER, "goal", body={"x": -2.0, "y": -2.0, "theta": 0.0}, method="POST")
+    end = time.time() + 20
+    while status()["state"] != "following":
+        if time.time() > end:
+            fail(f"second goal was not followed: {status()}")
+        time.sleep(0.2)
+    time.sleep(2.0)
+    v = api(MANAGER, "view")
+    print(f"view while following: {len(v['local_plan'])} local plan points, {len(v['obstacles'])} obstacles", flush=True)
+    if not v["local_plan"] or not v["obstacles"]:
+        fail("the view has no local plan or obstacles while following")
+    s = api(MANAGER, "cancel", body={}, method="POST")
+    if s["state"] != "canceled":
+        fail(f"cancel did not cancel: {s}")
+    time.sleep(1.0)  # 減速を待つ
+    a = call(SIM, "get_pose0", "mobile_sim_get_pose", "sim0", {"index": 0})["pose"]["position"]
+    time.sleep(1.0)
+    b = call(SIM, "get_pose0", "mobile_sim_get_pose", "sim0", {"index": 0})["pose"]["position"]
+    moved = math.hypot(a["x"] - b["x"], a["y"] - b["y"])
+    print(f"canceled; moved {moved:.3f} m in 1 s after stopping", flush=True)
+    if moved > 0.05:
+        fail(f"the robot kept moving after cancel ({moved:.3f} m in 1 s)")
+    print("end-to-end test passed", flush=True)

navigation_manager-0.3.1/navigation_manager/manager.py

@@ -0,0 +1,195 @@
+"""状態機械を動かし、その Action に従って各アプリを HTTP で呼ぶ。
+
+- 周期処理(既定 5 Hz): localization_amcl の推定姿勢と planner_dwa の状態を読み、状態機械に渡す
+- ゴール: planner_dijkstra の set_goal を呼んで経路を受け、planner_dwa の set_path に渡す(引き直しも同じ)
+- キャンセル・到着・失敗: planner_dwa の cancel を呼んで止める
+- 初期位置: mobile_sim の真の姿勢(または指定の姿勢)を localization_amcl の set_initial_pose に渡す
+- 地図の配布: mobile_sim の地図(get_map・get_map_info)、または地図の YAML(SLAM で作ったものなど)を
+  planner_dijkstra と localization_amcl の set_map に渡す。配った地図を覚えておき、画面にはその地図を描く
+"""
+from __future__ import annotations
+
+import logging
+import threading
+import time
+from typing import List, Optional
+
+from .apps import AppError, Apps, map_parts
+from .maps import MapData, load_map_file, load_map_upload
+from .state import PLAN, STOP, Navigator, Params, Pose
+
+log = logging.getLogger("navigation_manager")
+
+
+class Manager:
+    def __init__(self, apps: Apps, params: Optional[Params] = None, rate: float = 5.0):
+        self.apps = apps
+        self.nav = Navigator(params or Params())
+        self.rate = rate
+        self.lock = threading.RLock()
+        self.path: Optional[dict] = None
+        self.local: Optional[dict] = None
+        self.last_error: Optional[str] = None
+        self.map: Optional[MapData] = None   # 配った地図。None なら各アプリの地図のまま(画面は mobile_sim の地図)
+        self.map_version = 0                  # 地図を配るたびに増やす(画面が描き直すため)
+        self._stop = threading.Event()
+        self._thread: Optional[threading.Thread] = None
+
+    # 外からのイベント(UI・API)
+    def set_goal(self, goal: Pose) -> dict:
+        with self.lock:
+            self._run(self.nav.set_goal(goal, time.time()))
+            return self.status()
+
+    def cancel(self) -> dict:
+        with self.lock:
+            self._run(self.nav.cancel(time.time()))
+            return self.status()
+
+    def set_initial_pose(self, pose: Optional[Pose] = None) -> Pose:
+        """pose を省くと mobile_sim の真の姿勢を使う"""
+        p = pose or self.apps.true_pose()
+        if p is None:
+            raise AppError("mobile_sim did not return a pose")
+        self.apps.set_initial_pose(p)
+        return p
+
+    def distribute_map(self) -> dict:
+        """mobile_sim の地図を planner_dijkstra と localization_amcl に渡す。追っている途中なら止める"""
+        png_type, png = self.apps.map_png()
+        info = self.apps.map_info()
+        return self._distribute(MapData(png, dict(info), "mobile_sim"))
+
+    def load_map(self, path) -> dict:
+        """地図の YAML(とそこに書かれた画像)を読んで、planner_dijkstra と localization_amcl に渡す"""
+        return self._distribute(load_map_file(path))
+
+    def load_map_upload(self, yaml_text: str, images: dict, name: Optional[str] = None) -> dict:
+        """画面から送られた地図の YAML と画像を読んで渡す"""
+        return self._distribute(load_map_upload(yaml_text, images, name))
+
+    def _distribute(self, m: MapData) -> dict:
+        parts = map_parts(m.png, m.info)
+        with self.lock:
+            if self.nav.state in ("planning", "following"):
+                self._run(self.nav.cancel(time.time()))
+            self.apps.set_planner_map(parts)
+            self.apps.set_amcl_map(parts)
+            self.map = m
+            self.map_version += 1
+            self.path = None
+        log.info("distributed the %s map (%sx%s, %s m/px) to planner_dijkstra and localization_amcl",
+                 m.source, m.info.get("width"), m.info.get("height"), m.info.get("resolution"))
+        return {"source": m.source, "info": m.info}
+
+    # 画面に描く地図: 配った地図、まだ配っていなければ mobile_sim の地図
+    def map_png(self):
+        m = self.map
+        if m is not None:
+            return "image/png", m.png
+        return self.apps.map_png()
+
+    def map_info(self) -> dict:
+        m = self.map
+        if m is not None:
+            return dict(m.info, source=m.source)
+        return self.apps.map_info()
+
+    # 周期処理
+    def step(self) -> None:
+        with self.lock:
+            now = time.time()
+            try:
+                pose = self.apps.estimated_pose()
+                if pose is not None:
+                    self._run(self.nav.on_pose(pose, now))
+                self.local = self.apps.local_status()
+                self._run(self.nav.on_local_status(str(self.local.get("status", "")), now))
+                self.last_error = None
+            except AppError as e:
+                self.last_error = str(e)
+                log.warning("%s", e)
+
+    def _run(self, actions: List[str]) -> None:
+        for a in actions:
+            if a == PLAN:
+                self._plan()
+            elif a == STOP:
+                try:
+                    self.apps.stop()
+                except AppError as e:
+                    self.last_error = str(e)
+                    log.warning("stop failed: %s", e)
+
+    def _plan(self) -> None:
+        goal = self.nav.goal
+        if goal is None:
+            return
+        try:
+            path = self.apps.plan(goal)
+        except AppError as e:
+            path = {"ok": False, "message": str(e), "poses": []}
+        self.path = path
+        ok = bool(path.get("ok"))
+        if ok:
+            try:
+                self.apps.follow(path)
+            except AppError as e:
+                ok = False
+                path = dict(path, message=f"planner_dwa refused the path: {e}")
+        # 失敗なら状態機械が引き直しか FAILED を決め、また PLAN を返すことがある
+        self._run(self.nav.on_plan_result(ok, str(path.get("message", "")), time.time()))
+
+    def status(self) -> dict:
+        with self.lock:
+            s = self.nav.status()
+            s["local"] = self.local
+            s["path_ok"] = None if self.path is None else bool(self.path.get("ok"))
+            s["error"] = self.last_error
+            s["map_source"] = None if self.map is None else self.map.source
+            s["map_version"] = self.map_version
+            return s
+
+    def view(self) -> dict:
+        """画面用: 状態、真の姿勢、推定姿勢、大域経路、局所計画(DWA の最良の軌道)、障害物(スキャン)、粒子"""
+        s = self.status()
+        try:
+            s["true_pose"] = _pose_dict(self.apps.true_pose())
+        except AppError:
+            s["true_pose"] = None
+        try:
+            a = self.apps.amcl_state()
+            s["particles"] = a.get("particles") or []
+            s["particle_count"] = a.get("particle_count")
+        except AppError:
+            s["particles"], s["particle_count"] = [], None
+        try:
+            d = self.apps.local_state()
+            best = d.get("best") or {}
+            s["local_plan"] = best.get("points") or []
+            s["obstacles"] = d.get("obstacles") or []
+        except AppError:
+            s["local_plan"], s["obstacles"] = [], []
+        with self.lock:
+            s["path"] = self.path
+        return s
+
+    def start(self) -> None:
+        def loop():
+            period = 1.0 / self.rate
+            while not self._stop.is_set():
+                t = time.time()
+                self.step()
+                self._stop.wait(max(0.0, period - (time.time() - t)))
+
+        self._thread = threading.Thread(target=loop, name="navigation_manager", daemon=True)
+        self._thread.start()
+
+    def shutdown(self) -> None:
+        self._stop.set()
+        if self._thread:
+            self._thread.join(2.0)
+
+
+def _pose_dict(p: Optional[Pose]) -> Optional[dict]:
+    return None if p is None else {"x": p.x, "y": p.y, "theta": p.theta}