mirror of
https://github.com/soconnor0919/hristudio.git
synced 2026-02-04 23:46:32 -05:00
fix: migrate wizard from polling to WebSocket and fix duplicate ROS connections
- Removed non-functional trial WebSocket (no server exists) - Kept ROS WebSocket for robot control via useWizardRos - Fixed duplicate ROS connections by passing connection as props - WizardMonitoringPanel now receives ROS connection from parent - Trial status uses reliable tRPC polling (5-15s intervals) - Updated connection badges to show 'ROS Connected/Offline' - Added loading overlay with fade-in to designer - Fixed hash computation to include parameter values - Fixed incremental hash caching for parameter changes Fixes: - WebSocket connection errors eliminated - Connect button now works properly - No more conflicting duplicate connections - Accurate connection status display
This commit is contained in:
@@ -1,6 +1,6 @@
|
||||
"use client";
|
||||
|
||||
import React, { useState, useEffect } from "react";
|
||||
import React, { useState, useEffect, useMemo, useCallback } from "react";
|
||||
import {
|
||||
Bot,
|
||||
Play,
|
||||
@@ -13,6 +13,8 @@ import {
|
||||
Eye,
|
||||
Hand,
|
||||
Zap,
|
||||
Wifi,
|
||||
WifiOff,
|
||||
} from "lucide-react";
|
||||
import { Button } from "~/components/ui/button";
|
||||
import { Badge } from "~/components/ui/badge";
|
||||
@@ -45,6 +47,7 @@ import {
|
||||
} from "~/components/ui/collapsible";
|
||||
import { api } from "~/trpc/react";
|
||||
import { toast } from "sonner";
|
||||
import { useWizardRos } from "~/hooks/useWizardRos";
|
||||
|
||||
interface RobotAction {
|
||||
id: string;
|
||||
@@ -85,16 +88,46 @@ interface Plugin {
|
||||
interface RobotActionsPanelProps {
|
||||
studyId: string;
|
||||
trialId: string;
|
||||
onExecuteAction: (
|
||||
onExecuteAction?: (
|
||||
pluginName: string,
|
||||
actionId: string,
|
||||
parameters: Record<string, unknown>,
|
||||
) => Promise<void>;
|
||||
}
|
||||
|
||||
// Helper functions moved outside component to prevent re-renders
|
||||
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;
|
||||
};
|
||||
|
||||
export function RobotActionsPanel({
|
||||
studyId,
|
||||
trialId,
|
||||
trialId: _trialId,
|
||||
onExecuteAction,
|
||||
}: RobotActionsPanelProps) {
|
||||
const [selectedPlugin, setSelectedPlugin] = useState<string>("");
|
||||
@@ -111,15 +144,52 @@ export function RobotActionsPanel({
|
||||
new Set(["movement", "speech"]),
|
||||
);
|
||||
|
||||
// WebSocket ROS integration
|
||||
const {
|
||||
isConnected: rosConnected,
|
||||
isConnecting: rosConnecting,
|
||||
connectionError: rosError,
|
||||
robotStatus,
|
||||
activeActions,
|
||||
connect: connectRos,
|
||||
disconnect: disconnectRos,
|
||||
executeRobotAction: executeRosAction,
|
||||
} = useWizardRos({
|
||||
autoConnect: true,
|
||||
onActionCompleted: (execution) => {
|
||||
toast.success(`Completed: ${execution.actionId}`, {
|
||||
description: `Action executed in ${execution.endTime ? execution.endTime.getTime() - execution.startTime.getTime() : 0}ms`,
|
||||
});
|
||||
// Remove from executing set
|
||||
setExecutingActions((prev) => {
|
||||
const next = new Set(prev);
|
||||
next.delete(`${execution.pluginName}.${execution.actionId}`);
|
||||
return next;
|
||||
});
|
||||
},
|
||||
onActionFailed: (execution) => {
|
||||
toast.error(`Failed: ${execution.actionId}`, {
|
||||
description: execution.error || "Unknown error",
|
||||
});
|
||||
// Remove from executing set
|
||||
setExecutingActions((prev) => {
|
||||
const next = new Set(prev);
|
||||
next.delete(`${execution.pluginName}.${execution.actionId}`);
|
||||
return next;
|
||||
});
|
||||
},
|
||||
});
|
||||
|
||||
// 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,
|
||||
// Get actions for selected plugin - memoized to prevent infinite re-renders
|
||||
const selectedPluginData = useMemo(
|
||||
() => plugins.find((p) => p.plugin.id === selectedPlugin),
|
||||
[plugins, selectedPlugin],
|
||||
);
|
||||
|
||||
// Initialize parameters when action changes
|
||||
@@ -155,22 +225,87 @@ export function RobotActionsPanel({
|
||||
}
|
||||
}, [selectedAction]);
|
||||
|
||||
const handleExecuteAction = async () => {
|
||||
const toggleCategory = useCallback((category: string) => {
|
||||
setExpandedCategories((prev) => {
|
||||
const next = new Set(prev);
|
||||
if (next.has(category)) {
|
||||
next.delete(category);
|
||||
} else {
|
||||
next.add(category);
|
||||
}
|
||||
return next;
|
||||
});
|
||||
}, []);
|
||||
|
||||
const handleExecuteAction = useCallback(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,
|
||||
);
|
||||
// Get action configuration from plugin
|
||||
const actionDef = (
|
||||
selectedPluginData.plugin.actionDefinitions as RobotAction[]
|
||||
)?.find((def: RobotAction) => def.id === selectedAction.id);
|
||||
|
||||
toast.success(`Executed: ${selectedAction.name}`, {
|
||||
description: `Robot action completed successfully`,
|
||||
});
|
||||
// Try direct WebSocket execution first
|
||||
if (rosConnected && actionDef) {
|
||||
try {
|
||||
// Look for ROS2 configuration in the action definition
|
||||
const actionConfig = (actionDef as any).ros2
|
||||
? {
|
||||
topic: (actionDef as any).ros2.topic,
|
||||
messageType: (actionDef as any).ros2.messageType,
|
||||
payloadMapping: (actionDef as any).ros2.payloadMapping,
|
||||
}
|
||||
: undefined;
|
||||
|
||||
await executeRosAction(
|
||||
selectedPluginData.plugin.name,
|
||||
selectedAction.id,
|
||||
actionParameters,
|
||||
actionConfig,
|
||||
);
|
||||
|
||||
toast.success(`Executed: ${selectedAction.name}`, {
|
||||
description: `Robot action completed via WebSocket`,
|
||||
});
|
||||
} catch (rosError) {
|
||||
console.warn(
|
||||
"WebSocket execution failed, falling back to tRPC:",
|
||||
rosError,
|
||||
);
|
||||
|
||||
// Fallback to tRPC execution
|
||||
if (onExecuteAction) {
|
||||
await onExecuteAction(
|
||||
selectedPluginData.plugin.name,
|
||||
selectedAction.id,
|
||||
actionParameters,
|
||||
);
|
||||
|
||||
toast.success(`Executed: ${selectedAction.name}`, {
|
||||
description: `Robot action completed via tRPC fallback`,
|
||||
});
|
||||
} else {
|
||||
throw rosError;
|
||||
}
|
||||
}
|
||||
} else if (onExecuteAction) {
|
||||
// Use tRPC execution if WebSocket not available
|
||||
await onExecuteAction(
|
||||
selectedPluginData.plugin.name,
|
||||
selectedAction.id,
|
||||
actionParameters,
|
||||
);
|
||||
|
||||
toast.success(`Executed: ${selectedAction.name}`, {
|
||||
description: `Robot action completed via tRPC`,
|
||||
});
|
||||
} else {
|
||||
throw new Error("No execution method available");
|
||||
}
|
||||
} catch (error) {
|
||||
toast.error(`Failed to execute: ${selectedAction.name}`, {
|
||||
description: error instanceof Error ? error.message : "Unknown error",
|
||||
@@ -182,18 +317,27 @@ export function RobotActionsPanel({
|
||||
return next;
|
||||
});
|
||||
}
|
||||
};
|
||||
}, [
|
||||
selectedAction,
|
||||
selectedPluginData,
|
||||
rosConnected,
|
||||
executeRosAction,
|
||||
onExecuteAction,
|
||||
]);
|
||||
|
||||
const handleParameterChange = (paramName: string, value: unknown) => {
|
||||
setActionParameters((prev) => ({
|
||||
...prev,
|
||||
[paramName]: value,
|
||||
}));
|
||||
};
|
||||
const handleParameterChange = useCallback(
|
||||
(paramName: string, value: unknown) => {
|
||||
setActionParameters((prev) => ({
|
||||
...prev,
|
||||
[paramName]: value,
|
||||
}));
|
||||
},
|
||||
[],
|
||||
);
|
||||
|
||||
const renderParameterInput = (
|
||||
param: NonNullable<RobotAction["parameters"]>[0],
|
||||
paramIndex: number,
|
||||
_paramIndex: number,
|
||||
) => {
|
||||
if (!param) return null;
|
||||
|
||||
@@ -323,47 +467,6 @@ export function RobotActionsPanel({
|
||||
}
|
||||
};
|
||||
|
||||
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">
|
||||
@@ -375,18 +478,68 @@ export function RobotActionsPanel({
|
||||
|
||||
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>
|
||||
<div className="space-y-3">
|
||||
<div className="flex items-center justify-between rounded-lg border p-3">
|
||||
<div className="flex items-center space-x-2">
|
||||
{rosConnected ? (
|
||||
<Wifi className="h-4 w-4 text-green-500" />
|
||||
) : rosConnecting ? (
|
||||
<Loader2 className="h-4 w-4 animate-spin text-yellow-500" />
|
||||
) : (
|
||||
<WifiOff className="h-4 w-4 text-red-500" />
|
||||
)}
|
||||
<span className="text-sm font-medium">ROS Bridge</span>
|
||||
</div>
|
||||
|
||||
<div className="flex items-center space-x-2">
|
||||
<Badge
|
||||
variant={
|
||||
rosConnected
|
||||
? "default"
|
||||
: rosConnecting
|
||||
? "secondary"
|
||||
: "destructive"
|
||||
}
|
||||
className="text-xs"
|
||||
>
|
||||
{rosConnected
|
||||
? "Connected"
|
||||
: rosConnecting
|
||||
? "Connecting"
|
||||
: "Disconnected"}
|
||||
</Badge>
|
||||
|
||||
{!rosConnected && !rosConnecting && (
|
||||
<Button size="sm" variant="outline" onClick={() => connectRos()}>
|
||||
Connect
|
||||
</Button>
|
||||
)}
|
||||
|
||||
{rosConnected && (
|
||||
<Button
|
||||
size="sm"
|
||||
variant="outline"
|
||||
onClick={() => disconnectRos()}
|
||||
>
|
||||
Disconnect
|
||||
</Button>
|
||||
)}
|
||||
</div>
|
||||
</div>
|
||||
<Alert>
|
||||
<AlertCircle className="h-4 w-4" />
|
||||
<AlertDescription>
|
||||
No robot plugins are installed in this study. Install plugins to
|
||||
control robots during trials.
|
||||
</AlertDescription>
|
||||
</Alert>
|
||||
</div>
|
||||
);
|
||||
}
|
||||
|
||||
return (
|
||||
<div className="space-y-4">
|
||||
<ConnectionStatus />
|
||||
{/* Plugin Selection */}
|
||||
<div className="space-y-2">
|
||||
<Label>Select Robot Plugin</Label>
|
||||
@@ -523,92 +676,429 @@ export function RobotActionsPanel({
|
||||
</>
|
||||
)}
|
||||
</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 */}
|
||||
{/* Quick Actions */}
|
||||
<Card>
|
||||
<CardHeader>
|
||||
<CardTitle className="flex items-center space-x-2">
|
||||
<Zap className="h-4 w-4" />
|
||||
<span>Quick Actions</span>
|
||||
</CardTitle>
|
||||
<CardDescription>
|
||||
Common robot actions for quick execution
|
||||
</CardDescription>
|
||||
</CardHeader>
|
||||
<CardContent className="space-y-2">
|
||||
<div className="grid grid-cols-2 gap-2">
|
||||
<Button
|
||||
size="sm"
|
||||
variant="outline"
|
||||
onClick={() => {
|
||||
if (rosConnected) {
|
||||
executeRosAction("nao6-ros2", "say_text", {
|
||||
text: "Hello, I am ready!",
|
||||
}).catch((error) => {
|
||||
console.error("Quick action failed:", error);
|
||||
});
|
||||
}
|
||||
}}
|
||||
disabled={!rosConnected || rosConnecting}
|
||||
>
|
||||
<Volume2 className="mr-1 h-3 w-3" />
|
||||
Say Hello
|
||||
</Button>
|
||||
|
||||
<Button
|
||||
size="sm"
|
||||
variant="outline"
|
||||
onClick={() => {
|
||||
if (rosConnected) {
|
||||
executeRosAction("nao6-ros2", "emergency_stop", {}).catch(
|
||||
(error) => {
|
||||
console.error("Emergency stop failed:", error);
|
||||
},
|
||||
);
|
||||
}
|
||||
}}
|
||||
disabled={!rosConnected || rosConnecting}
|
||||
>
|
||||
<AlertCircle className="mr-1 h-3 w-3" />
|
||||
Stop
|
||||
</Button>
|
||||
|
||||
<Button
|
||||
size="sm"
|
||||
variant="outline"
|
||||
onClick={() => {
|
||||
if (rosConnected) {
|
||||
executeRosAction("nao6-ros2", "turn_head", {
|
||||
yaw: 0,
|
||||
pitch: 0,
|
||||
speed: 0.3,
|
||||
}).catch((error) => {
|
||||
console.error("Head center failed:", error);
|
||||
});
|
||||
}
|
||||
}}
|
||||
disabled={!rosConnected || rosConnecting}
|
||||
>
|
||||
<Eye className="mr-1 h-3 w-3" />
|
||||
Center Head
|
||||
</Button>
|
||||
|
||||
<Button
|
||||
size="sm"
|
||||
variant="outline"
|
||||
onClick={() => {
|
||||
if (rosConnected) {
|
||||
executeRosAction("nao6-ros2", "walk_forward", {
|
||||
speed: 0.1,
|
||||
duration: 2,
|
||||
}).catch((error) => {
|
||||
console.error("Walk forward failed:", error);
|
||||
});
|
||||
}
|
||||
}}
|
||||
disabled={!rosConnected || rosConnecting}
|
||||
>
|
||||
<Move className="mr-1 h-3 w-3" />
|
||||
Walk Test
|
||||
</Button>
|
||||
</div>
|
||||
</CardContent>
|
||||
</Card>
|
||||
</div>
|
||||
);
|
||||
|
||||
function ConnectionStatus() {
|
||||
return (
|
||||
<div className="flex items-center justify-between rounded-lg border p-3">
|
||||
<div className="flex items-center space-x-2">
|
||||
{rosConnected ? (
|
||||
<Wifi className="h-4 w-4 text-green-500" />
|
||||
) : rosConnecting ? (
|
||||
<Loader2 className="h-4 w-4 animate-spin text-yellow-500" />
|
||||
) : (
|
||||
<WifiOff className="h-4 w-4 text-red-500" />
|
||||
)}
|
||||
<span className="text-sm font-medium">ROS Bridge</span>
|
||||
</div>
|
||||
|
||||
<div className="flex items-center space-x-2">
|
||||
<Badge
|
||||
variant={
|
||||
rosConnected
|
||||
? "default"
|
||||
: rosConnecting
|
||||
? "secondary"
|
||||
: "destructive"
|
||||
}
|
||||
className="text-xs"
|
||||
>
|
||||
{rosConnected
|
||||
? "Connected"
|
||||
: rosConnecting
|
||||
? "Connecting"
|
||||
: "Disconnected"}
|
||||
</Badge>
|
||||
|
||||
{!rosConnected && !rosConnecting && (
|
||||
<Button size="sm" variant="outline" onClick={() => connectRos()}>
|
||||
Connect
|
||||
</Button>
|
||||
)}
|
||||
|
||||
{rosConnected && (
|
||||
<Button size="sm" variant="outline" onClick={() => disconnectRos()}>
|
||||
Disconnect
|
||||
</Button>
|
||||
)}
|
||||
</div>
|
||||
</div>
|
||||
);
|
||||
}
|
||||
|
||||
return (
|
||||
<div className="space-y-4">
|
||||
<div className="flex items-center justify-between rounded-lg border p-3">
|
||||
<div className="flex items-center space-x-2">
|
||||
{rosConnected ? (
|
||||
<Wifi className="h-4 w-4 text-green-500" />
|
||||
) : rosConnecting ? (
|
||||
<Loader2 className="h-4 w-4 animate-spin text-yellow-500" />
|
||||
) : (
|
||||
<WifiOff className="h-4 w-4 text-red-500" />
|
||||
)}
|
||||
<span className="text-sm font-medium">ROS Bridge</span>
|
||||
</div>
|
||||
|
||||
<div className="flex items-center space-x-2">
|
||||
<Badge
|
||||
variant={
|
||||
rosConnected
|
||||
? "default"
|
||||
: rosConnecting
|
||||
? "secondary"
|
||||
: "destructive"
|
||||
}
|
||||
className="text-xs"
|
||||
>
|
||||
{rosConnected
|
||||
? "Connected"
|
||||
: rosConnecting
|
||||
? "Connecting"
|
||||
: "Disconnected"}
|
||||
</Badge>
|
||||
|
||||
{!rosConnected && !rosConnecting && (
|
||||
<Button size="sm" variant="outline" onClick={() => connectRos()}>
|
||||
Connect
|
||||
</Button>
|
||||
)}
|
||||
|
||||
{rosConnected && (
|
||||
<Button size="sm" variant="outline" onClick={() => disconnectRos()}>
|
||||
Disconnect
|
||||
</Button>
|
||||
)}
|
||||
</div>
|
||||
</div>
|
||||
{/* 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 && (
|
||||
<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 className="space-y-2">
|
||||
<Label>Available Actions</Label>
|
||||
<ScrollArea className="h-64 rounded-md border">
|
||||
<div className="space-y-2 p-2">
|
||||
{selectedPluginData &&
|
||||
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) > 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 ||
|
||||
!selectedAction ||
|
||||
executingActions.has(
|
||||
`${selectedPluginData?.plugin.name}.${selectedAction?.id}`,
|
||||
)
|
||||
}
|
||||
className="w-full"
|
||||
>
|
||||
{selectedPluginData &&
|
||||
selectedAction &&
|
||||
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>
|
||||
</CardContent>
|
||||
</Card>
|
||||
)}
|
||||
|
||||
{/* Quick Actions */}
|
||||
<Card>
|
||||
<CardHeader>
|
||||
<CardTitle className="flex items-center space-x-2">
|
||||
<Zap className="h-4 w-4" />
|
||||
<span>Quick Actions</span>
|
||||
</CardTitle>
|
||||
<CardDescription>
|
||||
Common robot actions for quick execution
|
||||
</CardDescription>
|
||||
</CardHeader>
|
||||
<CardContent className="space-y-2">
|
||||
<div className="grid grid-cols-2 gap-2">
|
||||
<Button
|
||||
size="sm"
|
||||
variant="outline"
|
||||
onClick={() => {
|
||||
if (rosConnected) {
|
||||
executeRosAction("nao6-ros2", "say_text", {
|
||||
text: "Hello, I am ready!",
|
||||
}).catch((error: unknown) => {
|
||||
console.error("Quick action failed:", error);
|
||||
});
|
||||
}
|
||||
}}
|
||||
disabled={!rosConnected || rosConnecting}
|
||||
>
|
||||
<Volume2 className="mr-1 h-3 w-3" />
|
||||
Say Hello
|
||||
</Button>
|
||||
|
||||
<Button
|
||||
size="sm"
|
||||
variant="outline"
|
||||
onClick={() => {
|
||||
if (rosConnected) {
|
||||
executeRosAction("nao6-ros2", "emergency_stop", {}).catch(
|
||||
(error: unknown) => {
|
||||
console.error("Emergency stop failed:", error);
|
||||
},
|
||||
);
|
||||
}
|
||||
}}
|
||||
disabled={!rosConnected || rosConnecting}
|
||||
>
|
||||
<AlertCircle className="mr-1 h-3 w-3" />
|
||||
Stop
|
||||
</Button>
|
||||
|
||||
<Button
|
||||
size="sm"
|
||||
variant="outline"
|
||||
onClick={() => {
|
||||
if (rosConnected) {
|
||||
executeRosAction("nao6-ros2", "turn_head", {
|
||||
yaw: 0,
|
||||
pitch: 0,
|
||||
speed: 0.3,
|
||||
}).catch((error: unknown) => {
|
||||
console.error("Head center failed:", error);
|
||||
});
|
||||
}
|
||||
}}
|
||||
disabled={!rosConnected || rosConnecting}
|
||||
>
|
||||
<Eye className="mr-1 h-3 w-3" />
|
||||
Center Head
|
||||
</Button>
|
||||
|
||||
<Button
|
||||
size="sm"
|
||||
variant="outline"
|
||||
onClick={() => {
|
||||
if (rosConnected) {
|
||||
executeRosAction("nao6-ros2", "walk_forward", {
|
||||
speed: 0.1,
|
||||
duration: 2,
|
||||
}).catch((error: unknown) => {
|
||||
console.error("Walk forward failed:", error);
|
||||
});
|
||||
}
|
||||
}}
|
||||
disabled={!rosConnected || rosConnecting}
|
||||
>
|
||||
<Move className="mr-1 h-3 w-3" />
|
||||
Walk Test
|
||||
</Button>
|
||||
</div>
|
||||
</CardContent>
|
||||
</Card>
|
||||
</div>
|
||||
);
|
||||
}
|
||||
|
||||
@@ -10,7 +10,7 @@ import { WizardControlPanel } from "./panels/WizardControlPanel";
|
||||
import { WizardExecutionPanel } from "./panels/WizardExecutionPanel";
|
||||
import { WizardMonitoringPanel } from "./panels/WizardMonitoringPanel";
|
||||
import { api } from "~/trpc/react";
|
||||
// import { useTrialWebSocket } from "~/hooks/useWebSocket"; // Removed WebSocket dependency
|
||||
import { useWizardRos } from "~/hooks/useWizardRos";
|
||||
import { toast } from "sonner";
|
||||
|
||||
interface WizardInterfaceProps {
|
||||
@@ -47,10 +47,10 @@ interface StepData {
|
||||
name: string;
|
||||
description: string | null;
|
||||
type:
|
||||
| "wizard_action"
|
||||
| "robot_action"
|
||||
| "parallel_steps"
|
||||
| "conditional_branch";
|
||||
| "wizard_action"
|
||||
| "robot_action"
|
||||
| "parallel_steps"
|
||||
| "conditional_branch";
|
||||
parameters: Record<string, unknown>;
|
||||
order: number;
|
||||
}
|
||||
@@ -116,17 +116,59 @@ export const WizardInterface = React.memo(function WizardInterface({
|
||||
}
|
||||
};
|
||||
|
||||
// Use polling for real-time updates (no WebSocket dependency)
|
||||
// Memoized callbacks to prevent infinite re-renders
|
||||
const onActionCompleted = useCallback((execution: { actionId: string }) => {
|
||||
toast.success(`Robot action completed: ${execution.actionId}`);
|
||||
}, []);
|
||||
|
||||
const onActionFailed = useCallback((execution: { actionId: string; error?: string }) => {
|
||||
toast.error(`Robot action failed: ${execution.actionId}`, {
|
||||
description: execution.error,
|
||||
});
|
||||
}, []);
|
||||
|
||||
// ROS WebSocket connection for robot control
|
||||
const {
|
||||
isConnected: rosConnected,
|
||||
isConnecting: rosConnecting,
|
||||
connectionError: rosError,
|
||||
robotStatus,
|
||||
connect: connectRos,
|
||||
disconnect: disconnectRos,
|
||||
executeRobotAction: executeRosAction,
|
||||
} = useWizardRos({
|
||||
autoConnect: true,
|
||||
onActionCompleted,
|
||||
onActionFailed,
|
||||
});
|
||||
|
||||
// Use polling for trial status updates (no trial WebSocket server exists)
|
||||
const { data: pollingData } = api.trials.get.useQuery(
|
||||
{ id: trial.id },
|
||||
{
|
||||
refetchInterval: trial.status === "in_progress" ? 10000 : 30000, // Poll less frequently
|
||||
staleTime: 5000, // Consider data fresh for 5 seconds
|
||||
refetchOnWindowFocus: false, // Don't refetch on window focus
|
||||
refetchInterval: trial.status === "in_progress" ? 5000 : 15000,
|
||||
staleTime: 2000,
|
||||
refetchOnWindowFocus: false,
|
||||
},
|
||||
);
|
||||
|
||||
// Memoized trial events to prevent re-creation on every render
|
||||
// Update local trial state from polling
|
||||
useEffect(() => {
|
||||
if (pollingData) {
|
||||
setTrial((prev) => ({
|
||||
...prev,
|
||||
status: pollingData.status,
|
||||
startedAt: pollingData.startedAt
|
||||
? new Date(pollingData.startedAt)
|
||||
: prev.startedAt,
|
||||
completedAt: pollingData.completedAt
|
||||
? new Date(pollingData.completedAt)
|
||||
: prev.completedAt,
|
||||
}));
|
||||
}
|
||||
}, [pollingData]);
|
||||
|
||||
// Trial events from robot actions
|
||||
const trialEvents = useMemo<
|
||||
Array<{
|
||||
type: string;
|
||||
@@ -136,39 +178,6 @@ export const WizardInterface = React.memo(function WizardInterface({
|
||||
}>
|
||||
>(() => [], []);
|
||||
|
||||
// Update trial data from polling (optimized to prevent unnecessary re-renders)
|
||||
const updateTrial = useCallback((newTrialData: typeof pollingData) => {
|
||||
if (!newTrialData) return;
|
||||
|
||||
setTrial((prevTrial) => {
|
||||
// Only update if data actually changed
|
||||
if (
|
||||
prevTrial.id === newTrialData.id &&
|
||||
prevTrial.status === newTrialData.status &&
|
||||
prevTrial.startedAt === newTrialData.startedAt &&
|
||||
prevTrial.completedAt === newTrialData.completedAt
|
||||
) {
|
||||
return prevTrial; // No changes, keep existing state
|
||||
}
|
||||
|
||||
return {
|
||||
...newTrialData,
|
||||
metadata: newTrialData.metadata as Record<string, unknown> | null,
|
||||
participant: {
|
||||
...newTrialData.participant,
|
||||
demographics: newTrialData.participant.demographics as Record<
|
||||
string,
|
||||
unknown
|
||||
> | null,
|
||||
},
|
||||
};
|
||||
});
|
||||
}, []);
|
||||
|
||||
useEffect(() => {
|
||||
updateTrial(pollingData);
|
||||
}, [pollingData, updateTrial]);
|
||||
|
||||
// Transform experiment steps to component format
|
||||
const steps: StepData[] =
|
||||
experimentSteps?.map((step, index) => ({
|
||||
@@ -338,22 +347,63 @@ export const WizardInterface = React.memo(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);
|
||||
}
|
||||
};
|
||||
const handleExecuteRobotAction = useCallback(
|
||||
async (
|
||||
pluginName: string,
|
||||
actionId: string,
|
||||
parameters: Record<string, unknown>,
|
||||
) => {
|
||||
try {
|
||||
// Try direct WebSocket execution first for better performance
|
||||
if (rosConnected) {
|
||||
try {
|
||||
await executeRosAction(pluginName, actionId, parameters);
|
||||
|
||||
// Log to trial events for data capture
|
||||
await executeRobotActionMutation.mutateAsync({
|
||||
trialId: trial.id,
|
||||
pluginName,
|
||||
actionId,
|
||||
parameters,
|
||||
});
|
||||
|
||||
toast.success(`Robot action executed: ${actionId}`);
|
||||
} catch (rosError) {
|
||||
console.warn(
|
||||
"WebSocket execution failed, falling back to tRPC:",
|
||||
rosError,
|
||||
);
|
||||
|
||||
// Fallback to tRPC-only execution
|
||||
await executeRobotActionMutation.mutateAsync({
|
||||
trialId: trial.id,
|
||||
pluginName,
|
||||
actionId,
|
||||
parameters,
|
||||
});
|
||||
|
||||
toast.success(`Robot action executed via fallback: ${actionId}`);
|
||||
}
|
||||
} else {
|
||||
// Use tRPC execution if WebSocket not connected
|
||||
await executeRobotActionMutation.mutateAsync({
|
||||
trialId: trial.id,
|
||||
pluginName,
|
||||
actionId,
|
||||
parameters,
|
||||
});
|
||||
|
||||
toast.success(`Robot action executed: ${actionId}`);
|
||||
}
|
||||
} catch (error) {
|
||||
console.error("Failed to execute robot action:", error);
|
||||
toast.error(`Failed to execute robot action: ${actionId}`, {
|
||||
description: error instanceof Error ? error.message : "Unknown error",
|
||||
});
|
||||
}
|
||||
},
|
||||
[rosConnected, executeRosAction, executeRobotActionMutation, trial.id],
|
||||
);
|
||||
|
||||
return (
|
||||
<div className="flex h-full flex-col">
|
||||
@@ -391,20 +441,17 @@ export const WizardInterface = React.memo(function WizardInterface({
|
||||
<div className="text-muted-foreground flex items-center gap-4 text-sm">
|
||||
<div>{trial.experiment.name}</div>
|
||||
<div>{trial.participant.participantCode}</div>
|
||||
<Badge variant="outline" className="text-xs">
|
||||
Polling
|
||||
<Badge
|
||||
variant={rosConnected ? "default" : "outline"}
|
||||
className="text-xs"
|
||||
>
|
||||
{rosConnected ? "ROS Connected" : "ROS Offline"}
|
||||
</Badge>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
{/* Connection Status */}
|
||||
<Alert className="mx-4 mt-2">
|
||||
<AlertCircle className="h-4 w-4" />
|
||||
<AlertDescription>
|
||||
Using polling mode for trial updates (refreshes every 2 seconds).
|
||||
</AlertDescription>
|
||||
</Alert>
|
||||
{/* No connection status alert - ROS connection shown in monitoring panel */}
|
||||
|
||||
{/* Main Content - Three Panel Layout */}
|
||||
<div className="min-h-0 flex-1">
|
||||
@@ -423,7 +470,7 @@ export const WizardInterface = React.memo(function WizardInterface({
|
||||
onExecuteAction={handleExecuteAction}
|
||||
onExecuteRobotAction={handleExecuteRobotAction}
|
||||
studyId={trial.experiment.studyId}
|
||||
_isConnected={true}
|
||||
_isConnected={rosConnected}
|
||||
activeTab={controlPanelTab}
|
||||
onTabChange={setControlPanelTab}
|
||||
isStarting={startTrialMutation.isPending}
|
||||
@@ -446,10 +493,17 @@ export const WizardInterface = React.memo(function WizardInterface({
|
||||
<WizardMonitoringPanel
|
||||
trial={trial}
|
||||
trialEvents={trialEvents}
|
||||
isConnected={true}
|
||||
isConnected={rosConnected}
|
||||
wsError={undefined}
|
||||
activeTab={monitoringPanelTab}
|
||||
onTabChange={setMonitoringPanelTab}
|
||||
rosConnected={rosConnected}
|
||||
rosConnecting={rosConnecting}
|
||||
rosError={rosError ?? undefined}
|
||||
robotStatus={robotStatus}
|
||||
connectRos={connectRos}
|
||||
disconnectRos={disconnectRos}
|
||||
executeRosAction={executeRosAction}
|
||||
/>
|
||||
}
|
||||
showDividers={true}
|
||||
|
||||
@@ -13,6 +13,9 @@ import {
|
||||
Power,
|
||||
PowerOff,
|
||||
Eye,
|
||||
Volume2,
|
||||
Move,
|
||||
Hand,
|
||||
} from "lucide-react";
|
||||
import { Badge } from "~/components/ui/badge";
|
||||
import { Separator } from "~/components/ui/separator";
|
||||
@@ -61,6 +64,25 @@ interface WizardMonitoringPanelProps {
|
||||
wsError?: string;
|
||||
activeTab: "status" | "robot" | "events";
|
||||
onTabChange: (tab: "status" | "robot" | "events") => void;
|
||||
// ROS connection props
|
||||
rosConnected: boolean;
|
||||
rosConnecting: boolean;
|
||||
rosError?: string;
|
||||
robotStatus: {
|
||||
connected: boolean;
|
||||
battery: number;
|
||||
position: { x: number; y: number; theta: number };
|
||||
joints: Record<string, unknown>;
|
||||
sensors: Record<string, unknown>;
|
||||
lastUpdate: Date;
|
||||
};
|
||||
connectRos: () => Promise<void>;
|
||||
disconnectRos: () => void;
|
||||
executeRosAction: (
|
||||
pluginName: string,
|
||||
actionId: string,
|
||||
parameters: Record<string, unknown>,
|
||||
) => Promise<unknown>;
|
||||
}
|
||||
|
||||
const WizardMonitoringPanel = React.memo(function WizardMonitoringPanel({
|
||||
@@ -70,331 +92,15 @@ const WizardMonitoringPanel = React.memo(function WizardMonitoringPanel({
|
||||
wsError,
|
||||
activeTab,
|
||||
onTabChange,
|
||||
rosConnected,
|
||||
rosConnecting,
|
||||
rosError,
|
||||
robotStatus,
|
||||
connectRos,
|
||||
disconnectRos,
|
||||
executeRosAction,
|
||||
}: WizardMonitoringPanelProps) {
|
||||
// ROS Bridge connection state
|
||||
const [rosConnected, setRosConnected] = useState(false);
|
||||
const [rosConnecting, setRosConnecting] = useState(false);
|
||||
const [rosError, setRosError] = useState<string | null>(null);
|
||||
const [rosSocket, setRosSocket] = useState<WebSocket | null>(null);
|
||||
const [robotStatus, setRobotStatus] = useState({
|
||||
connected: false,
|
||||
battery: 0,
|
||||
position: { x: 0, y: 0, theta: 0 },
|
||||
joints: {},
|
||||
sensors: {},
|
||||
lastUpdate: new Date(),
|
||||
});
|
||||
|
||||
const ROS_BRIDGE_URL = "ws://134.82.159.25:9090";
|
||||
|
||||
// Use refs to persist connection state across re-renders
|
||||
const connectionAttemptRef = useRef(false);
|
||||
const socketRef = useRef<WebSocket | null>(null);
|
||||
|
||||
const connectRos = () => {
|
||||
// Prevent multiple connection attempts
|
||||
if (connectionAttemptRef.current) {
|
||||
console.log("Connection already in progress, skipping");
|
||||
return;
|
||||
}
|
||||
|
||||
if (
|
||||
rosSocket?.readyState === WebSocket.OPEN ||
|
||||
socketRef.current?.readyState === WebSocket.OPEN
|
||||
) {
|
||||
console.log("Already connected, skipping");
|
||||
return;
|
||||
}
|
||||
|
||||
// Prevent rapid reconnection attempts
|
||||
if (rosConnecting) {
|
||||
console.log("Connection in progress, please wait");
|
||||
return;
|
||||
}
|
||||
|
||||
connectionAttemptRef.current = true;
|
||||
setRosConnecting(true);
|
||||
setRosError(null);
|
||||
|
||||
console.log("🔌 Connecting to ROS Bridge:", ROS_BRIDGE_URL);
|
||||
const socket = new WebSocket(ROS_BRIDGE_URL);
|
||||
socketRef.current = socket;
|
||||
|
||||
// Add connection timeout
|
||||
const connectionTimeout = setTimeout(() => {
|
||||
if (socket.readyState === WebSocket.CONNECTING) {
|
||||
socket.close();
|
||||
connectionAttemptRef.current = false;
|
||||
setRosConnecting(false);
|
||||
setRosError("Connection timeout (10s) - ROS Bridge not responding");
|
||||
}
|
||||
}, 10000);
|
||||
|
||||
socket.onopen = () => {
|
||||
clearTimeout(connectionTimeout);
|
||||
connectionAttemptRef.current = false;
|
||||
console.log("Connected to ROS Bridge successfully");
|
||||
setRosConnected(true);
|
||||
setRosConnecting(false);
|
||||
setRosSocket(socket);
|
||||
setRosError(null);
|
||||
|
||||
// Just log connection success - no auto actions
|
||||
console.log("WebSocket connected successfully to ROS Bridge");
|
||||
|
||||
setRobotStatus((prev) => ({
|
||||
...prev,
|
||||
connected: true,
|
||||
lastUpdate: new Date(),
|
||||
}));
|
||||
};
|
||||
|
||||
socket.onmessage = (event) => {
|
||||
try {
|
||||
const data = JSON.parse(event.data as string) as {
|
||||
topic?: string;
|
||||
msg?: Record<string, unknown>;
|
||||
op?: string;
|
||||
level?: string;
|
||||
};
|
||||
|
||||
// Handle status messages
|
||||
if (data.op === "status") {
|
||||
console.log("ROS Bridge status:", data.msg, "Level:", data.level);
|
||||
return;
|
||||
}
|
||||
|
||||
// Handle topic messages
|
||||
if (data.topic === "/joint_states" && data.msg) {
|
||||
setRobotStatus((prev) => ({
|
||||
...prev,
|
||||
joints: data.msg ?? {},
|
||||
lastUpdate: new Date(),
|
||||
}));
|
||||
} else if (data.topic === "/naoqi_driver/battery" && data.msg) {
|
||||
const batteryPercent = (data.msg.percentage as number) || 0;
|
||||
setRobotStatus((prev) => ({
|
||||
...prev,
|
||||
battery: Math.round(batteryPercent),
|
||||
lastUpdate: new Date(),
|
||||
}));
|
||||
} else if (data.topic === "/diagnostics" && data.msg) {
|
||||
// Handle diagnostic messages for battery
|
||||
console.log("Diagnostics received:", data.msg);
|
||||
}
|
||||
} catch (error) {
|
||||
console.error("Error parsing ROS message:", error);
|
||||
}
|
||||
};
|
||||
|
||||
socket.onclose = (event) => {
|
||||
clearTimeout(connectionTimeout);
|
||||
connectionAttemptRef.current = false;
|
||||
setRosConnected(false);
|
||||
setRosConnecting(false);
|
||||
setRosSocket(null);
|
||||
socketRef.current = null;
|
||||
setRobotStatus((prev) => ({
|
||||
...prev,
|
||||
connected: false,
|
||||
battery: 0,
|
||||
joints: {},
|
||||
sensors: {},
|
||||
}));
|
||||
|
||||
// Only show error if it wasn't a normal closure (code 1000)
|
||||
if (event.code !== 1000) {
|
||||
let errorMsg = "Connection lost";
|
||||
if (event.code === 1006) {
|
||||
errorMsg =
|
||||
"ROS Bridge not responding - check if rosbridge_server is running";
|
||||
} else if (event.code === 1011) {
|
||||
errorMsg = "Server error in ROS Bridge";
|
||||
} else if (event.code === 1002) {
|
||||
errorMsg = "Protocol error - check ROS Bridge version";
|
||||
} else if (event.code === 1001) {
|
||||
errorMsg = "Server going away - ROS Bridge may have restarted";
|
||||
}
|
||||
console.log(
|
||||
`🔌 Connection closed - Code: ${event.code}, Reason: ${event.reason}`,
|
||||
);
|
||||
setRosError(`${errorMsg} (${event.code})`);
|
||||
}
|
||||
};
|
||||
|
||||
socket.onerror = (error) => {
|
||||
clearTimeout(connectionTimeout);
|
||||
connectionAttemptRef.current = false;
|
||||
console.error("ROS Bridge WebSocket error:", error);
|
||||
setRosConnected(false);
|
||||
setRosConnecting(false);
|
||||
setRosError(
|
||||
"Failed to connect to ROS bridge - check if rosbridge_server is running",
|
||||
);
|
||||
setRobotStatus((prev) => ({ ...prev, connected: false }));
|
||||
};
|
||||
};
|
||||
|
||||
const disconnectRos = () => {
|
||||
console.log("Manually disconnecting from ROS Bridge");
|
||||
connectionAttemptRef.current = false;
|
||||
if (rosSocket) {
|
||||
// Close with normal closure code to avoid error messages
|
||||
rosSocket.close(1000, "User disconnected");
|
||||
}
|
||||
if (socketRef.current) {
|
||||
socketRef.current.close(1000, "User disconnected");
|
||||
}
|
||||
// Clear all state
|
||||
setRosSocket(null);
|
||||
socketRef.current = null;
|
||||
setRosConnected(false);
|
||||
setRosConnecting(false);
|
||||
setRosError(null);
|
||||
setRobotStatus({
|
||||
connected: false,
|
||||
battery: 0,
|
||||
position: { x: 0, y: 0, theta: 0 },
|
||||
joints: {},
|
||||
sensors: {},
|
||||
lastUpdate: new Date(),
|
||||
});
|
||||
};
|
||||
|
||||
const executeRobotAction = (
|
||||
action: string,
|
||||
parameters?: Record<string, unknown>,
|
||||
) => {
|
||||
if (!rosSocket || !rosConnected) {
|
||||
setRosError("Robot not connected");
|
||||
return;
|
||||
}
|
||||
|
||||
let message: {
|
||||
op: string;
|
||||
topic: string;
|
||||
type: string;
|
||||
msg: Record<string, unknown>;
|
||||
};
|
||||
|
||||
switch (action) {
|
||||
case "say_text":
|
||||
const speechText = parameters?.text ?? "Hello from wizard interface!";
|
||||
console.log("🔊 Preparing speech command:", speechText);
|
||||
message = {
|
||||
op: "publish",
|
||||
topic: "/speech",
|
||||
type: "std_msgs/String",
|
||||
msg: { data: speechText },
|
||||
};
|
||||
console.log(
|
||||
"📤 Speech message constructed:",
|
||||
JSON.stringify(message, null, 2),
|
||||
);
|
||||
break;
|
||||
|
||||
case "move_forward":
|
||||
case "move_backward":
|
||||
case "turn_left":
|
||||
case "turn_right":
|
||||
const speed = (parameters?.speed as number) || 0.1;
|
||||
const linear = action.includes("forward")
|
||||
? speed
|
||||
: action.includes("backward")
|
||||
? -speed
|
||||
: 0;
|
||||
const angular = action.includes("left")
|
||||
? speed
|
||||
: action.includes("right")
|
||||
? -speed
|
||||
: 0;
|
||||
|
||||
message = {
|
||||
op: "publish",
|
||||
topic: "/cmd_vel",
|
||||
type: "geometry_msgs/Twist",
|
||||
msg: {
|
||||
linear: { x: linear, y: 0, z: 0 },
|
||||
angular: { x: 0, y: 0, z: angular },
|
||||
},
|
||||
};
|
||||
break;
|
||||
|
||||
case "stop_movement":
|
||||
message = {
|
||||
op: "publish",
|
||||
topic: "/cmd_vel",
|
||||
type: "geometry_msgs/Twist",
|
||||
msg: {
|
||||
linear: { x: 0, y: 0, z: 0 },
|
||||
angular: { x: 0, y: 0, z: 0 },
|
||||
},
|
||||
};
|
||||
break;
|
||||
|
||||
case "head_movement":
|
||||
case "turn_head":
|
||||
const yaw = (parameters?.yaw as number) || 0;
|
||||
const pitch = (parameters?.pitch as number) || 0;
|
||||
const headSpeed = (parameters?.speed as number) || 0.3;
|
||||
|
||||
message = {
|
||||
op: "publish",
|
||||
topic: "/joint_angles",
|
||||
type: "naoqi_bridge_msgs/JointAnglesWithSpeed",
|
||||
msg: {
|
||||
joint_names: ["HeadYaw", "HeadPitch"],
|
||||
joint_angles: [yaw, pitch],
|
||||
speed: headSpeed,
|
||||
},
|
||||
};
|
||||
break;
|
||||
|
||||
case "play_animation":
|
||||
const animation = (parameters?.animation as string) ?? "Hello";
|
||||
|
||||
message = {
|
||||
op: "publish",
|
||||
topic: "/naoqi_driver/animation",
|
||||
type: "std_msgs/String",
|
||||
msg: { data: animation },
|
||||
};
|
||||
break;
|
||||
|
||||
default:
|
||||
setRosError(`Unknown action: ${String(action)}`);
|
||||
return;
|
||||
}
|
||||
|
||||
try {
|
||||
const messageStr = JSON.stringify(message);
|
||||
console.log("📡 Sending to ROS Bridge:", messageStr);
|
||||
rosSocket.send(messageStr);
|
||||
console.log(`✅ Sent robot action: ${action}`, parameters);
|
||||
} catch (error) {
|
||||
console.error("❌ Failed to send command:", error);
|
||||
setRosError(`Failed to send command: ${String(error)}`);
|
||||
}
|
||||
};
|
||||
|
||||
const subscribeToTopic = (topic: string, messageType: string) => {
|
||||
if (!rosSocket || !rosConnected) {
|
||||
setRosError("Cannot subscribe - not connected");
|
||||
return;
|
||||
}
|
||||
|
||||
try {
|
||||
const subscribeMsg = {
|
||||
op: "subscribe",
|
||||
topic: topic,
|
||||
type: messageType,
|
||||
};
|
||||
rosSocket.send(JSON.stringify(subscribeMsg));
|
||||
console.log(`Manually subscribed to ${topic}`);
|
||||
} catch (error) {
|
||||
setRosError(`Failed to subscribe to ${topic}: ${String(error)}`);
|
||||
}
|
||||
};
|
||||
// ROS connection is now passed as props, no need for separate hook
|
||||
|
||||
// Don't close connection on unmount to prevent disconnection issues
|
||||
// Connection will persist across component re-renders
|
||||
@@ -526,7 +232,7 @@ const WizardMonitoringPanel = React.memo(function WizardMonitoringPanel({
|
||||
<div className="space-y-2">
|
||||
<div className="flex items-center justify-between">
|
||||
<span className="text-muted-foreground text-xs">
|
||||
WebSocket
|
||||
ROS Bridge
|
||||
</span>
|
||||
<Badge
|
||||
variant={isConnected ? "default" : "secondary"}
|
||||
@@ -759,7 +465,7 @@ const WizardMonitoringPanel = React.memo(function WizardMonitoringPanel({
|
||||
size="sm"
|
||||
variant="outline"
|
||||
className="w-full text-xs"
|
||||
onClick={connectRos}
|
||||
onClick={() => connectRos()}
|
||||
disabled={rosConnecting || rosConnected}
|
||||
>
|
||||
<Bot className="mr-1 h-3 w-3" />
|
||||
@@ -774,7 +480,7 @@ const WizardMonitoringPanel = React.memo(function WizardMonitoringPanel({
|
||||
size="sm"
|
||||
variant="outline"
|
||||
className="w-full text-xs"
|
||||
onClick={disconnectRos}
|
||||
onClick={() => disconnectRos()}
|
||||
>
|
||||
<PowerOff className="mr-1 h-3 w-3" />
|
||||
Disconnect
|
||||
@@ -801,7 +507,7 @@ const WizardMonitoringPanel = React.memo(function WizardMonitoringPanel({
|
||||
<div>
|
||||
1. Check ROS Bridge:{" "}
|
||||
<code className="bg-muted rounded px-1 text-xs">
|
||||
telnet 134.82.159.25 9090
|
||||
telnet localhost 9090
|
||||
</code>
|
||||
</div>
|
||||
<div>2. NAO6 must be awake and connected</div>
|
||||
@@ -850,10 +556,10 @@ const WizardMonitoringPanel = React.memo(function WizardMonitoringPanel({
|
||||
))}
|
||||
{trialEvents.filter((e) => e.type.includes("robot"))
|
||||
.length === 0 && (
|
||||
<div className="text-muted-foreground py-2 text-center text-xs">
|
||||
No robot events yet
|
||||
</div>
|
||||
)}
|
||||
<div className="text-muted-foreground py-2 text-center text-xs">
|
||||
No robot events yet
|
||||
</div>
|
||||
)}
|
||||
</div>
|
||||
</div>
|
||||
|
||||
@@ -905,15 +611,17 @@ const WizardMonitoringPanel = React.memo(function WizardMonitoringPanel({
|
||||
<Button
|
||||
size="sm"
|
||||
variant="outline"
|
||||
className="text-xs"
|
||||
onClick={() =>
|
||||
executeRobotAction("say_text", {
|
||||
text: "Connection test - can you hear me?",
|
||||
})
|
||||
}
|
||||
onClick={() => {
|
||||
if (rosConnected) {
|
||||
executeRosAction("nao6-ros2", "say_text", {
|
||||
text: "Connection test - can you hear me?",
|
||||
}).catch(console.error);
|
||||
}
|
||||
}}
|
||||
disabled={!rosConnected}
|
||||
>
|
||||
{rosConnected ? "🔊 Test Speech" : "🔊 Not Ready"}
|
||||
<Volume2 className="mr-1 h-3 w-3" />
|
||||
Test Speech
|
||||
</Button>
|
||||
</div>
|
||||
|
||||
@@ -923,45 +631,9 @@ const WizardMonitoringPanel = React.memo(function WizardMonitoringPanel({
|
||||
Subscribe to Topics:
|
||||
</div>
|
||||
<div className="grid grid-cols-1 gap-1">
|
||||
<Button
|
||||
size="sm"
|
||||
variant="ghost"
|
||||
className="justify-start text-xs"
|
||||
onClick={() =>
|
||||
subscribeToTopic(
|
||||
"/naoqi_driver/battery",
|
||||
"naoqi_bridge_msgs/Battery",
|
||||
)
|
||||
}
|
||||
>
|
||||
🔋 Battery Status
|
||||
</Button>
|
||||
<Button
|
||||
size="sm"
|
||||
variant="ghost"
|
||||
className="justify-start text-xs"
|
||||
onClick={() =>
|
||||
subscribeToTopic(
|
||||
"/naoqi_driver/joint_states",
|
||||
"sensor_msgs/JointState",
|
||||
)
|
||||
}
|
||||
>
|
||||
🤖 Joint States
|
||||
</Button>
|
||||
<Button
|
||||
size="sm"
|
||||
variant="ghost"
|
||||
className="justify-start text-xs"
|
||||
onClick={() =>
|
||||
subscribeToTopic(
|
||||
"/naoqi_driver/bumper",
|
||||
"naoqi_bridge_msgs/Bumper",
|
||||
)
|
||||
}
|
||||
>
|
||||
👟 Bumper Sensors
|
||||
</Button>
|
||||
<div className="text-muted-foreground text-xs">
|
||||
Subscriptions managed automatically
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
@@ -978,9 +650,14 @@ const WizardMonitoringPanel = React.memo(function WizardMonitoringPanel({
|
||||
size="sm"
|
||||
variant="outline"
|
||||
className="text-xs"
|
||||
onClick={() =>
|
||||
executeRobotAction("move_forward", { speed: 0.05 })
|
||||
}
|
||||
onClick={() => {
|
||||
if (rosConnected) {
|
||||
executeRosAction("nao6-ros2", "walk_forward", {
|
||||
speed: 0.05,
|
||||
duration: 2,
|
||||
}).catch(console.error);
|
||||
}
|
||||
}}
|
||||
>
|
||||
Forward
|
||||
</Button>
|
||||
@@ -988,9 +665,14 @@ const WizardMonitoringPanel = React.memo(function WizardMonitoringPanel({
|
||||
size="sm"
|
||||
variant="outline"
|
||||
className="text-xs"
|
||||
onClick={() =>
|
||||
executeRobotAction("turn_left", { speed: 0.3 })
|
||||
}
|
||||
onClick={() => {
|
||||
if (rosConnected) {
|
||||
executeRosAction("nao6-ros2", "turn_left", {
|
||||
speed: 0.3,
|
||||
duration: 2,
|
||||
}).catch(console.error);
|
||||
}
|
||||
}}
|
||||
>
|
||||
Turn Left
|
||||
</Button>
|
||||
@@ -998,9 +680,14 @@ const WizardMonitoringPanel = React.memo(function WizardMonitoringPanel({
|
||||
size="sm"
|
||||
variant="outline"
|
||||
className="text-xs"
|
||||
onClick={() =>
|
||||
executeRobotAction("turn_right", { speed: 0.3 })
|
||||
}
|
||||
onClick={() => {
|
||||
if (rosConnected) {
|
||||
executeRosAction("nao6-ros2", "turn_right", {
|
||||
speed: 0.3,
|
||||
duration: 2,
|
||||
}).catch(console.error);
|
||||
}
|
||||
}}
|
||||
>
|
||||
Turn Right
|
||||
</Button>
|
||||
@@ -1012,13 +699,15 @@ const WizardMonitoringPanel = React.memo(function WizardMonitoringPanel({
|
||||
size="sm"
|
||||
variant="outline"
|
||||
className="text-xs"
|
||||
onClick={() =>
|
||||
executeRobotAction("turn_head", {
|
||||
yaw: 0,
|
||||
pitch: 0,
|
||||
speed: 0.3,
|
||||
})
|
||||
}
|
||||
onClick={() => {
|
||||
if (rosConnected) {
|
||||
executeRosAction("nao6-ros2", "turn_head", {
|
||||
yaw: 0,
|
||||
pitch: 0,
|
||||
speed: 0.3,
|
||||
}).catch(console.error);
|
||||
}
|
||||
}}
|
||||
>
|
||||
Center Head
|
||||
</Button>
|
||||
@@ -1026,13 +715,15 @@ const WizardMonitoringPanel = React.memo(function WizardMonitoringPanel({
|
||||
size="sm"
|
||||
variant="outline"
|
||||
className="text-xs"
|
||||
onClick={() =>
|
||||
executeRobotAction("turn_head", {
|
||||
yaw: 0.5,
|
||||
pitch: 0,
|
||||
speed: 0.3,
|
||||
})
|
||||
}
|
||||
onClick={() => {
|
||||
if (rosConnected) {
|
||||
executeRosAction("nao6-ros2", "turn_head", {
|
||||
yaw: 0.5,
|
||||
pitch: 0,
|
||||
speed: 0.3,
|
||||
}).catch(console.error);
|
||||
}
|
||||
}}
|
||||
>
|
||||
Look Left
|
||||
</Button>
|
||||
@@ -1040,13 +731,15 @@ const WizardMonitoringPanel = React.memo(function WizardMonitoringPanel({
|
||||
size="sm"
|
||||
variant="outline"
|
||||
className="text-xs"
|
||||
onClick={() =>
|
||||
executeRobotAction("turn_head", {
|
||||
yaw: -0.5,
|
||||
pitch: 0,
|
||||
speed: 0.3,
|
||||
})
|
||||
}
|
||||
onClick={() => {
|
||||
if (rosConnected) {
|
||||
executeRosAction("nao6-ros2", "turn_head", {
|
||||
yaw: -0.5,
|
||||
pitch: 0,
|
||||
speed: 0.3,
|
||||
}).catch(console.error);
|
||||
}
|
||||
}}
|
||||
>
|
||||
Look Right
|
||||
</Button>
|
||||
@@ -1058,23 +751,27 @@ const WizardMonitoringPanel = React.memo(function WizardMonitoringPanel({
|
||||
size="sm"
|
||||
variant="outline"
|
||||
className="text-xs"
|
||||
onClick={() =>
|
||||
executeRobotAction("play_animation", {
|
||||
animation: "Hello",
|
||||
})
|
||||
}
|
||||
onClick={() => {
|
||||
if (rosConnected) {
|
||||
executeRosAction("nao6-ros2", "say_text", {
|
||||
text: "Hello! I am NAO!",
|
||||
}).catch(console.error);
|
||||
}
|
||||
}}
|
||||
>
|
||||
Wave Hello
|
||||
Say Hello
|
||||
</Button>
|
||||
<Button
|
||||
size="sm"
|
||||
variant="outline"
|
||||
className="text-xs"
|
||||
onClick={() =>
|
||||
executeRobotAction("say_text", {
|
||||
text: "Experiment ready!",
|
||||
})
|
||||
}
|
||||
onClick={() => {
|
||||
if (rosConnected) {
|
||||
executeRosAction("nao6-ros2", "say_text", {
|
||||
text: "Experiment ready!",
|
||||
}).catch(console.error);
|
||||
}
|
||||
}}
|
||||
>
|
||||
Say Ready
|
||||
</Button>
|
||||
@@ -1086,7 +783,15 @@ const WizardMonitoringPanel = React.memo(function WizardMonitoringPanel({
|
||||
size="sm"
|
||||
variant="destructive"
|
||||
className="text-xs"
|
||||
onClick={() => executeRobotAction("stop_movement", {})}
|
||||
onClick={() => {
|
||||
if (rosConnected) {
|
||||
executeRosAction(
|
||||
"nao6-ros2",
|
||||
"emergency_stop",
|
||||
{},
|
||||
).catch(console.error);
|
||||
}
|
||||
}}
|
||||
>
|
||||
🛑 Emergency Stop
|
||||
</Button>
|
||||
|
||||
296
src/hooks/useWizardRos.ts
Normal file
296
src/hooks/useWizardRos.ts
Normal file
@@ -0,0 +1,296 @@
|
||||
"use client";
|
||||
|
||||
import { useEffect, useState, useCallback, useRef } from "react";
|
||||
import {
|
||||
WizardRosService,
|
||||
type RobotStatus,
|
||||
type RobotActionExecution,
|
||||
getWizardRosService,
|
||||
} from "~/lib/ros/wizard-ros-service";
|
||||
|
||||
export interface UseWizardRosOptions {
|
||||
autoConnect?: boolean;
|
||||
onConnected?: () => void;
|
||||
onDisconnected?: () => void;
|
||||
onError?: (error: unknown) => void;
|
||||
onActionCompleted?: (execution: RobotActionExecution) => void;
|
||||
onActionFailed?: (execution: RobotActionExecution) => void;
|
||||
}
|
||||
|
||||
export interface UseWizardRosReturn {
|
||||
isConnected: boolean;
|
||||
isConnecting: boolean;
|
||||
connectionError: string | null;
|
||||
robotStatus: RobotStatus;
|
||||
activeActions: RobotActionExecution[];
|
||||
connect: () => Promise<void>;
|
||||
disconnect: () => void;
|
||||
executeRobotAction: (
|
||||
pluginName: string,
|
||||
actionId: string,
|
||||
parameters: Record<string, unknown>,
|
||||
actionConfig?: {
|
||||
topic: string;
|
||||
messageType: string;
|
||||
payloadMapping: {
|
||||
type: string;
|
||||
payload?: Record<string, unknown>;
|
||||
transformFn?: string;
|
||||
};
|
||||
},
|
||||
) => Promise<RobotActionExecution>;
|
||||
}
|
||||
|
||||
export function useWizardRos(
|
||||
options: UseWizardRosOptions = {},
|
||||
): UseWizardRosReturn {
|
||||
const {
|
||||
autoConnect = true,
|
||||
onConnected,
|
||||
onDisconnected,
|
||||
onError,
|
||||
onActionCompleted,
|
||||
onActionFailed,
|
||||
} = options;
|
||||
|
||||
const [isConnected, setIsConnected] = useState(false);
|
||||
const [isConnecting, setIsConnecting] = useState(false);
|
||||
const [connectionError, setConnectionError] = useState<string | null>(null);
|
||||
const [robotStatus, setRobotStatus] = useState<RobotStatus>({
|
||||
connected: false,
|
||||
battery: 0,
|
||||
position: { x: 0, y: 0, theta: 0 },
|
||||
joints: {},
|
||||
sensors: {},
|
||||
lastUpdate: new Date(),
|
||||
});
|
||||
const [activeActions, setActiveActions] = useState<RobotActionExecution[]>(
|
||||
[],
|
||||
);
|
||||
|
||||
// Prevent multiple connections
|
||||
const isInitializedRef = useRef(false);
|
||||
const connectAttemptRef = useRef(false);
|
||||
|
||||
const serviceRef = useRef<WizardRosService | null>(null);
|
||||
const mountedRef = useRef(true);
|
||||
|
||||
// Use refs for callbacks to prevent infinite re-renders
|
||||
const onConnectedRef = useRef(onConnected);
|
||||
const onDisconnectedRef = useRef(onDisconnected);
|
||||
const onErrorRef = useRef(onError);
|
||||
const onActionCompletedRef = useRef(onActionCompleted);
|
||||
const onActionFailedRef = useRef(onActionFailed);
|
||||
|
||||
// Update refs when callbacks change
|
||||
onConnectedRef.current = onConnected;
|
||||
onDisconnectedRef.current = onDisconnected;
|
||||
onErrorRef.current = onError;
|
||||
onActionCompletedRef.current = onActionCompleted;
|
||||
onActionFailedRef.current = onActionFailed;
|
||||
|
||||
// Initialize service (only once)
|
||||
useEffect(() => {
|
||||
if (!isInitializedRef.current) {
|
||||
serviceRef.current = getWizardRosService();
|
||||
isInitializedRef.current = true;
|
||||
}
|
||||
|
||||
return () => {
|
||||
mountedRef.current = false;
|
||||
};
|
||||
}, []);
|
||||
|
||||
// Set up event listeners with stable callbacks
|
||||
useEffect(() => {
|
||||
const service = serviceRef.current;
|
||||
if (!service) return;
|
||||
|
||||
const handleConnected = () => {
|
||||
if (!mountedRef.current) return;
|
||||
console.log("[useWizardRos] Connected to ROS bridge");
|
||||
setIsConnected(true);
|
||||
setIsConnecting(false);
|
||||
setConnectionError(null);
|
||||
onConnectedRef.current?.();
|
||||
};
|
||||
|
||||
const handleDisconnected = () => {
|
||||
if (!mountedRef.current) return;
|
||||
console.log("[useWizardRos] Disconnected from ROS bridge");
|
||||
setIsConnected(false);
|
||||
setIsConnecting(false);
|
||||
onDisconnectedRef.current?.();
|
||||
};
|
||||
|
||||
const handleError = (error: unknown) => {
|
||||
if (!mountedRef.current) return;
|
||||
console.error("[useWizardRos] ROS connection error:", error);
|
||||
setConnectionError(
|
||||
error instanceof Error ? error.message : "Connection error",
|
||||
);
|
||||
setIsConnecting(false);
|
||||
onErrorRef.current?.(error);
|
||||
};
|
||||
|
||||
const handleRobotStatusUpdated = (status: RobotStatus) => {
|
||||
if (!mountedRef.current) return;
|
||||
setRobotStatus(status);
|
||||
};
|
||||
|
||||
const handleActionStarted = (execution: RobotActionExecution) => {
|
||||
if (!mountedRef.current) return;
|
||||
setActiveActions((prev) => {
|
||||
const filtered = prev.filter((action) => action.id !== execution.id);
|
||||
return [...filtered, execution];
|
||||
});
|
||||
};
|
||||
|
||||
const handleActionCompleted = (execution: RobotActionExecution) => {
|
||||
if (!mountedRef.current) return;
|
||||
setActiveActions((prev) =>
|
||||
prev.map((action) => (action.id === execution.id ? execution : action)),
|
||||
);
|
||||
onActionCompletedRef.current?.(execution);
|
||||
};
|
||||
|
||||
const handleActionFailed = (execution: RobotActionExecution) => {
|
||||
if (!mountedRef.current) return;
|
||||
setActiveActions((prev) =>
|
||||
prev.map((action) => (action.id === execution.id ? execution : action)),
|
||||
);
|
||||
onActionFailedRef.current?.(execution);
|
||||
};
|
||||
|
||||
const handleMaxReconnectsReached = () => {
|
||||
if (!mountedRef.current) return;
|
||||
setConnectionError("Maximum reconnection attempts reached");
|
||||
setIsConnecting(false);
|
||||
};
|
||||
|
||||
// Add event listeners
|
||||
service.on("connected", handleConnected);
|
||||
service.on("disconnected", handleDisconnected);
|
||||
service.on("error", handleError);
|
||||
service.on("robot_status_updated", handleRobotStatusUpdated);
|
||||
service.on("action_started", handleActionStarted);
|
||||
service.on("action_completed", handleActionCompleted);
|
||||
service.on("action_failed", handleActionFailed);
|
||||
service.on("max_reconnects_reached", handleMaxReconnectsReached);
|
||||
|
||||
// Initialize connection status
|
||||
setIsConnected(service.getConnectionStatus());
|
||||
setRobotStatus(service.getRobotStatus());
|
||||
setActiveActions(service.getActiveActions());
|
||||
|
||||
return () => {
|
||||
service.off("connected", handleConnected);
|
||||
service.off("disconnected", handleDisconnected);
|
||||
service.off("error", handleError);
|
||||
service.off("robot_status_updated", handleRobotStatusUpdated);
|
||||
service.off("action_started", handleActionStarted);
|
||||
service.off("action_completed", handleActionCompleted);
|
||||
service.off("action_failed", handleActionFailed);
|
||||
service.off("max_reconnects_reached", handleMaxReconnectsReached);
|
||||
};
|
||||
}, []); // Empty deps since we use refs
|
||||
|
||||
const connect = useCallback(async (): Promise<void> => {
|
||||
const service = serviceRef.current;
|
||||
if (!service || isConnected || isConnecting || connectAttemptRef.current)
|
||||
return;
|
||||
|
||||
connectAttemptRef.current = true;
|
||||
setIsConnecting(true);
|
||||
setConnectionError(null);
|
||||
|
||||
try {
|
||||
await service.connect();
|
||||
} catch (error) {
|
||||
if (mountedRef.current) {
|
||||
setIsConnecting(false);
|
||||
setConnectionError(
|
||||
error instanceof Error ? error.message : "Connection failed",
|
||||
);
|
||||
}
|
||||
throw error;
|
||||
} finally {
|
||||
connectAttemptRef.current = false;
|
||||
}
|
||||
}, [isConnected, isConnecting]);
|
||||
|
||||
// Auto-connect if enabled (only once per hook instance)
|
||||
useEffect(() => {
|
||||
if (
|
||||
autoConnect &&
|
||||
serviceRef.current &&
|
||||
!isConnected &&
|
||||
!isConnecting &&
|
||||
!connectAttemptRef.current
|
||||
) {
|
||||
const timeoutId = setTimeout(() => {
|
||||
connect().catch((error) => {
|
||||
console.error("[useWizardRos] Auto-connect failed:", error);
|
||||
});
|
||||
}, 100); // Small delay to prevent immediate connection attempts
|
||||
|
||||
return () => clearTimeout(timeoutId);
|
||||
}
|
||||
}, [autoConnect, isConnected, isConnecting, connect]);
|
||||
|
||||
const disconnect = useCallback((): void => {
|
||||
const service = serviceRef.current;
|
||||
if (!service) return;
|
||||
|
||||
connectAttemptRef.current = false;
|
||||
service.disconnect();
|
||||
setIsConnected(false);
|
||||
setIsConnecting(false);
|
||||
setConnectionError(null);
|
||||
}, []);
|
||||
|
||||
const executeRobotAction = useCallback(
|
||||
async (
|
||||
pluginName: string,
|
||||
actionId: string,
|
||||
parameters: Record<string, unknown>,
|
||||
actionConfig?: {
|
||||
topic: string;
|
||||
messageType: string;
|
||||
payloadMapping: {
|
||||
type: string;
|
||||
payload?: Record<string, unknown>;
|
||||
transformFn?: string;
|
||||
};
|
||||
},
|
||||
): Promise<RobotActionExecution> => {
|
||||
const service = serviceRef.current;
|
||||
if (!service) {
|
||||
throw new Error("ROS service not initialized");
|
||||
}
|
||||
|
||||
if (!isConnected) {
|
||||
throw new Error("Not connected to ROS bridge");
|
||||
}
|
||||
|
||||
return service.executeRobotAction(
|
||||
pluginName,
|
||||
actionId,
|
||||
parameters,
|
||||
actionConfig,
|
||||
);
|
||||
},
|
||||
[isConnected],
|
||||
);
|
||||
|
||||
return {
|
||||
isConnected,
|
||||
isConnecting,
|
||||
connectionError,
|
||||
robotStatus,
|
||||
activeActions,
|
||||
connect,
|
||||
disconnect,
|
||||
executeRobotAction,
|
||||
};
|
||||
}
|
||||
671
src/lib/ros/wizard-ros-service.ts
Normal file
671
src/lib/ros/wizard-ros-service.ts
Normal file
@@ -0,0 +1,671 @@
|
||||
"use client";
|
||||
|
||||
import { EventEmitter } from "events";
|
||||
|
||||
export interface RosMessage {
|
||||
op: string;
|
||||
topic?: string;
|
||||
type?: string;
|
||||
msg?: Record<string, unknown>;
|
||||
service?: string;
|
||||
args?: Record<string, unknown>;
|
||||
id?: string;
|
||||
result?: boolean;
|
||||
values?: Record<string, unknown>;
|
||||
}
|
||||
|
||||
export interface RobotStatus {
|
||||
connected: boolean;
|
||||
battery: number;
|
||||
position: { x: number; y: number; theta: number };
|
||||
joints: Record<string, number>;
|
||||
sensors: Record<string, unknown>;
|
||||
lastUpdate: Date;
|
||||
}
|
||||
|
||||
export interface RobotActionExecution {
|
||||
id: string;
|
||||
actionId: string;
|
||||
pluginName: string;
|
||||
parameters: Record<string, unknown>;
|
||||
status: "pending" | "executing" | "completed" | "failed";
|
||||
startTime: Date;
|
||||
endTime?: Date;
|
||||
error?: string;
|
||||
}
|
||||
|
||||
/**
|
||||
* Unified ROS WebSocket service for wizard interface
|
||||
* Manages connection to rosbridge and handles robot action execution
|
||||
*/
|
||||
export class WizardRosService extends EventEmitter {
|
||||
private ws: WebSocket | null = null;
|
||||
private url: string;
|
||||
private reconnectInterval = 3000;
|
||||
private reconnectTimer: NodeJS.Timeout | null = null;
|
||||
private messageId = 0;
|
||||
private isConnected = false;
|
||||
private connectionAttempts = 0;
|
||||
private maxReconnectAttempts = 5;
|
||||
private isConnecting = false;
|
||||
|
||||
// Robot state
|
||||
private robotStatus: RobotStatus = {
|
||||
connected: false,
|
||||
battery: 0,
|
||||
position: { x: 0, y: 0, theta: 0 },
|
||||
joints: {},
|
||||
sensors: {},
|
||||
lastUpdate: new Date(),
|
||||
};
|
||||
|
||||
// Active action tracking
|
||||
private activeActions: Map<string, RobotActionExecution> = new Map();
|
||||
|
||||
constructor(url: string = "ws://localhost:9090") {
|
||||
super();
|
||||
this.url = url;
|
||||
}
|
||||
|
||||
/**
|
||||
* Connect to ROS bridge WebSocket
|
||||
*/
|
||||
async connect(): Promise<void> {
|
||||
return new Promise((resolve, reject) => {
|
||||
if (
|
||||
this.isConnected ||
|
||||
this.ws?.readyState === WebSocket.OPEN ||
|
||||
this.isConnecting
|
||||
) {
|
||||
if (this.isConnected) resolve();
|
||||
return;
|
||||
}
|
||||
|
||||
this.isConnecting = true;
|
||||
console.log(`[WizardROS] Connecting to ${this.url}`);
|
||||
this.ws = new WebSocket(this.url);
|
||||
|
||||
const connectionTimeout = setTimeout(() => {
|
||||
if (this.ws?.readyState !== WebSocket.OPEN) {
|
||||
this.ws?.close();
|
||||
reject(new Error("Connection timeout"));
|
||||
}
|
||||
}, 10000);
|
||||
|
||||
this.ws.onopen = () => {
|
||||
clearTimeout(connectionTimeout);
|
||||
console.log("[WizardROS] Connected successfully");
|
||||
this.isConnected = true;
|
||||
this.isConnecting = false;
|
||||
this.connectionAttempts = 0;
|
||||
this.clearReconnectTimer();
|
||||
|
||||
// Subscribe to robot topics
|
||||
this.subscribeToRobotTopics();
|
||||
|
||||
this.emit("connected");
|
||||
resolve();
|
||||
};
|
||||
|
||||
this.ws.onmessage = (event) => {
|
||||
try {
|
||||
const message = JSON.parse(event.data) as RosMessage;
|
||||
this.handleMessage(message);
|
||||
} catch (error) {
|
||||
console.error("[WizardROS] Failed to parse message:", error);
|
||||
}
|
||||
};
|
||||
|
||||
this.ws.onclose = (event) => {
|
||||
console.log(
|
||||
`[WizardROS] Connection closed: ${event.code} - ${event.reason}`,
|
||||
);
|
||||
this.isConnected = false;
|
||||
this.isConnecting = false;
|
||||
this.emit("disconnected");
|
||||
|
||||
// Schedule reconnect if not manually closed
|
||||
if (
|
||||
event.code !== 1000 &&
|
||||
this.connectionAttempts < this.maxReconnectAttempts
|
||||
) {
|
||||
this.scheduleReconnect();
|
||||
}
|
||||
};
|
||||
|
||||
this.ws.onerror = (error) => {
|
||||
console.error("[WizardROS] WebSocket error:", error);
|
||||
clearTimeout(connectionTimeout);
|
||||
this.isConnecting = false;
|
||||
this.emit("error", error);
|
||||
reject(error);
|
||||
};
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* Disconnect from ROS bridge
|
||||
*/
|
||||
disconnect(): void {
|
||||
this.clearReconnectTimer();
|
||||
|
||||
if (this.ws) {
|
||||
this.ws.close(1000, "Manual disconnect");
|
||||
this.ws = null;
|
||||
}
|
||||
|
||||
this.isConnected = false;
|
||||
this.isConnecting = false;
|
||||
this.robotStatus.connected = false;
|
||||
this.emit("disconnected");
|
||||
}
|
||||
|
||||
/**
|
||||
* Check if connected to ROS bridge
|
||||
*/
|
||||
getConnectionStatus(): boolean {
|
||||
return this.isConnected && this.ws?.readyState === WebSocket.OPEN;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get current robot status
|
||||
*/
|
||||
getRobotStatus(): RobotStatus {
|
||||
return { ...this.robotStatus };
|
||||
}
|
||||
|
||||
/**
|
||||
* Execute robot action using plugin configuration
|
||||
*/
|
||||
async executeRobotAction(
|
||||
pluginName: string,
|
||||
actionId: string,
|
||||
parameters: Record<string, unknown>,
|
||||
actionConfig?: {
|
||||
topic: string;
|
||||
messageType: string;
|
||||
payloadMapping: {
|
||||
type: string;
|
||||
payload?: Record<string, unknown>;
|
||||
transformFn?: string;
|
||||
};
|
||||
},
|
||||
): Promise<RobotActionExecution> {
|
||||
if (!this.isConnected) {
|
||||
throw new Error("Not connected to ROS bridge");
|
||||
}
|
||||
|
||||
const executionId = `${pluginName}_${actionId}_${Date.now()}`;
|
||||
const execution: RobotActionExecution = {
|
||||
id: executionId,
|
||||
actionId,
|
||||
pluginName,
|
||||
parameters,
|
||||
status: "pending",
|
||||
startTime: new Date(),
|
||||
};
|
||||
|
||||
this.activeActions.set(executionId, execution);
|
||||
this.emit("action_started", execution);
|
||||
|
||||
try {
|
||||
execution.status = "executing";
|
||||
this.activeActions.set(executionId, execution);
|
||||
|
||||
// Execute based on action configuration or built-in mappings
|
||||
if (actionConfig) {
|
||||
await this.executeWithConfig(actionConfig, parameters);
|
||||
} else {
|
||||
await this.executeBuiltinAction(actionId, parameters);
|
||||
}
|
||||
|
||||
execution.status = "completed";
|
||||
execution.endTime = new Date();
|
||||
this.emit("action_completed", execution);
|
||||
} catch (error) {
|
||||
execution.status = "failed";
|
||||
execution.error = error instanceof Error ? error.message : String(error);
|
||||
execution.endTime = new Date();
|
||||
this.emit("action_failed", execution);
|
||||
}
|
||||
|
||||
this.activeActions.set(executionId, execution);
|
||||
return execution;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get list of active actions
|
||||
*/
|
||||
getActiveActions(): RobotActionExecution[] {
|
||||
return Array.from(this.activeActions.values());
|
||||
}
|
||||
|
||||
/**
|
||||
* Subscribe to robot sensor topics
|
||||
*/
|
||||
private subscribeToRobotTopics(): void {
|
||||
const topics = [
|
||||
{ topic: "/joint_states", type: "sensor_msgs/JointState" },
|
||||
{
|
||||
topic: "/naoqi_driver/battery",
|
||||
type: "naoqi_bridge_msgs/BatteryState",
|
||||
},
|
||||
{ topic: "/naoqi_driver/bumper", type: "naoqi_bridge_msgs/Bumper" },
|
||||
{
|
||||
topic: "/naoqi_driver/hand_touch",
|
||||
type: "naoqi_bridge_msgs/HandTouch",
|
||||
},
|
||||
{
|
||||
topic: "/naoqi_driver/head_touch",
|
||||
type: "naoqi_bridge_msgs/HeadTouch",
|
||||
},
|
||||
{ topic: "/naoqi_driver/sonar/left", type: "sensor_msgs/Range" },
|
||||
{ topic: "/naoqi_driver/sonar/right", type: "sensor_msgs/Range" },
|
||||
];
|
||||
|
||||
topics.forEach(({ topic, type }) => {
|
||||
this.subscribe(topic, type);
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* Subscribe to a ROS topic
|
||||
*/
|
||||
private subscribe(topic: string, messageType: string): void {
|
||||
const message: RosMessage = {
|
||||
op: "subscribe",
|
||||
topic,
|
||||
type: messageType,
|
||||
id: `sub_${this.messageId++}`,
|
||||
};
|
||||
|
||||
this.send(message);
|
||||
}
|
||||
|
||||
/**
|
||||
* Publish message to ROS topic
|
||||
*/
|
||||
private publish(
|
||||
topic: string,
|
||||
messageType: string,
|
||||
msg: Record<string, unknown>,
|
||||
): void {
|
||||
const message: RosMessage = {
|
||||
op: "publish",
|
||||
topic,
|
||||
type: messageType,
|
||||
msg,
|
||||
};
|
||||
|
||||
this.send(message);
|
||||
}
|
||||
|
||||
/**
|
||||
* Send WebSocket message
|
||||
*/
|
||||
private send(message: RosMessage): void {
|
||||
if (this.ws?.readyState === WebSocket.OPEN) {
|
||||
this.ws.send(JSON.stringify(message));
|
||||
} else {
|
||||
console.warn("[WizardROS] Cannot send message - not connected");
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Handle incoming ROS messages
|
||||
*/
|
||||
private handleMessage(message: RosMessage): void {
|
||||
if (message.topic) {
|
||||
this.handleTopicMessage(message);
|
||||
}
|
||||
|
||||
this.emit("message", message);
|
||||
}
|
||||
|
||||
/**
|
||||
* Handle topic-specific messages
|
||||
*/
|
||||
private handleTopicMessage(message: RosMessage): void {
|
||||
if (!message.topic || !message.msg) return;
|
||||
|
||||
switch (message.topic) {
|
||||
case "/joint_states":
|
||||
this.updateJointStates(message.msg);
|
||||
break;
|
||||
case "/naoqi_driver/battery":
|
||||
this.updateBatteryStatus(message.msg);
|
||||
break;
|
||||
case "/naoqi_driver/bumper":
|
||||
case "/naoqi_driver/hand_touch":
|
||||
case "/naoqi_driver/head_touch":
|
||||
case "/naoqi_driver/sonar/left":
|
||||
case "/naoqi_driver/sonar/right":
|
||||
this.updateSensorData(message.topic, message.msg);
|
||||
break;
|
||||
}
|
||||
|
||||
this.robotStatus.lastUpdate = new Date();
|
||||
this.emit("robot_status_updated", this.robotStatus);
|
||||
}
|
||||
|
||||
/**
|
||||
* Update joint states from ROS message
|
||||
*/
|
||||
private updateJointStates(msg: Record<string, unknown>): void {
|
||||
if (
|
||||
msg.name &&
|
||||
msg.position &&
|
||||
Array.isArray(msg.name) &&
|
||||
Array.isArray(msg.position)
|
||||
) {
|
||||
const joints: Record<string, number> = {};
|
||||
|
||||
for (let i = 0; i < msg.name.length; i++) {
|
||||
const jointName = msg.name[i] as string;
|
||||
const position = msg.position[i] as number;
|
||||
if (jointName && typeof position === "number") {
|
||||
joints[jointName] = position;
|
||||
}
|
||||
}
|
||||
|
||||
this.robotStatus.joints = joints;
|
||||
this.robotStatus.connected = true;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Update battery status from ROS message
|
||||
*/
|
||||
private updateBatteryStatus(msg: Record<string, unknown>): void {
|
||||
if (typeof msg.percentage === "number") {
|
||||
this.robotStatus.battery = msg.percentage;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Update sensor data from ROS message
|
||||
*/
|
||||
private updateSensorData(topic: string, msg: Record<string, unknown>): void {
|
||||
this.robotStatus.sensors[topic] = msg;
|
||||
}
|
||||
|
||||
/**
|
||||
* Execute action with plugin configuration
|
||||
*/
|
||||
private async executeWithConfig(
|
||||
config: {
|
||||
topic: string;
|
||||
messageType: string;
|
||||
payloadMapping: {
|
||||
type: string;
|
||||
payload?: Record<string, unknown>;
|
||||
transformFn?: string;
|
||||
};
|
||||
},
|
||||
parameters: Record<string, unknown>,
|
||||
): Promise<void> {
|
||||
let msg: Record<string, unknown>;
|
||||
|
||||
if (
|
||||
config.payloadMapping.type === "template" &&
|
||||
config.payloadMapping.payload
|
||||
) {
|
||||
// Template-based payload construction
|
||||
msg = this.buildTemplatePayload(
|
||||
config.payloadMapping.payload,
|
||||
parameters,
|
||||
);
|
||||
} else if (config.payloadMapping.transformFn) {
|
||||
// Custom transform function
|
||||
msg = this.applyTransformFunction(
|
||||
config.payloadMapping.transformFn,
|
||||
parameters,
|
||||
);
|
||||
} else {
|
||||
// Direct parameter mapping
|
||||
msg = parameters;
|
||||
}
|
||||
|
||||
this.publish(config.topic, config.messageType, msg);
|
||||
|
||||
// Wait for action completion (simple delay for now)
|
||||
await new Promise((resolve) => setTimeout(resolve, 100));
|
||||
}
|
||||
|
||||
/**
|
||||
* Execute built-in robot actions
|
||||
*/
|
||||
private async executeBuiltinAction(
|
||||
actionId: string,
|
||||
parameters: Record<string, unknown>,
|
||||
): Promise<void> {
|
||||
switch (actionId) {
|
||||
case "say_text":
|
||||
this.publish("/speech", "std_msgs/String", {
|
||||
data: parameters.text || "Hello",
|
||||
});
|
||||
break;
|
||||
|
||||
case "walk_forward":
|
||||
case "walk_backward":
|
||||
case "turn_left":
|
||||
case "turn_right":
|
||||
this.executeMovementAction(actionId, parameters);
|
||||
break;
|
||||
|
||||
case "turn_head":
|
||||
this.executeTurnHead(parameters);
|
||||
break;
|
||||
|
||||
case "emergency_stop":
|
||||
this.publish("/cmd_vel", "geometry_msgs/Twist", {
|
||||
linear: { x: 0, y: 0, z: 0 },
|
||||
angular: { x: 0, y: 0, z: 0 },
|
||||
});
|
||||
break;
|
||||
|
||||
default:
|
||||
throw new Error(`Unknown action: ${actionId}`);
|
||||
}
|
||||
|
||||
// Wait for action completion
|
||||
await new Promise((resolve) => setTimeout(resolve, 100));
|
||||
}
|
||||
|
||||
/**
|
||||
* Execute movement actions
|
||||
*/
|
||||
private executeMovementAction(
|
||||
actionId: string,
|
||||
parameters: Record<string, unknown>,
|
||||
): void {
|
||||
let linear = { x: 0, y: 0, z: 0 };
|
||||
let angular = { x: 0, y: 0, z: 0 };
|
||||
|
||||
const speed = Number(parameters.speed) || 0.1;
|
||||
|
||||
switch (actionId) {
|
||||
case "walk_forward":
|
||||
linear.x = speed;
|
||||
break;
|
||||
case "walk_backward":
|
||||
linear.x = -speed;
|
||||
break;
|
||||
case "turn_left":
|
||||
angular.z = speed;
|
||||
break;
|
||||
case "turn_right":
|
||||
angular.z = -speed;
|
||||
break;
|
||||
}
|
||||
|
||||
this.publish("/cmd_vel", "geometry_msgs/Twist", { linear, angular });
|
||||
}
|
||||
|
||||
/**
|
||||
* Execute head turn action
|
||||
*/
|
||||
private executeTurnHead(parameters: Record<string, unknown>): void {
|
||||
const yaw = Number(parameters.yaw) || 0;
|
||||
const pitch = Number(parameters.pitch) || 0;
|
||||
const speed = Number(parameters.speed) || 0.3;
|
||||
|
||||
this.publish("/joint_angles", "naoqi_bridge_msgs/JointAnglesWithSpeed", {
|
||||
joint_names: ["HeadYaw", "HeadPitch"],
|
||||
joint_angles: [yaw, pitch],
|
||||
speed: speed,
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* Build template-based payload
|
||||
*/
|
||||
private buildTemplatePayload(
|
||||
template: Record<string, unknown>,
|
||||
parameters: Record<string, unknown>,
|
||||
): Record<string, unknown> {
|
||||
const result: Record<string, unknown> = {};
|
||||
|
||||
for (const [key, value] of Object.entries(template)) {
|
||||
if (typeof value === "string" && value.includes("{{")) {
|
||||
// Template substitution
|
||||
let substituted = value;
|
||||
for (const [paramKey, paramValue] of Object.entries(parameters)) {
|
||||
const placeholder = `{{${paramKey}}}`;
|
||||
substituted = substituted.replace(
|
||||
new RegExp(placeholder.replace(/[.*+?^${}()|[\]\\]/g, "\\$&"), "g"),
|
||||
String(paramValue ?? ""),
|
||||
);
|
||||
}
|
||||
result[key] = isNaN(Number(substituted))
|
||||
? substituted
|
||||
: Number(substituted);
|
||||
} else if (typeof value === "object" && value !== null) {
|
||||
result[key] = this.buildTemplatePayload(
|
||||
value as Record<string, unknown>,
|
||||
parameters,
|
||||
);
|
||||
} else {
|
||||
result[key] = value;
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* Apply transform function for NAO6 actions
|
||||
*/
|
||||
private applyTransformFunction(
|
||||
transformFn: string,
|
||||
parameters: Record<string, unknown>,
|
||||
): Record<string, unknown> {
|
||||
switch (transformFn) {
|
||||
case "naoVelocityTransform":
|
||||
return {
|
||||
linear: {
|
||||
x: Number(parameters.linear) || 0,
|
||||
y: 0,
|
||||
z: 0,
|
||||
},
|
||||
angular: {
|
||||
x: 0,
|
||||
y: 0,
|
||||
z: Number(parameters.angular) || 0,
|
||||
},
|
||||
};
|
||||
|
||||
case "naoSpeechTransform":
|
||||
return {
|
||||
data: String(parameters.text || "Hello"),
|
||||
};
|
||||
|
||||
case "naoHeadTransform":
|
||||
return {
|
||||
joint_names: ["HeadYaw", "HeadPitch"],
|
||||
joint_angles: [
|
||||
Number(parameters.yaw) || 0,
|
||||
Number(parameters.pitch) || 0,
|
||||
],
|
||||
speed: Number(parameters.speed) || 0.3,
|
||||
};
|
||||
|
||||
default:
|
||||
console.warn(`Unknown transform function: ${transformFn}`);
|
||||
return parameters;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Schedule reconnection attempt
|
||||
*/
|
||||
private scheduleReconnect(): void {
|
||||
if (this.reconnectTimer) return;
|
||||
|
||||
this.connectionAttempts++;
|
||||
console.log(
|
||||
`[WizardROS] Scheduling reconnect attempt ${this.connectionAttempts}/${this.maxReconnectAttempts}`,
|
||||
);
|
||||
|
||||
this.reconnectTimer = setTimeout(async () => {
|
||||
this.reconnectTimer = null;
|
||||
try {
|
||||
await this.connect();
|
||||
} catch (error) {
|
||||
console.error("[WizardROS] Reconnect failed:", error);
|
||||
if (this.connectionAttempts < this.maxReconnectAttempts) {
|
||||
this.scheduleReconnect();
|
||||
} else {
|
||||
this.emit("max_reconnects_reached");
|
||||
}
|
||||
}
|
||||
}, this.reconnectInterval);
|
||||
}
|
||||
|
||||
/**
|
||||
* Clear reconnect timer
|
||||
*/
|
||||
private clearReconnectTimer(): void {
|
||||
if (this.reconnectTimer) {
|
||||
clearTimeout(this.reconnectTimer);
|
||||
this.reconnectTimer = null;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Global service instance
|
||||
let wizardRosService: WizardRosService | null = null;
|
||||
let isCreatingInstance = false;
|
||||
|
||||
/**
|
||||
* Get or create the global wizard ROS service (true singleton)
|
||||
*/
|
||||
export function getWizardRosService(): WizardRosService {
|
||||
// Prevent multiple instances during creation
|
||||
if (isCreatingInstance && !wizardRosService) {
|
||||
throw new Error("WizardRosService is being initialized, please wait");
|
||||
}
|
||||
|
||||
if (!wizardRosService) {
|
||||
isCreatingInstance = true;
|
||||
try {
|
||||
wizardRosService = new WizardRosService();
|
||||
} finally {
|
||||
isCreatingInstance = false;
|
||||
}
|
||||
}
|
||||
return wizardRosService;
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize wizard ROS service with connection
|
||||
*/
|
||||
export async function initWizardRosService(): Promise<WizardRosService> {
|
||||
const service = getWizardRosService();
|
||||
|
||||
if (!service.getConnectionStatus()) {
|
||||
await service.connect();
|
||||
}
|
||||
|
||||
return service;
|
||||
}
|
||||
Reference in New Issue
Block a user