import { PanelExtension, MessageEvent } from "@avala-ai/panel-sdk";
interface Pose {
position: { x: number; y: number; z: number };
orientation: { x: number; y: number; z: number; w: number };
}
interface PoseStamped {
header: { stamp: { sec: number; nsec: number } };
pose: Pose;
}
export const panel: PanelExtension = {
id: "trajectory-2d",
name: "Trajectory Panel",
description: "Plot 2D robot paths from pose topics",
topics: [
{ topic: "/robot/pose", schemaName: "geometry_msgs/PoseStamped" },
],
settings: {
lineColor: { type: "color", label: "Path Color", default: "#00ff88" },
lineWidth: { type: "number", label: "Line Width", min: 1, max: 8, default: 2 },
showGrid: { type: "boolean", label: "Show Grid", default: true },
tailLength: {
type: "number",
label: "Trail Length (messages)",
min: 10,
max: 10000,
default: 1000,
},
},
onInit(context) {
const canvas = document.createElement("canvas");
canvas.style.width = "100%";
canvas.style.height = "100%";
context.panelElement.appendChild(canvas);
context.state.canvas = canvas;
context.state.ctx = canvas.getContext("2d");
context.state.poses = [];
},
onMessage(context, topic, message: MessageEvent) {
const msg = message.data as PoseStamped;
context.state.poses.push({
x: msg.pose.position.x,
y: msg.pose.position.y,
});
// Trim to tail length
const maxLen = context.settings.tailLength;
if (context.state.poses.length > maxLen) {
context.state.poses = context.state.poses.slice(-maxLen);
}
drawTrajectory(context);
},
onSeek(context, timestamp) {
// Clear accumulated poses on seek -- they will be
// re-delivered by the viewer for the new time range
context.state.poses = [];
},
onResize(context, width, height) {
context.state.canvas.width = width;
context.state.canvas.height = height;
drawTrajectory(context);
},
onSettingsChange(context, settings) {
drawTrajectory(context);
},
onDestroy(context) {
context.state.canvas.remove();
},
};
function drawTrajectory(context: any) {
const { ctx, canvas, poses } = context.state;
const { lineColor, lineWidth, showGrid } = context.settings;
ctx.clearRect(0, 0, canvas.width, canvas.height);
// Draw grid
if (showGrid) {
ctx.strokeStyle = "#333333";
ctx.lineWidth = 0.5;
for (let x = 0; x < canvas.width; x += 50) {
ctx.beginPath();
ctx.moveTo(x, 0);
ctx.lineTo(x, canvas.height);
ctx.stroke();
}
for (let y = 0; y < canvas.height; y += 50) {
ctx.beginPath();
ctx.moveTo(0, y);
ctx.lineTo(canvas.width, y);
ctx.stroke();
}
}
if (poses.length < 2) return;
// Compute bounds for auto-scaling
const xs = poses.map((p: any) => p.x);
const ys = poses.map((p: any) => p.y);
const minX = Math.min(...xs), maxX = Math.max(...xs);
const minY = Math.min(...ys), maxY = Math.max(...ys);
const rangeX = maxX - minX || 1;
const rangeY = maxY - minY || 1;
const padding = 40;
const scaleX = (canvas.width - 2 * padding) / rangeX;
const scaleY = (canvas.height - 2 * padding) / rangeY;
const scale = Math.min(scaleX, scaleY);
const toScreen = (p: any) => ({
x: padding + (p.x - minX) * scale,
y: canvas.height - padding - (p.y - minY) * scale,
});
// Draw path
ctx.strokeStyle = lineColor;
ctx.lineWidth = lineWidth;
ctx.beginPath();
const start = toScreen(poses[0]);
ctx.moveTo(start.x, start.y);
for (let i = 1; i < poses.length; i++) {
const pt = toScreen(poses[i]);
ctx.lineTo(pt.x, pt.y);
}
ctx.stroke();
// Draw current position marker
const current = toScreen(poses[poses.length - 1]);
ctx.fillStyle = lineColor;
ctx.beginPath();
ctx.arc(current.x, current.y, 6, 0, Math.PI * 2);
ctx.fill();
}
