Initial commit

2026-07-23 09:12:00 +09:00
commit 521d11ffa6
41 changed files with 8799 additions and 0 deletions
@@ -0,0 +1,260 @@
+import { Hono } from "hono";
+import type { AgentConfig } from "./config.ts";
+
+type ThreePose = {
+  x: number;
+  y: number;
+  z: number;
+  heading: number;
+};
+
+type MapCenter = {
+  x: number;
+  y: number;
+};
+
+type MapPose = {
+  x: number;
+  y: number;
+  z: number;
+  qx: number;
+  qy: number;
+  qz: number;
+  qw: number;
+};
+
+function bridgeUrl(config: AgentConfig): string {
+  return config.ros.bridgeUrl || `http://${config.ros.bridgeHost}:${config.ros.bridgePort}`;
+}
+
+function defaultCenter(): MapCenter {
+  return {
+    x: Number(process.env.CENTER_X ?? "3763.558837890625"),
+    y: Number(process.env.CENTER_Y ?? "73675.99609375"),
+  };
+}
+
+function validCenter(center?: MapCenter): MapCenter | undefined {
+  return center && Number.isFinite(center.x) && Number.isFinite(center.y) ? center : undefined;
+}
+
+function threeToMap(pose: ThreePose, center = defaultCenter()): MapPose {
+  const headingRad = (pose.heading * Math.PI) / 180;
+  const yaw = Math.PI / 2 - headingRad;
+  return {
+    x: pose.x + center.x,
+    y: center.y - pose.z,
+    z: pose.y,
+    qx: 0,
+    qy: 0,
+    qz: Math.sin(yaw / 2),
+    qw: Math.cos(yaw / 2),
+  };
+}
+
+function mapToThree(mx: number, my: number, mz: number, center = defaultCenter()) {
+  return {
+    x: mx - center.x,
+    y: mz,
+    z: -(my - center.y),
+  };
+}
+
+function sampleTrajectory(points: number[][], minDist = 0.5): number[][] {
+  if (points.length === 0) return [];
+  const result: number[][] = [points[0]];
+  let lx = points[0][0];
+  let ly = points[0][1];
+
+  for (const point of points.slice(1)) {
+    const dx = point[0] - lx;
+    const dy = point[1] - ly;
+    if (Math.sqrt(dx * dx + dy * dy) >= minDist) {
+      result.push(point.map((value, index) => index <= 3 ? Number(value.toFixed(3)) : value));
+      lx = point[0];
+      ly = point[1];
+    }
+  }
+  return result;
+}
+
+function trimBehindStart(points: [number, number, number, number][], start: ThreePose) {
+  const headingRad = (start.heading * Math.PI) / 180;
+  const fwdX = Math.sin(headingRad);
+  const fwdZ = -Math.cos(headingRad);
+
+  return points.filter((point) => {
+    const dx = point[0] - start.x;
+    const dz = point[2] - start.z;
+    return dx * fwdX + dz * fwdZ >= -0.5;
+  });
+}
+
+function convertPoints(points: { x: number; y: number; z: number; t: number }[], center?: MapCenter) {
+  return points.map((point) => {
+    const three = mapToThree(point.x, point.y, point.z, center);
+    return [three.x, three.y, three.z, point.t] as [number, number, number, number];
+  });
+}
+
+async function bridgeFetch(config: AgentConfig, path: string, init?: RequestInit): Promise<Response> {
+  return fetch(`${bridgeUrl(config)}${path}`, init);
+}
+
+async function bridgeJson<T>(config: AgentConfig, path: string, init?: RequestInit): Promise<T> {
+  const response = await bridgeFetch(config, path, init);
+  if (!response.ok) throw new Error(await response.text());
+  return response.json() as Promise<T>;
+}
+
+function errorJson(error: unknown, fallback: string, status = 502): Response {
+  return Response.json({
+    ok: false,
+    error: fallback,
+    detail: error instanceof Error ? error.message : String(error),
+  }, { status });
+}
+
+export function createBridgeApi(config: AgentConfig): Hono {
+  const app = new Hono();
+
+  app.get("/status", async (c) => {
+    try {
+      const [health, stack] = await Promise.all([
+        bridgeJson(config, "/health").catch((error) => ({ ok: false, error: String(error) })),
+        bridgeJson(config, "/stack").catch((error) => ({ ok: false, error: String(error) })),
+      ]);
+      return c.json({
+        ok: true,
+        configured: config.ros,
+        bridgeUrl: bridgeUrl(config),
+        health,
+        stack,
+      });
+    } catch (error) {
+      return c.json({ ok: false, bridgeUrl: bridgeUrl(config), message: error instanceof Error ? error.message : String(error) }, 502);
+    }
+  });
+
+  app.get("/stack", async (c) => {
+    try {
+      return c.json(await bridgeJson(config, "/stack"));
+    } catch (error) {
+      return errorJson(error, "Failed to read bridge stack.");
+    }
+  });
+
+  app.post("/clear", async (c) => {
+    try {
+      return c.json(await bridgeJson(config, "/clear_route", { method: "POST" }));
+    } catch (error) {
+      return errorJson(error, "Failed to clear route.");
+    }
+  });
+
+  app.post("/stop", async (c) => {
+    try {
+      return c.json(await bridgeJson(config, "/disengage", { method: "POST" }));
+    } catch (error) {
+      return errorJson(error, "Failed to stop robot.");
+    }
+  });
+
+  app.post("/reset", async (c) => {
+    try {
+      return c.json(await bridgeJson(config, "/reset", { method: "POST" }));
+    } catch (error) {
+      return errorJson(error, "Failed to reset robot.");
+    }
+  });
+
+  return app;
+}
+
+export function createViewerControlApi(config: AgentConfig): Hono {
+  const app = new Hono();
+
+  app.post("/plan", async (c) => {
+    try {
+      const body = await c.req.json<{ start?: ThreePose; goal?: ThreePose; center?: MapCenter }>();
+      if (!body.goal) return c.json({ error: "goal is required" }, 400);
+
+      const center = validCenter(body.center);
+      const goalMap = threeToMap(body.goal, center);
+      const startMap = body.start ? threeToMap(body.start, center) : null;
+
+      if (startMap) {
+        await bridgeJson(config, "/set_initial_pose", {
+          method: "POST",
+          headers: { "Content-Type": "application/json" },
+          body: JSON.stringify(startMap),
+        });
+        await Bun.sleep(800);
+      }
+
+      await bridgeJson(config, "/set_route", {
+        method: "POST",
+        headers: { "Content-Type": "application/json" },
+        body: JSON.stringify(goalMap),
+      });
+
+      const [pathData, trajData] = await Promise.all([
+        bridgeJson<{ points: { x: number; y: number; z: number; t: number }[] }>(config, "/get_behavior_path"),
+        bridgeJson<{ points: { x: number; y: number; z: number; t: number }[] }>(config, "/get_trajectory"),
+      ]);
+
+      const planned = sampleTrajectory(convertPoints(pathData.points, center), 0.5);
+      const rawTrajectoryPoints = convertPoints(trajData.points, center);
+      const visibleTrajectoryPoints = body.start ? trimBehindStart(rawTrajectoryPoints, body.start) : rawTrajectoryPoints;
+
+      return c.json({
+        planned,
+        plannedTrajectory: sampleTrajectory(visibleTrajectoryPoints, 0.5),
+      });
+    } catch (error) {
+      return errorJson(error, "Failed to plan.");
+    }
+  });
+
+  app.post("/drive", async (c) => {
+    try {
+      const body = await c.req.json<{ goal?: ThreePose }>();
+      if (!body.goal) return c.json({ error: "goal is required" }, 400);
+
+      await bridgeJson(config, "/engage", {
+        method: "POST",
+        headers: { "Content-Type": "application/json" },
+        body: JSON.stringify({ engage: true }),
+      });
+      return c.json({ ok: true });
+    } catch (error) {
+      return errorJson(error, "Failed to engage.");
+    }
+  });
+
+  app.post("/clear", async (c) => {
+    try {
+      return c.json(await bridgeJson(config, "/clear_route", { method: "POST" }));
+    } catch (error) {
+      return errorJson(error, "Failed to clear route.");
+    }
+  });
+
+  app.post("/stop", async (c) => {
+    try {
+      return c.json(await bridgeJson(config, "/disengage", { method: "POST" }));
+    } catch (error) {
+      return errorJson(error, "Failed to stop robot.");
+    }
+  });
+
+  app.post("/reset", async (c) => {
+    try {
+      return c.json(await bridgeJson(config, "/reset", { method: "POST" }));
+    } catch (error) {
+      return errorJson(error, "Failed to reset robot.");
+    }
+  });
+
+  return app;
+}