269 lines
7.6 KiB
TypeScript
269 lines
7.6 KiB
TypeScript
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("/emergency-stop", async (c) => {
|
|
try {
|
|
return c.json(await bridgeJson(config, "/emergency_stop", { method: "POST" }));
|
|
} catch (error) {
|
|
return errorJson(error, "Failed to emergency 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;
|
|
}
|