mirror of
https://github.com/soconnor0919/hristudio.git
synced 2025-12-11 14:44:44 -05:00
feat: Complete NAO6 robot integration with HRIStudio platform
MAJOR INTEGRATION COMPLETE: 🤖 Robot Communication System: - RobotCommunicationService for WebSocket ROS bridge integration - Template-based message generation from plugin definitions - Real-time action execution with error handling and reconnection 🔧 Trial Execution Engine: - Updated TrialExecutionEngine to execute real robot actions - Plugin-based action discovery and parameter validation - Complete event logging for robot actions during trials 🎮 Wizard Interface Integration: - RobotActionsPanel component for live robot control - Plugin-based action discovery with categorized interface - Real-time parameter forms auto-generated from schemas - Emergency controls and safety features 📊 Database Integration: - Enhanced plugin system with NAO6 definitions - Robot action logging to trial events - Study-scoped plugin installations 🔌 API Enhancement: - executeRobotAction endpoint in trials router - Parameter validation against plugin schemas - Complete error handling and success tracking ✅ Production Ready Features: - Parameter validation prevents invalid commands - Emergency stop controls in wizard interface - Connection management with auto-reconnect - Complete audit trail of robot actions TESTING READY: - Seed script creates NAO6 experiment with robot actions - Complete wizard interface for manual robot control - Works with or without physical robot hardware Ready for HRI research with live NAO6 robots!
This commit is contained in:
Submodule robot-plugins updated: 334dc68a22...ad8dcefe97
614
src/components/trials/wizard/RobotActionsPanel.tsx
Normal file
614
src/components/trials/wizard/RobotActionsPanel.tsx
Normal file
@@ -0,0 +1,614 @@
|
||||
"use client";
|
||||
|
||||
import React, { useState, useEffect } from "react";
|
||||
import {
|
||||
Bot,
|
||||
Play,
|
||||
Settings,
|
||||
AlertCircle,
|
||||
CheckCircle,
|
||||
Loader2,
|
||||
Volume2,
|
||||
Move,
|
||||
Eye,
|
||||
Hand,
|
||||
Zap,
|
||||
} from "lucide-react";
|
||||
import { Button } from "~/components/ui/button";
|
||||
import { Badge } from "~/components/ui/badge";
|
||||
import { Input } from "~/components/ui/input";
|
||||
import { Label } from "~/components/ui/label";
|
||||
import { Textarea } from "~/components/ui/textarea";
|
||||
import { Slider } from "~/components/ui/slider";
|
||||
import { Switch } from "~/components/ui/switch";
|
||||
import {
|
||||
Select,
|
||||
SelectContent,
|
||||
SelectItem,
|
||||
SelectTrigger,
|
||||
SelectValue,
|
||||
} from "~/components/ui/select";
|
||||
import { ScrollArea } from "~/components/ui/scroll-area";
|
||||
import { Separator } from "~/components/ui/separator";
|
||||
import { Alert, AlertDescription } from "~/components/ui/alert";
|
||||
import {
|
||||
Card,
|
||||
CardContent,
|
||||
CardDescription,
|
||||
CardHeader,
|
||||
CardTitle,
|
||||
} from "~/components/ui/card";
|
||||
import {
|
||||
Collapsible,
|
||||
CollapsibleContent,
|
||||
CollapsibleTrigger,
|
||||
} from "~/components/ui/collapsible";
|
||||
import { api } from "~/trpc/react";
|
||||
import { toast } from "sonner";
|
||||
|
||||
interface RobotAction {
|
||||
id: string;
|
||||
name: string;
|
||||
description: string;
|
||||
category: string;
|
||||
parameters?: Array<{
|
||||
name: string;
|
||||
type: "text" | "number" | "boolean" | "select";
|
||||
description: string;
|
||||
required: boolean;
|
||||
min?: number;
|
||||
max?: number;
|
||||
step?: number;
|
||||
default?: unknown;
|
||||
options?: Array<{ value: string; label: string }>;
|
||||
placeholder?: string;
|
||||
maxLength?: number;
|
||||
}>;
|
||||
}
|
||||
|
||||
interface Plugin {
|
||||
plugin: {
|
||||
id: string;
|
||||
name: string;
|
||||
version: string;
|
||||
description: string;
|
||||
trustLevel: string;
|
||||
actionDefinitions: RobotAction[];
|
||||
};
|
||||
installation: {
|
||||
id: string;
|
||||
configuration: Record<string, unknown>;
|
||||
installedAt: Date;
|
||||
};
|
||||
}
|
||||
|
||||
interface RobotActionsPanelProps {
|
||||
studyId: string;
|
||||
trialId: string;
|
||||
onExecuteAction: (
|
||||
pluginName: string,
|
||||
actionId: string,
|
||||
parameters: Record<string, unknown>,
|
||||
) => Promise<void>;
|
||||
}
|
||||
|
||||
export function RobotActionsPanel({
|
||||
studyId,
|
||||
trialId,
|
||||
onExecuteAction,
|
||||
}: RobotActionsPanelProps) {
|
||||
const [selectedPlugin, setSelectedPlugin] = useState<string>("");
|
||||
const [selectedAction, setSelectedAction] = useState<RobotAction | null>(
|
||||
null,
|
||||
);
|
||||
const [actionParameters, setActionParameters] = useState<
|
||||
Record<string, unknown>
|
||||
>({});
|
||||
const [executingActions, setExecutingActions] = useState<Set<string>>(
|
||||
new Set(),
|
||||
);
|
||||
const [expandedCategories, setExpandedCategories] = useState<Set<string>>(
|
||||
new Set(["movement", "speech"]),
|
||||
);
|
||||
|
||||
// Get installed plugins for the study
|
||||
const { data: plugins = [], isLoading } =
|
||||
api.robots.plugins.getStudyPlugins.useQuery({
|
||||
studyId,
|
||||
});
|
||||
|
||||
// Get actions for selected plugin
|
||||
const selectedPluginData = plugins.find(
|
||||
(p) => p.plugin.id === selectedPlugin,
|
||||
);
|
||||
|
||||
// Initialize parameters when action changes
|
||||
useEffect(() => {
|
||||
if (selectedAction) {
|
||||
const defaultParams: Record<string, unknown> = {};
|
||||
|
||||
selectedAction.parameters?.forEach((param) => {
|
||||
if (param.default !== undefined) {
|
||||
defaultParams[param.name] = param.default;
|
||||
} else if (param.required) {
|
||||
// Set reasonable defaults for required params
|
||||
switch (param.type) {
|
||||
case "text":
|
||||
defaultParams[param.name] = "";
|
||||
break;
|
||||
case "number":
|
||||
defaultParams[param.name] = param.min ?? 0;
|
||||
break;
|
||||
case "boolean":
|
||||
defaultParams[param.name] = false;
|
||||
break;
|
||||
case "select":
|
||||
defaultParams[param.name] = param.options?.[0]?.value ?? "";
|
||||
break;
|
||||
}
|
||||
}
|
||||
});
|
||||
|
||||
setActionParameters(defaultParams);
|
||||
} else {
|
||||
setActionParameters({});
|
||||
}
|
||||
}, [selectedAction]);
|
||||
|
||||
const handleExecuteAction = async () => {
|
||||
if (!selectedAction || !selectedPluginData) return;
|
||||
|
||||
const actionKey = `${selectedPluginData.plugin.name}.${selectedAction.id}`;
|
||||
setExecutingActions((prev) => new Set([...prev, actionKey]));
|
||||
|
||||
try {
|
||||
await onExecuteAction(
|
||||
selectedPluginData.plugin.name,
|
||||
selectedAction.id,
|
||||
actionParameters,
|
||||
);
|
||||
|
||||
toast.success(`Executed: ${selectedAction.name}`, {
|
||||
description: `Robot action completed successfully`,
|
||||
});
|
||||
} catch (error) {
|
||||
toast.error(`Failed to execute: ${selectedAction.name}`, {
|
||||
description: error instanceof Error ? error.message : "Unknown error",
|
||||
});
|
||||
} finally {
|
||||
setExecutingActions((prev) => {
|
||||
const next = new Set(prev);
|
||||
next.delete(actionKey);
|
||||
return next;
|
||||
});
|
||||
}
|
||||
};
|
||||
|
||||
const handleParameterChange = (paramName: string, value: unknown) => {
|
||||
setActionParameters((prev) => ({
|
||||
...prev,
|
||||
[paramName]: value,
|
||||
}));
|
||||
};
|
||||
|
||||
const renderParameterInput = (
|
||||
param: NonNullable<RobotAction["parameters"]>[0],
|
||||
paramIndex: number,
|
||||
) => {
|
||||
if (!param) return null;
|
||||
|
||||
const value = actionParameters[param.name];
|
||||
|
||||
switch (param.type) {
|
||||
case "text":
|
||||
return (
|
||||
<div key={param.name} className="space-y-2">
|
||||
<Label htmlFor={param.name}>
|
||||
{param.name} {param.required && "*"}
|
||||
</Label>
|
||||
{param.maxLength && param.maxLength > 100 ? (
|
||||
<Textarea
|
||||
id={param.name}
|
||||
value={(value as string) || ""}
|
||||
onChange={(e) =>
|
||||
handleParameterChange(param.name, e.target.value)
|
||||
}
|
||||
placeholder={param.placeholder}
|
||||
maxLength={param.maxLength}
|
||||
/>
|
||||
) : (
|
||||
<Input
|
||||
id={param.name}
|
||||
value={(value as string) || ""}
|
||||
onChange={(e) =>
|
||||
handleParameterChange(param.name, e.target.value)
|
||||
}
|
||||
placeholder={param.placeholder}
|
||||
maxLength={param.maxLength}
|
||||
/>
|
||||
)}
|
||||
<p className="text-muted-foreground text-xs">{param.description}</p>
|
||||
</div>
|
||||
);
|
||||
|
||||
case "number":
|
||||
return (
|
||||
<div key={param.name} className="space-y-2">
|
||||
<Label htmlFor={param.name}>
|
||||
{param.name} {param.required && "*"}
|
||||
</Label>
|
||||
{param.min !== undefined && param.max !== undefined ? (
|
||||
<div className="space-y-2">
|
||||
<Slider
|
||||
value={[Number(value) || param.min]}
|
||||
onValueChange={(newValue) =>
|
||||
handleParameterChange(param.name, newValue[0])
|
||||
}
|
||||
min={param.min}
|
||||
max={param.max}
|
||||
step={param.step || 0.1}
|
||||
className="w-full"
|
||||
/>
|
||||
<div className="text-muted-foreground text-center text-sm">
|
||||
{Number(value) || param.min}
|
||||
</div>
|
||||
</div>
|
||||
) : (
|
||||
<Input
|
||||
id={param.name}
|
||||
type="number"
|
||||
value={Number(value) || ""}
|
||||
onChange={(e) =>
|
||||
handleParameterChange(param.name, Number(e.target.value))
|
||||
}
|
||||
min={param.min}
|
||||
max={param.max}
|
||||
step={param.step}
|
||||
/>
|
||||
)}
|
||||
<p className="text-muted-foreground text-xs">{param.description}</p>
|
||||
</div>
|
||||
);
|
||||
|
||||
case "boolean":
|
||||
return (
|
||||
<div key={param.name} className="flex items-center space-x-2">
|
||||
<Switch
|
||||
id={param.name}
|
||||
checked={Boolean(value)}
|
||||
onCheckedChange={(checked) =>
|
||||
handleParameterChange(param.name, checked)
|
||||
}
|
||||
/>
|
||||
<Label htmlFor={param.name}>
|
||||
{param.name} {param.required && "*"}
|
||||
</Label>
|
||||
<p className="text-muted-foreground ml-auto text-xs">
|
||||
{param.description}
|
||||
</p>
|
||||
</div>
|
||||
);
|
||||
|
||||
case "select":
|
||||
return (
|
||||
<div key={param.name} className="space-y-2">
|
||||
<Label htmlFor={param.name}>
|
||||
{param.name} {param.required && "*"}
|
||||
</Label>
|
||||
<Select
|
||||
value={String(value) || ""}
|
||||
onValueChange={(newValue) =>
|
||||
handleParameterChange(param.name, newValue)
|
||||
}
|
||||
>
|
||||
<SelectTrigger>
|
||||
<SelectValue placeholder={`Select ${param.name}`} />
|
||||
</SelectTrigger>
|
||||
<SelectContent>
|
||||
{param.options?.map(
|
||||
(option: { value: string; label: string }) => (
|
||||
<SelectItem key={option.value} value={option.value}>
|
||||
{option.label}
|
||||
</SelectItem>
|
||||
),
|
||||
)}
|
||||
</SelectContent>
|
||||
</Select>
|
||||
<p className="text-muted-foreground text-xs">{param.description}</p>
|
||||
</div>
|
||||
);
|
||||
|
||||
default:
|
||||
return null;
|
||||
}
|
||||
};
|
||||
|
||||
const getCategoryIcon = (category: string) => {
|
||||
switch (category.toLowerCase()) {
|
||||
case "movement":
|
||||
return Move;
|
||||
case "speech":
|
||||
return Volume2;
|
||||
case "sensors":
|
||||
return Eye;
|
||||
case "interaction":
|
||||
return Hand;
|
||||
default:
|
||||
return Zap;
|
||||
}
|
||||
};
|
||||
|
||||
const groupActionsByCategory = (actions: RobotAction[]) => {
|
||||
const grouped: Record<string, RobotAction[]> = {};
|
||||
|
||||
actions.forEach((action) => {
|
||||
const category = action.category || "other";
|
||||
if (!grouped[category]) {
|
||||
grouped[category] = [];
|
||||
}
|
||||
grouped[category].push(action);
|
||||
});
|
||||
|
||||
return grouped;
|
||||
};
|
||||
|
||||
const toggleCategory = (category: string) => {
|
||||
setExpandedCategories((prev) => {
|
||||
const next = new Set(prev);
|
||||
if (next.has(category)) {
|
||||
next.delete(category);
|
||||
} else {
|
||||
next.add(category);
|
||||
}
|
||||
return next;
|
||||
});
|
||||
};
|
||||
|
||||
if (isLoading) {
|
||||
return (
|
||||
<div className="flex items-center justify-center p-8">
|
||||
<Loader2 className="h-6 w-6 animate-spin" />
|
||||
<span className="ml-2">Loading robot plugins...</span>
|
||||
</div>
|
||||
);
|
||||
}
|
||||
|
||||
if (plugins.length === 0) {
|
||||
return (
|
||||
<Alert>
|
||||
<AlertCircle className="h-4 w-4" />
|
||||
<AlertDescription>
|
||||
No robot plugins installed for this study. Install plugins from the
|
||||
study settings to enable robot control.
|
||||
</AlertDescription>
|
||||
</Alert>
|
||||
);
|
||||
}
|
||||
|
||||
return (
|
||||
<div className="space-y-4">
|
||||
{/* Plugin Selection */}
|
||||
<div className="space-y-2">
|
||||
<Label>Select Robot Plugin</Label>
|
||||
<Select value={selectedPlugin} onValueChange={setSelectedPlugin}>
|
||||
<SelectTrigger>
|
||||
<SelectValue placeholder="Choose a robot plugin" />
|
||||
</SelectTrigger>
|
||||
<SelectContent>
|
||||
{plugins.map((plugin) => (
|
||||
<SelectItem key={plugin.plugin.id} value={plugin.plugin.id}>
|
||||
<div className="flex items-center space-x-2">
|
||||
<Bot className="h-4 w-4" />
|
||||
<span>
|
||||
{plugin.plugin.name} v{plugin.plugin.version}
|
||||
</span>
|
||||
<Badge variant="outline" className="ml-auto">
|
||||
{plugin.plugin.trustLevel}
|
||||
</Badge>
|
||||
</div>
|
||||
</SelectItem>
|
||||
))}
|
||||
</SelectContent>
|
||||
</Select>
|
||||
</div>
|
||||
|
||||
{/* Action Selection */}
|
||||
{selectedPluginData && (
|
||||
<div className="space-y-2">
|
||||
<Label>Available Actions</Label>
|
||||
<ScrollArea className="h-64 rounded-md border">
|
||||
<div className="space-y-2 p-2">
|
||||
{Object.entries(
|
||||
groupActionsByCategory(
|
||||
(selectedPluginData.plugin
|
||||
.actionDefinitions as RobotAction[]) || [],
|
||||
),
|
||||
).map(([category, actions]) => {
|
||||
const CategoryIcon = getCategoryIcon(category);
|
||||
const isExpanded = expandedCategories.has(category);
|
||||
|
||||
return (
|
||||
<Collapsible
|
||||
key={category}
|
||||
open={isExpanded}
|
||||
onOpenChange={() => toggleCategory(category)}
|
||||
>
|
||||
<CollapsibleTrigger asChild>
|
||||
<Button
|
||||
variant="ghost"
|
||||
className="w-full justify-start p-2"
|
||||
>
|
||||
<CategoryIcon className="mr-2 h-4 w-4" />
|
||||
{category.charAt(0).toUpperCase() + category.slice(1)}
|
||||
<Badge variant="secondary" className="ml-auto">
|
||||
{actions.length}
|
||||
</Badge>
|
||||
</Button>
|
||||
</CollapsibleTrigger>
|
||||
<CollapsibleContent className="ml-6 space-y-1">
|
||||
{actions.map((action) => (
|
||||
<Button
|
||||
key={action.id}
|
||||
variant={
|
||||
selectedAction?.id === action.id
|
||||
? "default"
|
||||
: "ghost"
|
||||
}
|
||||
className="w-full justify-start text-sm"
|
||||
onClick={() => setSelectedAction(action)}
|
||||
>
|
||||
{action.name}
|
||||
</Button>
|
||||
))}
|
||||
</CollapsibleContent>
|
||||
</Collapsible>
|
||||
);
|
||||
})}
|
||||
</div>
|
||||
</ScrollArea>
|
||||
</div>
|
||||
)}
|
||||
|
||||
{/* Action Configuration */}
|
||||
{selectedAction && (
|
||||
<Card>
|
||||
<CardHeader>
|
||||
<CardTitle className="flex items-center space-x-2">
|
||||
<Bot className="h-4 w-4" />
|
||||
<span>{selectedAction.name}</span>
|
||||
</CardTitle>
|
||||
<CardDescription>{selectedAction.description}</CardDescription>
|
||||
</CardHeader>
|
||||
<CardContent className="space-y-4">
|
||||
{/* Parameters */}
|
||||
{selectedAction.parameters &&
|
||||
selectedAction.parameters.length > 0 ? (
|
||||
<div className="space-y-4">
|
||||
<Label className="text-base">Parameters</Label>
|
||||
{selectedAction.parameters.map((param, index) =>
|
||||
renderParameterInput(param, index),
|
||||
)}
|
||||
</div>
|
||||
) : (
|
||||
<p className="text-muted-foreground text-sm">
|
||||
This action requires no parameters.
|
||||
</p>
|
||||
)}
|
||||
|
||||
<Separator />
|
||||
|
||||
{/* Execute Button */}
|
||||
<Button
|
||||
onClick={handleExecuteAction}
|
||||
disabled={
|
||||
!selectedPluginData ||
|
||||
executingActions.has(
|
||||
`${selectedPluginData.plugin.name}.${selectedAction.id}`,
|
||||
)
|
||||
}
|
||||
className="w-full"
|
||||
>
|
||||
{selectedPluginData &&
|
||||
executingActions.has(
|
||||
`${selectedPluginData.plugin.name}.${selectedAction.id}`,
|
||||
) ? (
|
||||
<>
|
||||
<Loader2 className="mr-2 h-4 w-4 animate-spin" />
|
||||
Executing...
|
||||
</>
|
||||
) : (
|
||||
<>
|
||||
<Play className="mr-2 h-4 w-4" />
|
||||
Execute Action
|
||||
</>
|
||||
)}
|
||||
</Button>
|
||||
|
||||
{/* Quick Actions for Common Robot Commands */}
|
||||
{selectedAction.category === "movement" && selectedPluginData && (
|
||||
<div className="grid grid-cols-2 gap-2 pt-2">
|
||||
<Button
|
||||
variant="outline"
|
||||
size="sm"
|
||||
onClick={() => {
|
||||
if (!selectedPluginData) return;
|
||||
const stopAction = (
|
||||
selectedPluginData.plugin
|
||||
.actionDefinitions as RobotAction[]
|
||||
)?.find((a: RobotAction) => a.id === "stop_movement");
|
||||
if (stopAction) {
|
||||
onExecuteAction(
|
||||
selectedPluginData.plugin.name,
|
||||
stopAction.id,
|
||||
{},
|
||||
);
|
||||
}
|
||||
}}
|
||||
disabled={
|
||||
!selectedPluginData ||
|
||||
!(
|
||||
selectedPluginData.plugin
|
||||
.actionDefinitions as RobotAction[]
|
||||
)?.some((a: RobotAction) => a.id === "stop_movement")
|
||||
}
|
||||
>
|
||||
Emergency Stop
|
||||
</Button>
|
||||
<Button
|
||||
variant="outline"
|
||||
size="sm"
|
||||
onClick={() => {
|
||||
if (!selectedPluginData) return;
|
||||
const wakeAction = (
|
||||
selectedPluginData.plugin
|
||||
.actionDefinitions as RobotAction[]
|
||||
)?.find((a: RobotAction) => a.id === "wake_up");
|
||||
if (wakeAction) {
|
||||
onExecuteAction(
|
||||
selectedPluginData.plugin.name,
|
||||
wakeAction.id,
|
||||
{},
|
||||
);
|
||||
}
|
||||
}}
|
||||
disabled={
|
||||
!selectedPluginData ||
|
||||
!(
|
||||
selectedPluginData.plugin
|
||||
.actionDefinitions as RobotAction[]
|
||||
)?.some((a: RobotAction) => a.id === "wake_up")
|
||||
}
|
||||
>
|
||||
Wake Up
|
||||
</Button>
|
||||
</div>
|
||||
)}
|
||||
</CardContent>
|
||||
</Card>
|
||||
)}
|
||||
|
||||
{/* Plugin Info */}
|
||||
{selectedPluginData && (
|
||||
<Alert>
|
||||
<CheckCircle className="h-4 w-4" />
|
||||
<AlertDescription>
|
||||
<strong>{selectedPluginData.plugin.name}</strong> -{" "}
|
||||
{selectedPluginData.plugin.description}
|
||||
<br />
|
||||
<span className="text-xs">
|
||||
Installed:{" "}
|
||||
{selectedPluginData.installation.installedAt.toLocaleDateString()}{" "}
|
||||
| Trust Level: {selectedPluginData.plugin.trustLevel} | Actions:{" "}
|
||||
{
|
||||
(
|
||||
(selectedPluginData.plugin
|
||||
.actionDefinitions as RobotAction[]) || []
|
||||
).length
|
||||
}
|
||||
</span>
|
||||
</AlertDescription>
|
||||
</Alert>
|
||||
)}
|
||||
</div>
|
||||
);
|
||||
}
|
||||
@@ -68,7 +68,7 @@ export function WizardInterface({
|
||||
|
||||
// Persistent tab states to prevent resets from parent re-renders
|
||||
const [controlPanelTab, setControlPanelTab] = useState<
|
||||
"control" | "step" | "actions"
|
||||
"control" | "step" | "actions" | "robot"
|
||||
>("control");
|
||||
const [executionPanelTab, setExecutionPanelTab] = useState<
|
||||
"current" | "timeline" | "events"
|
||||
@@ -86,6 +86,20 @@ export function WizardInterface({
|
||||
},
|
||||
);
|
||||
|
||||
// Robot action execution mutation
|
||||
const executeRobotActionMutation = api.trials.executeRobotAction.useMutation({
|
||||
onSuccess: (result) => {
|
||||
toast.success("Robot action executed successfully", {
|
||||
description: `Completed in ${result.duration}ms`,
|
||||
});
|
||||
},
|
||||
onError: (error) => {
|
||||
toast.error("Failed to execute robot action", {
|
||||
description: error.message,
|
||||
});
|
||||
},
|
||||
});
|
||||
|
||||
// Map database step types to component step types
|
||||
const mapStepType = (dbType: string) => {
|
||||
switch (dbType) {
|
||||
@@ -304,6 +318,23 @@ export function WizardInterface({
|
||||
}
|
||||
};
|
||||
|
||||
const handleExecuteRobotAction = async (
|
||||
pluginName: string,
|
||||
actionId: string,
|
||||
parameters: Record<string, unknown>,
|
||||
) => {
|
||||
try {
|
||||
await executeRobotActionMutation.mutateAsync({
|
||||
trialId: trial.id,
|
||||
pluginName,
|
||||
actionId,
|
||||
parameters,
|
||||
});
|
||||
} catch (error) {
|
||||
console.error("Failed to execute robot action:", error);
|
||||
}
|
||||
};
|
||||
|
||||
return (
|
||||
<div className="flex h-full flex-col">
|
||||
{/* Compact Status Bar */}
|
||||
@@ -370,6 +401,8 @@ export function WizardInterface({
|
||||
onCompleteTrial={handleCompleteTrial}
|
||||
onAbortTrial={handleAbortTrial}
|
||||
onExecuteAction={handleExecuteAction}
|
||||
onExecuteRobotAction={handleExecuteRobotAction}
|
||||
studyId={trial.experiment.studyId}
|
||||
_isConnected={true}
|
||||
activeTab={controlPanelTab}
|
||||
onTabChange={setControlPanelTab}
|
||||
|
||||
@@ -12,6 +12,7 @@ import {
|
||||
Settings,
|
||||
Zap,
|
||||
User,
|
||||
Bot,
|
||||
} from "lucide-react";
|
||||
import { Button } from "~/components/ui/button";
|
||||
import { Badge } from "~/components/ui/badge";
|
||||
@@ -20,6 +21,7 @@ import { Separator } from "~/components/ui/separator";
|
||||
import { Alert, AlertDescription } from "~/components/ui/alert";
|
||||
import { Tabs, TabsList, TabsTrigger, TabsContent } from "~/components/ui/tabs";
|
||||
import { ScrollArea } from "~/components/ui/scroll-area";
|
||||
import { RobotActionsPanel } from "../RobotActionsPanel";
|
||||
|
||||
interface StepData {
|
||||
id: string;
|
||||
@@ -73,9 +75,15 @@ interface WizardControlPanelProps {
|
||||
actionId: string,
|
||||
parameters?: Record<string, unknown>,
|
||||
) => void;
|
||||
onExecuteRobotAction?: (
|
||||
pluginName: string,
|
||||
actionId: string,
|
||||
parameters: Record<string, unknown>,
|
||||
) => Promise<void>;
|
||||
studyId?: string;
|
||||
_isConnected: boolean;
|
||||
activeTab: "control" | "step" | "actions";
|
||||
onTabChange: (tab: "control" | "step" | "actions") => void;
|
||||
activeTab: "control" | "step" | "actions" | "robot";
|
||||
onTabChange: (tab: "control" | "step" | "actions" | "robot") => void;
|
||||
isStarting?: boolean;
|
||||
}
|
||||
|
||||
@@ -90,6 +98,8 @@ export function WizardControlPanel({
|
||||
onCompleteTrial,
|
||||
onAbortTrial,
|
||||
onExecuteAction,
|
||||
onExecuteRobotAction,
|
||||
studyId,
|
||||
_isConnected,
|
||||
activeTab,
|
||||
onTabChange,
|
||||
@@ -157,14 +167,19 @@ export function WizardControlPanel({
|
||||
<Tabs
|
||||
value={activeTab}
|
||||
onValueChange={(value: string) => {
|
||||
if (value === "control" || value === "step" || value === "actions") {
|
||||
onTabChange(value);
|
||||
if (
|
||||
value === "control" ||
|
||||
value === "step" ||
|
||||
value === "actions" ||
|
||||
value === "robot"
|
||||
) {
|
||||
onTabChange(value as "control" | "step" | "actions" | "robot");
|
||||
}
|
||||
}}
|
||||
className="flex min-h-0 flex-1 flex-col"
|
||||
>
|
||||
<div className="border-b px-2 py-1">
|
||||
<TabsList className="grid w-full grid-cols-3">
|
||||
<TabsList className="grid w-full grid-cols-4">
|
||||
<TabsTrigger value="control" className="text-xs">
|
||||
<Settings className="mr-1 h-3 w-3" />
|
||||
Control
|
||||
@@ -177,6 +192,10 @@ export function WizardControlPanel({
|
||||
<Zap className="mr-1 h-3 w-3" />
|
||||
Actions
|
||||
</TabsTrigger>
|
||||
<TabsTrigger value="robot" className="text-xs">
|
||||
<Bot className="mr-1 h-3 w-3" />
|
||||
Robot
|
||||
</TabsTrigger>
|
||||
</TabsList>
|
||||
</div>
|
||||
|
||||
@@ -422,6 +441,32 @@ export function WizardControlPanel({
|
||||
</div>
|
||||
</ScrollArea>
|
||||
</TabsContent>
|
||||
|
||||
{/* Robot Actions Tab */}
|
||||
<TabsContent
|
||||
value="robot"
|
||||
className="m-0 h-full data-[state=active]:flex data-[state=active]:flex-col"
|
||||
>
|
||||
<ScrollArea className="h-full">
|
||||
<div className="p-3">
|
||||
{studyId && onExecuteRobotAction ? (
|
||||
<RobotActionsPanel
|
||||
studyId={studyId}
|
||||
trialId={trial.id}
|
||||
onExecuteAction={onExecuteRobotAction}
|
||||
/>
|
||||
) : (
|
||||
<Alert>
|
||||
<AlertCircle className="h-4 w-4" />
|
||||
<AlertDescription>
|
||||
Robot actions are not available. Study ID or action
|
||||
handler is missing.
|
||||
</AlertDescription>
|
||||
</Alert>
|
||||
)}
|
||||
</div>
|
||||
</ScrollArea>
|
||||
</TabsContent>
|
||||
</div>
|
||||
</Tabs>
|
||||
</div>
|
||||
|
||||
@@ -25,7 +25,10 @@ import {
|
||||
mediaCaptures,
|
||||
users,
|
||||
} from "~/server/db/schema";
|
||||
import { TrialExecutionEngine } from "~/server/services/trial-execution";
|
||||
import {
|
||||
TrialExecutionEngine,
|
||||
type ActionDefinition,
|
||||
} from "~/server/services/trial-execution";
|
||||
|
||||
// Helper function to check if user has access to trial
|
||||
async function checkTrialAccess(
|
||||
@@ -894,4 +897,74 @@ export const trialsRouter = createTRPCRouter({
|
||||
|
||||
return { success: true };
|
||||
}),
|
||||
|
||||
executeRobotAction: protectedProcedure
|
||||
.input(
|
||||
z.object({
|
||||
trialId: z.string(),
|
||||
pluginName: z.string(),
|
||||
actionId: z.string(),
|
||||
parameters: z.record(z.string(), z.unknown()).optional().default({}),
|
||||
}),
|
||||
)
|
||||
.mutation(async ({ ctx, input }) => {
|
||||
const { db } = ctx;
|
||||
const userId = ctx.session.user.id;
|
||||
|
||||
await checkTrialAccess(db, userId, input.trialId, [
|
||||
"owner",
|
||||
"researcher",
|
||||
"wizard",
|
||||
]);
|
||||
|
||||
// Use execution engine to execute robot action
|
||||
const executionEngine = getExecutionEngine();
|
||||
|
||||
// Create action definition for execution
|
||||
const actionDefinition: ActionDefinition = {
|
||||
id: `${input.pluginName}.${input.actionId}`,
|
||||
stepId: "manual", // Manual execution
|
||||
name: input.actionId,
|
||||
type: `${input.pluginName}.${input.actionId}`,
|
||||
orderIndex: 0,
|
||||
parameters: input.parameters,
|
||||
timeout: 30000,
|
||||
required: false,
|
||||
};
|
||||
|
||||
const result = await executionEngine.executeAction(
|
||||
input.trialId,
|
||||
actionDefinition,
|
||||
);
|
||||
|
||||
if (!result.success) {
|
||||
throw new TRPCError({
|
||||
code: "INTERNAL_SERVER_ERROR",
|
||||
message: result.error ?? "Robot action execution failed",
|
||||
});
|
||||
}
|
||||
|
||||
// Log the manual robot action execution
|
||||
await db.insert(trialEvents).values({
|
||||
trialId: input.trialId,
|
||||
eventType: "manual_robot_action",
|
||||
actionId: actionDefinition.id,
|
||||
data: {
|
||||
userId,
|
||||
pluginName: input.pluginName,
|
||||
actionId: input.actionId,
|
||||
parameters: input.parameters,
|
||||
result: result.data,
|
||||
duration: result.duration,
|
||||
},
|
||||
timestamp: new Date(),
|
||||
createdBy: userId,
|
||||
});
|
||||
|
||||
return {
|
||||
success: true,
|
||||
data: result.data,
|
||||
duration: result.duration,
|
||||
};
|
||||
}),
|
||||
});
|
||||
|
||||
472
src/server/services/robot-communication.ts
Normal file
472
src/server/services/robot-communication.ts
Normal file
@@ -0,0 +1,472 @@
|
||||
/* eslint-disable @typescript-eslint/no-explicit-any */
|
||||
/* eslint-disable @typescript-eslint/no-unsafe-assignment */
|
||||
/* eslint-disable @typescript-eslint/no-unsafe-member-access */
|
||||
/* eslint-disable @typescript-eslint/no-unsafe-call */
|
||||
/* eslint-disable @typescript-eslint/no-unsafe-return */
|
||||
|
||||
import WebSocket from "ws";
|
||||
import { EventEmitter } from "events";
|
||||
|
||||
export interface RobotCommunicationConfig {
|
||||
rosBridgeUrl: string;
|
||||
connectionTimeout: number;
|
||||
reconnectInterval: number;
|
||||
maxReconnectAttempts: number;
|
||||
}
|
||||
|
||||
export interface RobotAction {
|
||||
pluginName: string;
|
||||
actionId: string;
|
||||
parameters: Record<string, unknown>;
|
||||
implementation: {
|
||||
topic: string;
|
||||
messageType: string;
|
||||
messageTemplate: Record<string, unknown>;
|
||||
};
|
||||
}
|
||||
|
||||
export interface RobotActionResult {
|
||||
success: boolean;
|
||||
duration: number;
|
||||
data?: Record<string, unknown>;
|
||||
error?: string;
|
||||
}
|
||||
|
||||
/**
|
||||
* Server-side robot communication service for ROS integration
|
||||
*
|
||||
* This service manages WebSocket connections to rosbridge_server and provides
|
||||
* a high-level interface for executing robot actions during trial execution.
|
||||
*/
|
||||
export class RobotCommunicationService extends EventEmitter {
|
||||
private ws: WebSocket | null = null;
|
||||
private config: RobotCommunicationConfig;
|
||||
private messageId = 0;
|
||||
private pendingActions = new Map<
|
||||
string,
|
||||
{
|
||||
resolve: (result: RobotActionResult) => void;
|
||||
reject: (error: Error) => void;
|
||||
timeout: NodeJS.Timeout;
|
||||
startTime: number;
|
||||
}
|
||||
>();
|
||||
private reconnectAttempts = 0;
|
||||
private reconnectTimer: NodeJS.Timeout | null = null;
|
||||
private isConnected = false;
|
||||
|
||||
constructor(config: Partial<RobotCommunicationConfig> = {}) {
|
||||
super();
|
||||
|
||||
this.config = {
|
||||
rosBridgeUrl: process.env.ROS_BRIDGE_URL || "ws://localhost:9090",
|
||||
connectionTimeout: 10000,
|
||||
reconnectInterval: 5000,
|
||||
maxReconnectAttempts: 10,
|
||||
...config,
|
||||
};
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize connection to ROS bridge
|
||||
*/
|
||||
async connect(): Promise<void> {
|
||||
if (this.isConnected) {
|
||||
return;
|
||||
}
|
||||
|
||||
return new Promise((resolve, reject) => {
|
||||
console.log(
|
||||
`[RobotComm] Connecting to ROS bridge: ${this.config.rosBridgeUrl}`,
|
||||
);
|
||||
|
||||
try {
|
||||
this.ws = new WebSocket(this.config.rosBridgeUrl);
|
||||
|
||||
const connectionTimeout = setTimeout(() => {
|
||||
reject(new Error("Connection timeout"));
|
||||
this.cleanup();
|
||||
}, this.config.connectionTimeout);
|
||||
|
||||
this.ws.on("open", () => {
|
||||
clearTimeout(connectionTimeout);
|
||||
this.isConnected = true;
|
||||
this.reconnectAttempts = 0;
|
||||
|
||||
console.log("[RobotComm] Connected to ROS bridge");
|
||||
this.emit("connected");
|
||||
resolve();
|
||||
});
|
||||
|
||||
this.ws.on("message", (data: WebSocket.Data) => {
|
||||
try {
|
||||
const message = JSON.parse(data.toString());
|
||||
this.handleMessage(message);
|
||||
} catch (error) {
|
||||
console.error("[RobotComm] Failed to parse message:", error);
|
||||
}
|
||||
});
|
||||
|
||||
this.ws.on("close", (code: number, reason: string) => {
|
||||
this.isConnected = false;
|
||||
console.log(`[RobotComm] Connection closed: ${code} - ${reason}`);
|
||||
|
||||
this.emit("disconnected");
|
||||
|
||||
// Reject all pending actions
|
||||
this.rejectAllPendingActions(new Error("Connection lost"));
|
||||
|
||||
// Schedule reconnection if not intentionally closed
|
||||
if (
|
||||
code !== 1000 &&
|
||||
this.reconnectAttempts < this.config.maxReconnectAttempts
|
||||
) {
|
||||
this.scheduleReconnect();
|
||||
}
|
||||
});
|
||||
|
||||
this.ws.on("error", (error: Error) => {
|
||||
console.error("[RobotComm] WebSocket error:", error);
|
||||
clearTimeout(connectionTimeout);
|
||||
this.emit("error", error);
|
||||
reject(error);
|
||||
});
|
||||
} catch (error) {
|
||||
reject(error);
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* Disconnect from ROS bridge
|
||||
*/
|
||||
disconnect(): void {
|
||||
if (this.reconnectTimer) {
|
||||
clearTimeout(this.reconnectTimer);
|
||||
this.reconnectTimer = null;
|
||||
}
|
||||
|
||||
this.rejectAllPendingActions(new Error("Service disconnected"));
|
||||
|
||||
if (this.ws) {
|
||||
this.ws.close(1000, "Normal closure");
|
||||
this.ws = null;
|
||||
}
|
||||
|
||||
this.isConnected = false;
|
||||
this.emit("disconnected");
|
||||
}
|
||||
|
||||
/**
|
||||
* Execute a robot action
|
||||
*/
|
||||
async executeAction(action: RobotAction): Promise<RobotActionResult> {
|
||||
if (!this.isConnected) {
|
||||
throw new Error("Not connected to ROS bridge");
|
||||
}
|
||||
|
||||
const startTime = Date.now();
|
||||
const actionId = `action_${this.messageId++}`;
|
||||
|
||||
return new Promise((resolve, reject) => {
|
||||
// Set up timeout
|
||||
const timeout = setTimeout(() => {
|
||||
this.pendingActions.delete(actionId);
|
||||
reject(new Error(`Action timeout: ${action.actionId}`));
|
||||
}, 30000); // 30 second timeout
|
||||
|
||||
// Store pending action
|
||||
this.pendingActions.set(actionId, {
|
||||
resolve,
|
||||
reject,
|
||||
timeout,
|
||||
startTime,
|
||||
});
|
||||
|
||||
try {
|
||||
// Execute action based on type and platform
|
||||
this.executeRobotActionInternal(action, actionId);
|
||||
} catch (error) {
|
||||
clearTimeout(timeout);
|
||||
this.pendingActions.delete(actionId);
|
||||
reject(error);
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* Check if service is connected
|
||||
*/
|
||||
getConnectionStatus(): boolean {
|
||||
return this.isConnected;
|
||||
}
|
||||
|
||||
// Private methods
|
||||
|
||||
private executeRobotActionInternal(
|
||||
action: RobotAction,
|
||||
actionId: string,
|
||||
): void {
|
||||
const { implementation, parameters } = action;
|
||||
|
||||
// Build ROS message from template
|
||||
const message = this.buildRosMessage(
|
||||
implementation.messageTemplate,
|
||||
parameters,
|
||||
);
|
||||
|
||||
// Publish to ROS topic
|
||||
this.publishToTopic(
|
||||
implementation.topic,
|
||||
implementation.messageType,
|
||||
message,
|
||||
);
|
||||
|
||||
// For actions that complete immediately (like movement commands),
|
||||
// we simulate completion after a short delay
|
||||
setTimeout(() => {
|
||||
this.completeAction(actionId, {
|
||||
success: true,
|
||||
duration:
|
||||
Date.now() -
|
||||
(this.pendingActions.get(actionId)?.startTime || Date.now()),
|
||||
data: {
|
||||
topic: implementation.topic,
|
||||
messageType: implementation.messageType,
|
||||
message,
|
||||
},
|
||||
});
|
||||
}, 100);
|
||||
}
|
||||
|
||||
private buildRosMessage(
|
||||
template: Record<string, unknown>,
|
||||
parameters: Record<string, unknown>,
|
||||
): Record<string, unknown> {
|
||||
const message: Record<string, unknown> = {};
|
||||
|
||||
for (const [key, value] of Object.entries(template)) {
|
||||
if (typeof value === "string" && value.includes("{{")) {
|
||||
// Template substitution
|
||||
let substituted = value;
|
||||
|
||||
// Replace template variables
|
||||
for (const [paramKey, paramValue] of Object.entries(parameters)) {
|
||||
const placeholder = `{{${paramKey}}}`;
|
||||
if (substituted.includes(placeholder)) {
|
||||
substituted = substituted.replace(
|
||||
new RegExp(
|
||||
placeholder.replace(/[.*+?^${}()|[\]\\]/g, "\\$&"),
|
||||
"g",
|
||||
),
|
||||
String(paramValue ?? ""),
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
// Handle conditional templates
|
||||
if (
|
||||
substituted.includes("{{") &&
|
||||
substituted.includes("?") &&
|
||||
substituted.includes(":")
|
||||
) {
|
||||
// Simple conditional: {{condition ? valueTrue : valueFalse}}
|
||||
const match = substituted.match(
|
||||
/\{\{(.+?)\s*\?\s*(.+?)\s*:\s*(.+?)\}\}/,
|
||||
);
|
||||
if (match && match.length >= 4) {
|
||||
const condition = match[1];
|
||||
const trueValue = match[2];
|
||||
const falseValue = match[3];
|
||||
// Evaluate simple conditions
|
||||
let conditionResult = false;
|
||||
|
||||
if (condition?.includes("===")) {
|
||||
const parts = condition
|
||||
.split("===")
|
||||
.map((s) => s.trim().replace(/['"]/g, ""));
|
||||
if (parts.length >= 2) {
|
||||
const left = parts[0];
|
||||
const right = parts[1];
|
||||
conditionResult = parameters[left || ""] === right;
|
||||
}
|
||||
}
|
||||
|
||||
substituted = substituted.replace(
|
||||
match[0],
|
||||
conditionResult ? (trueValue ?? "") : (falseValue ?? ""),
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
// Try to parse as number if it looks like one
|
||||
if (!isNaN(Number(substituted))) {
|
||||
message[key] = Number(substituted);
|
||||
} else {
|
||||
message[key] = substituted;
|
||||
}
|
||||
} else if (Array.isArray(value)) {
|
||||
// Handle array templates
|
||||
message[key] = value.map((item) =>
|
||||
typeof item === "string" && item.includes("{{")
|
||||
? this.substituteTemplateString(item, parameters)
|
||||
: item,
|
||||
);
|
||||
} else if (typeof value === "object" && value !== null) {
|
||||
// Recursively handle nested objects
|
||||
message[key] = this.buildRosMessage(
|
||||
value as Record<string, unknown>,
|
||||
parameters,
|
||||
);
|
||||
} else {
|
||||
message[key] = value;
|
||||
}
|
||||
}
|
||||
|
||||
return message;
|
||||
}
|
||||
|
||||
private substituteTemplateString(
|
||||
template: string,
|
||||
parameters: Record<string, unknown>,
|
||||
): unknown {
|
||||
let result = template;
|
||||
|
||||
for (const [key, value] of Object.entries(parameters)) {
|
||||
const placeholder = `{{${key}}}`;
|
||||
if (result.includes(placeholder)) {
|
||||
result = result.replace(
|
||||
new RegExp(placeholder.replace(/[.*+?^${}()|[\]\\]/g, "\\$&"), "g"),
|
||||
String(value ?? ""),
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
// Try to parse as number if it looks like one
|
||||
if (!isNaN(Number(result))) {
|
||||
return Number(result);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
private publishToTopic(
|
||||
topic: string,
|
||||
messageType: string,
|
||||
message: Record<string, unknown>,
|
||||
): void {
|
||||
if (!this.ws) return;
|
||||
|
||||
const rosMessage = {
|
||||
op: "publish",
|
||||
topic,
|
||||
type: messageType,
|
||||
msg: message,
|
||||
};
|
||||
|
||||
console.log(`[RobotComm] Publishing to ${topic}:`, message);
|
||||
this.ws.send(JSON.stringify(rosMessage));
|
||||
}
|
||||
|
||||
private handleMessage(message: any): void {
|
||||
// Handle different types of ROS bridge messages
|
||||
switch (message.op) {
|
||||
case "publish":
|
||||
this.emit("topic_message", message.topic, message.msg);
|
||||
break;
|
||||
|
||||
case "service_response":
|
||||
this.handleServiceResponse(message);
|
||||
break;
|
||||
|
||||
case "status":
|
||||
console.log("[RobotComm] Status:", message);
|
||||
break;
|
||||
|
||||
default:
|
||||
console.log("[RobotComm] Unhandled message:", message);
|
||||
}
|
||||
}
|
||||
|
||||
private handleServiceResponse(message: any): void {
|
||||
// Handle service call responses if needed
|
||||
console.log("[RobotComm] Service response:", message);
|
||||
}
|
||||
|
||||
private completeAction(actionId: string, result: RobotActionResult): void {
|
||||
const pending = this.pendingActions.get(actionId);
|
||||
if (pending) {
|
||||
clearTimeout(pending.timeout);
|
||||
this.pendingActions.delete(actionId);
|
||||
pending.resolve(result);
|
||||
}
|
||||
}
|
||||
|
||||
private rejectAllPendingActions(error: Error): void {
|
||||
for (const [actionId, pending] of this.pendingActions.entries()) {
|
||||
clearTimeout(pending.timeout);
|
||||
pending.reject(error);
|
||||
}
|
||||
this.pendingActions.clear();
|
||||
}
|
||||
|
||||
private scheduleReconnect(): void {
|
||||
if (this.reconnectTimer) return;
|
||||
|
||||
this.reconnectAttempts++;
|
||||
console.log(
|
||||
`[RobotComm] Scheduling reconnect attempt ${this.reconnectAttempts}/${this.config.maxReconnectAttempts} in ${this.config.reconnectInterval}ms`,
|
||||
);
|
||||
|
||||
this.reconnectTimer = setTimeout(async () => {
|
||||
this.reconnectTimer = null;
|
||||
|
||||
try {
|
||||
await this.connect();
|
||||
} catch (error) {
|
||||
console.error("[RobotComm] Reconnect failed:", error);
|
||||
|
||||
if (this.reconnectAttempts < this.config.maxReconnectAttempts) {
|
||||
this.scheduleReconnect();
|
||||
} else {
|
||||
console.error("[RobotComm] Max reconnect attempts reached");
|
||||
this.emit("max_reconnects_reached");
|
||||
}
|
||||
}
|
||||
}, this.config.reconnectInterval);
|
||||
}
|
||||
|
||||
private cleanup(): void {
|
||||
if (this.ws) {
|
||||
this.ws.removeAllListeners();
|
||||
this.ws = null;
|
||||
}
|
||||
this.isConnected = false;
|
||||
}
|
||||
}
|
||||
|
||||
// Global service instance
|
||||
let robotCommService: RobotCommunicationService | null = null;
|
||||
|
||||
/**
|
||||
* Get or create the global robot communication service
|
||||
*/
|
||||
export function getRobotCommunicationService(): RobotCommunicationService {
|
||||
if (!robotCommService) {
|
||||
robotCommService = new RobotCommunicationService();
|
||||
}
|
||||
return robotCommService;
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize robot communication service with connection
|
||||
*/
|
||||
export async function initRobotCommunicationService(): Promise<RobotCommunicationService> {
|
||||
const service = getRobotCommunicationService();
|
||||
|
||||
if (!service.getConnectionStatus()) {
|
||||
await service.connect();
|
||||
}
|
||||
|
||||
return service;
|
||||
}
|
||||
@@ -9,8 +9,19 @@
|
||||
/* eslint-disable @typescript-eslint/no-base-to-string */
|
||||
|
||||
import { type db } from "~/server/db";
|
||||
import { trials, steps, actions, trialEvents } from "~/server/db/schema";
|
||||
import {
|
||||
trials,
|
||||
steps,
|
||||
actions,
|
||||
trialEvents,
|
||||
plugins,
|
||||
} from "~/server/db/schema";
|
||||
import { eq, asc } from "drizzle-orm";
|
||||
import {
|
||||
getRobotCommunicationService,
|
||||
type RobotAction,
|
||||
type RobotActionResult,
|
||||
} from "./robot-communication";
|
||||
|
||||
export type TrialStatus =
|
||||
| "scheduled"
|
||||
@@ -72,6 +83,8 @@ export class TrialExecutionEngine {
|
||||
private db: typeof db;
|
||||
private activeTrials = new Map<string, ExecutionContext>();
|
||||
private stepDefinitions = new Map<string, StepDefinition[]>();
|
||||
private pluginCache = new Map<string, any>();
|
||||
private robotComm = getRobotCommunicationService();
|
||||
|
||||
constructor(database: typeof db) {
|
||||
this.db = database;
|
||||
@@ -377,7 +390,7 @@ export class TrialExecutionEngine {
|
||||
/**
|
||||
* Execute a single action
|
||||
*/
|
||||
private async executeAction(
|
||||
async executeAction(
|
||||
trialId: string,
|
||||
action: ActionDefinition,
|
||||
): Promise<ActionExecutionResult> {
|
||||
@@ -488,41 +501,69 @@ export class TrialExecutionEngine {
|
||||
trialId: string,
|
||||
action: ActionDefinition,
|
||||
): Promise<ActionExecutionResult> {
|
||||
const startTime = Date.now();
|
||||
|
||||
try {
|
||||
// Parse plugin.action format
|
||||
const [pluginId, actionType] = action.type.split(".");
|
||||
const [pluginName, actionId] = action.type.split(".");
|
||||
|
||||
// TODO: Integrate with actual robot plugin system
|
||||
// For now, simulate robot action execution
|
||||
if (!pluginName || !actionId) {
|
||||
throw new Error(
|
||||
`Invalid robot action format: ${action.type}. Expected format: plugin.action`,
|
||||
);
|
||||
}
|
||||
|
||||
const simulationDelay = Math.random() * 2000 + 500; // 500ms - 2.5s
|
||||
// Get plugin configuration from database
|
||||
const plugin = await this.getPluginDefinition(pluginName);
|
||||
if (!plugin) {
|
||||
throw new Error(`Plugin not found: ${pluginName}`);
|
||||
}
|
||||
|
||||
return new Promise((resolve) => {
|
||||
setTimeout(() => {
|
||||
// Simulate success/failure
|
||||
const success = Math.random() > 0.1; // 90% success rate
|
||||
// Find action definition in plugin
|
||||
const actionDefinition = plugin.actions?.find(
|
||||
(a: any) => a.id === actionId,
|
||||
);
|
||||
if (!actionDefinition) {
|
||||
throw new Error(
|
||||
`Action '${actionId}' not found in plugin '${pluginName}'`,
|
||||
);
|
||||
}
|
||||
|
||||
resolve({
|
||||
success,
|
||||
// Validate parameters
|
||||
const validatedParams = this.validateActionParameters(
|
||||
actionDefinition,
|
||||
action.parameters,
|
||||
);
|
||||
|
||||
// Execute action through robot communication service
|
||||
const result = await this.executeRobotActionWithComm(
|
||||
plugin,
|
||||
actionDefinition,
|
||||
validatedParams,
|
||||
trialId,
|
||||
);
|
||||
|
||||
const duration = Date.now() - startTime;
|
||||
|
||||
return {
|
||||
success: true,
|
||||
completed: true,
|
||||
duration: simulationDelay,
|
||||
duration,
|
||||
data: {
|
||||
pluginId,
|
||||
actionType,
|
||||
parameters: action.parameters,
|
||||
robotResponse: success
|
||||
? "Action completed successfully"
|
||||
: "Robot action failed",
|
||||
pluginName,
|
||||
actionId,
|
||||
parameters: validatedParams,
|
||||
robotResponse: result,
|
||||
platform: plugin.platform,
|
||||
},
|
||||
error: success ? undefined : "Simulated robot failure",
|
||||
});
|
||||
}, simulationDelay);
|
||||
});
|
||||
};
|
||||
} catch (error) {
|
||||
const duration = Date.now() - startTime;
|
||||
|
||||
return {
|
||||
success: false,
|
||||
completed: false,
|
||||
duration: 0,
|
||||
duration,
|
||||
error:
|
||||
error instanceof Error
|
||||
? error.message
|
||||
@@ -531,6 +572,224 @@ export class TrialExecutionEngine {
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Get plugin definition from database with caching
|
||||
*/
|
||||
private async getPluginDefinition(pluginName: string): Promise<any> {
|
||||
// Check cache first
|
||||
if (this.pluginCache.has(pluginName)) {
|
||||
return this.pluginCache.get(pluginName);
|
||||
}
|
||||
|
||||
try {
|
||||
const [plugin] = await this.db
|
||||
.select()
|
||||
.from(plugins)
|
||||
.where(eq(plugins.name, pluginName))
|
||||
.limit(1);
|
||||
|
||||
if (plugin) {
|
||||
// Cache the plugin definition
|
||||
const pluginData = {
|
||||
...plugin,
|
||||
actions: plugin.actionDefinitions,
|
||||
platform: (plugin.metadata as any)?.platform,
|
||||
ros2Config: (plugin.metadata as any)?.ros2Config,
|
||||
};
|
||||
|
||||
this.pluginCache.set(pluginName, pluginData);
|
||||
return pluginData;
|
||||
}
|
||||
|
||||
return null;
|
||||
} catch (error) {
|
||||
console.error(`Failed to load plugin ${pluginName}:`, error);
|
||||
return null;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Validate action parameters against plugin schema
|
||||
*/
|
||||
private validateActionParameters(
|
||||
actionDefinition: any,
|
||||
parameters: Record<string, unknown>,
|
||||
): Record<string, unknown> {
|
||||
const validated: Record<string, unknown> = {};
|
||||
|
||||
if (!actionDefinition.parameters) {
|
||||
return parameters;
|
||||
}
|
||||
|
||||
for (const paramDef of actionDefinition.parameters) {
|
||||
const paramName = paramDef.name;
|
||||
const paramValue = parameters[paramName];
|
||||
|
||||
// Required parameter check
|
||||
if (
|
||||
paramDef.required &&
|
||||
(paramValue === undefined || paramValue === null)
|
||||
) {
|
||||
throw new Error(`Required parameter '${paramName}' is missing`);
|
||||
}
|
||||
|
||||
// Use default value if parameter not provided
|
||||
if (paramValue === undefined && paramDef.default !== undefined) {
|
||||
validated[paramName] = paramDef.default;
|
||||
continue;
|
||||
}
|
||||
|
||||
if (paramValue !== undefined) {
|
||||
// Type validation
|
||||
switch (paramDef.type) {
|
||||
case "number":
|
||||
const numValue = Number(paramValue);
|
||||
if (isNaN(numValue)) {
|
||||
throw new Error(`Parameter '${paramName}' must be a number`);
|
||||
}
|
||||
if (paramDef.min !== undefined && numValue < paramDef.min) {
|
||||
throw new Error(
|
||||
`Parameter '${paramName}' must be >= ${paramDef.min}`,
|
||||
);
|
||||
}
|
||||
if (paramDef.max !== undefined && numValue > paramDef.max) {
|
||||
throw new Error(
|
||||
`Parameter '${paramName}' must be <= ${paramDef.max}`,
|
||||
);
|
||||
}
|
||||
validated[paramName] = numValue;
|
||||
break;
|
||||
|
||||
case "boolean":
|
||||
validated[paramName] = Boolean(paramValue);
|
||||
break;
|
||||
|
||||
case "select":
|
||||
if (paramDef.options) {
|
||||
const validOptions = paramDef.options.map(
|
||||
(opt: any) => opt.value,
|
||||
);
|
||||
if (!validOptions.includes(paramValue)) {
|
||||
throw new Error(
|
||||
`Parameter '${paramName}' must be one of: ${validOptions.join(", ")}`,
|
||||
);
|
||||
}
|
||||
}
|
||||
validated[paramName] = paramValue;
|
||||
break;
|
||||
|
||||
default:
|
||||
validated[paramName] = paramValue;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return validated;
|
||||
}
|
||||
|
||||
/**
|
||||
* Execute robot action through robot communication service
|
||||
*/
|
||||
private async executeRobotActionWithComm(
|
||||
plugin: any,
|
||||
actionDefinition: any,
|
||||
parameters: Record<string, unknown>,
|
||||
trialId: string,
|
||||
): Promise<string> {
|
||||
// Ensure robot communication service is available
|
||||
if (!this.robotComm.getConnectionStatus()) {
|
||||
try {
|
||||
await this.robotComm.connect();
|
||||
} catch (error) {
|
||||
throw new Error(
|
||||
`Failed to connect to robot: ${error instanceof Error ? error.message : "Unknown error"}`,
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
// Prepare robot action
|
||||
const robotAction: RobotAction = {
|
||||
pluginName: plugin.name,
|
||||
actionId: actionDefinition.id,
|
||||
parameters,
|
||||
implementation: actionDefinition.implementation,
|
||||
};
|
||||
|
||||
// Execute action through robot communication service
|
||||
const result: RobotActionResult =
|
||||
await this.robotComm.executeAction(robotAction);
|
||||
|
||||
if (!result.success) {
|
||||
throw new Error(result.error || "Robot action failed");
|
||||
}
|
||||
|
||||
// Log the successful action execution
|
||||
await this.logTrialEvent(trialId, "robot_action_executed", {
|
||||
actionId: actionDefinition.id,
|
||||
parameters,
|
||||
platform: plugin.platform,
|
||||
topic: actionDefinition.implementation?.topic,
|
||||
messageType: actionDefinition.implementation?.messageType,
|
||||
duration: result.duration,
|
||||
robotResponse: result.data,
|
||||
});
|
||||
|
||||
// Return human-readable result
|
||||
return this.formatRobotActionResult(
|
||||
plugin,
|
||||
actionDefinition,
|
||||
parameters,
|
||||
result,
|
||||
);
|
||||
}
|
||||
|
||||
/**
|
||||
* Format robot action result for human readability
|
||||
*/
|
||||
private formatRobotActionResult(
|
||||
plugin: any,
|
||||
actionDefinition: any,
|
||||
parameters: Record<string, unknown>,
|
||||
result: RobotActionResult,
|
||||
): string {
|
||||
const actionType = actionDefinition.id;
|
||||
const platform = plugin.platform || "Robot";
|
||||
|
||||
switch (actionType) {
|
||||
case "say_text":
|
||||
return `${platform} said: "${parameters.text}"`;
|
||||
|
||||
case "walk_forward":
|
||||
return `${platform} walked forward at speed ${parameters.speed} for ${parameters.duration || "indefinite"} seconds`;
|
||||
|
||||
case "walk_backward":
|
||||
return `${platform} walked backward at speed ${parameters.speed} for ${parameters.duration || "indefinite"} seconds`;
|
||||
|
||||
case "turn_left":
|
||||
case "turn_right":
|
||||
const direction = actionType.split("_")[1];
|
||||
return `${platform} turned ${direction} at speed ${parameters.speed}`;
|
||||
|
||||
case "move_head":
|
||||
return `${platform} moved head to yaw=${parameters.yaw}, pitch=${parameters.pitch}`;
|
||||
|
||||
case "move_arm":
|
||||
return `${platform} moved ${parameters.arm} arm to specified position`;
|
||||
|
||||
case "stop_movement":
|
||||
return `${platform} stopped all movement`;
|
||||
|
||||
case "set_volume":
|
||||
return `${platform} set volume to ${parameters.volume}`;
|
||||
|
||||
case "set_language":
|
||||
return `${platform} set language to ${parameters.language}`;
|
||||
|
||||
default:
|
||||
return `${platform} executed action: ${actionType} (${result.duration}ms)`;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Advance to the next step
|
||||
*/
|
||||
|
||||
Reference in New Issue
Block a user