mirror of
https://github.com/soconnor0919/hristudio.git
synced 2025-12-11 22:54:45 -05:00
608 lines
20 KiB
TypeScript
Executable File
608 lines
20 KiB
TypeScript
Executable File
"use client";
|
|
|
|
import { useState, useEffect, useRef } from "react";
|
|
import { Button } from "~/components/ui/button";
|
|
import {
|
|
Card,
|
|
CardContent,
|
|
CardDescription,
|
|
CardHeader,
|
|
CardTitle,
|
|
} from "~/components/ui/card";
|
|
import { Input } from "~/components/ui/input";
|
|
import { Label } from "~/components/ui/label";
|
|
import { Textarea } from "~/components/ui/textarea";
|
|
import { Badge } from "~/components/ui/badge";
|
|
import { Separator } from "~/components/ui/separator";
|
|
import { Slider } from "~/components/ui/slider";
|
|
import { Tabs, TabsContent, TabsList, TabsTrigger } from "~/components/ui/tabs";
|
|
import { Alert, AlertDescription } from "~/components/ui/alert";
|
|
import { PageHeader } from "~/components/ui/page-header";
|
|
import { PageLayout } from "~/components/ui/page-layout";
|
|
import {
|
|
Play,
|
|
Square,
|
|
Volume2,
|
|
Camera,
|
|
Zap,
|
|
ArrowUp,
|
|
ArrowDown,
|
|
ArrowLeft,
|
|
ArrowRight,
|
|
RotateCcw,
|
|
RotateCw,
|
|
Wifi,
|
|
WifiOff,
|
|
AlertTriangle,
|
|
CheckCircle,
|
|
Activity,
|
|
Battery,
|
|
Eye,
|
|
Hand,
|
|
Footprints,
|
|
} from "lucide-react";
|
|
|
|
interface RosMessage {
|
|
topic: string;
|
|
msg: any;
|
|
type: string;
|
|
}
|
|
|
|
export default function NaoTestPage() {
|
|
const [connectionStatus, setConnectionStatus] = useState<
|
|
"disconnected" | "connecting" | "connected" | "error"
|
|
>("disconnected");
|
|
const [rosSocket, setRosSocket] = useState<WebSocket | null>(null);
|
|
const [robotStatus, setRobotStatus] = useState<any>(null);
|
|
const [jointStates, setJointStates] = useState<any>(null);
|
|
const [speechText, setSpeechText] = useState("");
|
|
const [walkSpeed, setWalkSpeed] = useState([0.1]);
|
|
const [turnSpeed, setTurnSpeed] = useState([0.3]);
|
|
const [headYaw, setHeadYaw] = useState([0]);
|
|
const [headPitch, setHeadPitch] = useState([0]);
|
|
const [logs, setLogs] = useState<string[]>([]);
|
|
const [sensorData, setSensorData] = useState<any>({});
|
|
const logsEndRef = useRef<HTMLDivElement>(null);
|
|
|
|
const ROS_BRIDGE_URL =
|
|
process.env.NEXT_PUBLIC_ROS_BRIDGE_URL || "ws://localhost:9090";
|
|
|
|
const addLog = (message: string) => {
|
|
const timestamp = new Date().toLocaleTimeString();
|
|
setLogs((prev) => [...prev.slice(-49), `[${timestamp}] ${message}`]);
|
|
};
|
|
|
|
useEffect(() => {
|
|
logsEndRef.current?.scrollIntoView({ behavior: "smooth" });
|
|
}, [logs]);
|
|
|
|
const connectToRos = () => {
|
|
if (rosSocket?.readyState === WebSocket.OPEN) return;
|
|
|
|
setConnectionStatus("connecting");
|
|
addLog("Connecting to ROS bridge...");
|
|
|
|
const socket = new WebSocket(ROS_BRIDGE_URL);
|
|
|
|
socket.onopen = () => {
|
|
setConnectionStatus("connected");
|
|
setRosSocket(socket);
|
|
addLog("Connected to ROS bridge successfully");
|
|
|
|
// Subscribe to robot topics
|
|
subscribeToTopics(socket);
|
|
};
|
|
|
|
socket.onmessage = (event) => {
|
|
try {
|
|
const data = JSON.parse(event.data);
|
|
handleRosMessage(data);
|
|
} catch (error) {
|
|
console.error("Error parsing ROS message:", error);
|
|
}
|
|
};
|
|
|
|
socket.onclose = () => {
|
|
setConnectionStatus("disconnected");
|
|
setRosSocket(null);
|
|
addLog("Disconnected from ROS bridge");
|
|
};
|
|
|
|
socket.onerror = () => {
|
|
setConnectionStatus("error");
|
|
addLog("Error connecting to ROS bridge");
|
|
};
|
|
};
|
|
|
|
const disconnectFromRos = () => {
|
|
if (rosSocket) {
|
|
rosSocket.close();
|
|
setRosSocket(null);
|
|
setConnectionStatus("disconnected");
|
|
addLog("Manually disconnected from ROS bridge");
|
|
}
|
|
};
|
|
|
|
const subscribeToTopics = (socket: WebSocket) => {
|
|
const topics = [
|
|
{ topic: "/naoqi_driver/joint_states", type: "sensor_msgs/JointState" },
|
|
{ topic: "/naoqi_driver/info", type: "naoqi_bridge_msgs/StringStamped" },
|
|
{ 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 }) => {
|
|
const subscribeMsg = {
|
|
op: "subscribe",
|
|
topic,
|
|
type,
|
|
};
|
|
socket.send(JSON.stringify(subscribeMsg));
|
|
addLog(`Subscribed to ${topic}`);
|
|
});
|
|
};
|
|
|
|
const handleRosMessage = (data: any) => {
|
|
if (data.topic === "/naoqi_driver/joint_states") {
|
|
setJointStates(data.msg);
|
|
} else if (data.topic === "/naoqi_driver/info") {
|
|
setRobotStatus(data.msg);
|
|
} else if (
|
|
data.topic?.includes("bumper") ||
|
|
data.topic?.includes("touch") ||
|
|
data.topic?.includes("sonar")
|
|
) {
|
|
setSensorData((prev) => ({
|
|
...prev,
|
|
[data.topic]: data.msg,
|
|
}));
|
|
}
|
|
};
|
|
|
|
const publishMessage = (topic: string, type: string, msg: any) => {
|
|
if (!rosSocket || rosSocket.readyState !== WebSocket.OPEN) {
|
|
addLog("Error: Not connected to ROS bridge");
|
|
return;
|
|
}
|
|
|
|
const rosMsg = {
|
|
op: "publish",
|
|
topic,
|
|
type,
|
|
msg,
|
|
};
|
|
|
|
rosSocket.send(JSON.stringify(rosMsg));
|
|
addLog(`Published to ${topic}: ${JSON.stringify(msg)}`);
|
|
};
|
|
|
|
const sayText = () => {
|
|
if (!speechText.trim()) return;
|
|
|
|
publishMessage("/speech", "std_msgs/String", {
|
|
data: speechText,
|
|
});
|
|
setSpeechText("");
|
|
};
|
|
|
|
const walkForward = () => {
|
|
publishMessage("/cmd_vel", "geometry_msgs/Twist", {
|
|
linear: { x: walkSpeed[0], y: 0, z: 0 },
|
|
angular: { x: 0, y: 0, z: 0 },
|
|
});
|
|
};
|
|
|
|
const walkBackward = () => {
|
|
publishMessage("/cmd_vel", "geometry_msgs/Twist", {
|
|
linear: { x: -walkSpeed[0], y: 0, z: 0 },
|
|
angular: { x: 0, y: 0, z: 0 },
|
|
});
|
|
};
|
|
|
|
const turnLeft = () => {
|
|
publishMessage("/cmd_vel", "geometry_msgs/Twist", {
|
|
linear: { x: 0, y: 0, z: 0 },
|
|
angular: { x: 0, y: 0, z: turnSpeed[0] },
|
|
});
|
|
};
|
|
|
|
const turnRight = () => {
|
|
publishMessage("/cmd_vel", "geometry_msgs/Twist", {
|
|
linear: { x: 0, y: 0, z: 0 },
|
|
angular: { x: 0, y: 0, z: -turnSpeed[0] },
|
|
});
|
|
};
|
|
|
|
const stopMovement = () => {
|
|
publishMessage("/cmd_vel", "geometry_msgs/Twist", {
|
|
linear: { x: 0, y: 0, z: 0 },
|
|
angular: { x: 0, y: 0, z: 0 },
|
|
});
|
|
};
|
|
|
|
const moveHead = () => {
|
|
publishMessage("/joint_angles", "naoqi_bridge_msgs/JointAnglesWithSpeed", {
|
|
joint_names: ["HeadYaw", "HeadPitch"],
|
|
joint_angles: [headYaw[0], headPitch[0]],
|
|
speed: 0.3,
|
|
});
|
|
};
|
|
|
|
const getConnectionStatusIcon = () => {
|
|
switch (connectionStatus) {
|
|
case "connected":
|
|
return <Wifi className="h-4 w-4 text-green-500" />;
|
|
case "connecting":
|
|
return <Activity className="h-4 w-4 animate-spin text-yellow-500" />;
|
|
case "error":
|
|
return <AlertTriangle className="h-4 w-4 text-red-500" />;
|
|
default:
|
|
return <WifiOff className="h-4 w-4 text-gray-500" />;
|
|
}
|
|
};
|
|
|
|
const getConnectionStatusBadge = () => {
|
|
const variants = {
|
|
connected: "default",
|
|
connecting: "secondary",
|
|
error: "destructive",
|
|
disconnected: "outline",
|
|
} as const;
|
|
|
|
return (
|
|
<Badge
|
|
variant={variants[connectionStatus]}
|
|
className="flex items-center gap-1"
|
|
>
|
|
{getConnectionStatusIcon()}
|
|
{connectionStatus.charAt(0).toUpperCase() + connectionStatus.slice(1)}
|
|
</Badge>
|
|
);
|
|
};
|
|
|
|
return (
|
|
<PageLayout>
|
|
<PageHeader
|
|
title="NAO Robot Test Console"
|
|
description="Test and control your NAO6 robot through ROS bridge"
|
|
/>
|
|
|
|
<div className="space-y-6">
|
|
{/* Connection Status */}
|
|
<Card>
|
|
<CardHeader>
|
|
<CardTitle className="flex items-center justify-between">
|
|
ROS Bridge Connection
|
|
{getConnectionStatusBadge()}
|
|
</CardTitle>
|
|
<CardDescription>
|
|
Connect to ROS bridge at {ROS_BRIDGE_URL}
|
|
</CardDescription>
|
|
</CardHeader>
|
|
<CardContent>
|
|
<div className="flex gap-2">
|
|
{connectionStatus === "connected" ? (
|
|
<Button onClick={disconnectFromRos} variant="destructive">
|
|
<WifiOff className="mr-2 h-4 w-4" />
|
|
Disconnect
|
|
</Button>
|
|
) : (
|
|
<Button
|
|
onClick={connectToRos}
|
|
disabled={connectionStatus === "connecting"}
|
|
>
|
|
<Wifi className="mr-2 h-4 w-4" />
|
|
{connectionStatus === "connecting"
|
|
? "Connecting..."
|
|
: "Connect"}
|
|
</Button>
|
|
)}
|
|
</div>
|
|
</CardContent>
|
|
</Card>
|
|
|
|
{connectionStatus === "connected" && (
|
|
<Tabs defaultValue="control" className="space-y-4">
|
|
<TabsList>
|
|
<TabsTrigger value="control">Robot Control</TabsTrigger>
|
|
<TabsTrigger value="sensors">Sensor Data</TabsTrigger>
|
|
<TabsTrigger value="status">Robot Status</TabsTrigger>
|
|
<TabsTrigger value="logs">Logs</TabsTrigger>
|
|
</TabsList>
|
|
|
|
<TabsContent value="control" className="space-y-4">
|
|
<div className="grid grid-cols-1 gap-4 md:grid-cols-2">
|
|
{/* Speech Control */}
|
|
<Card>
|
|
<CardHeader>
|
|
<CardTitle className="flex items-center gap-2">
|
|
<Volume2 className="h-4 w-4" />
|
|
Speech
|
|
</CardTitle>
|
|
</CardHeader>
|
|
<CardContent className="space-y-4">
|
|
<div className="space-y-2">
|
|
<Label htmlFor="speech">Text to Speech</Label>
|
|
<Textarea
|
|
id="speech"
|
|
placeholder="Enter text for NAO to say..."
|
|
value={speechText}
|
|
onChange={(e) => setSpeechText(e.target.value)}
|
|
onKeyDown={(e) =>
|
|
e.key === "Enter" &&
|
|
!e.shiftKey &&
|
|
(e.preventDefault(), sayText())
|
|
}
|
|
/>
|
|
</div>
|
|
<Button
|
|
onClick={sayText}
|
|
disabled={!speechText.trim()}
|
|
className="w-full"
|
|
>
|
|
<Play className="mr-2 h-4 w-4" />
|
|
Say Text
|
|
</Button>
|
|
</CardContent>
|
|
</Card>
|
|
|
|
{/* Movement Control */}
|
|
<Card>
|
|
<CardHeader>
|
|
<CardTitle className="flex items-center gap-2">
|
|
<Footprints className="h-4 w-4" />
|
|
Movement
|
|
</CardTitle>
|
|
</CardHeader>
|
|
<CardContent className="space-y-4">
|
|
<div className="space-y-2">
|
|
<Label>Walk Speed: {walkSpeed[0].toFixed(2)} m/s</Label>
|
|
<Slider
|
|
value={walkSpeed}
|
|
onValueChange={setWalkSpeed}
|
|
max={0.5}
|
|
min={0.05}
|
|
step={0.05}
|
|
/>
|
|
</div>
|
|
<div className="space-y-2">
|
|
<Label>Turn Speed: {turnSpeed[0].toFixed(2)} rad/s</Label>
|
|
<Slider
|
|
value={turnSpeed}
|
|
onValueChange={setTurnSpeed}
|
|
max={1.0}
|
|
min={0.1}
|
|
step={0.1}
|
|
/>
|
|
</div>
|
|
<div className="grid grid-cols-3 gap-2">
|
|
<Button variant="outline" onClick={walkForward}>
|
|
<ArrowUp className="h-4 w-4" />
|
|
</Button>
|
|
<Button variant="destructive" onClick={stopMovement}>
|
|
<Square className="h-4 w-4" />
|
|
</Button>
|
|
<Button variant="outline" onClick={walkBackward}>
|
|
<ArrowDown className="h-4 w-4" />
|
|
</Button>
|
|
<Button variant="outline" onClick={turnLeft}>
|
|
<RotateCcw className="h-4 w-4" />
|
|
</Button>
|
|
<div></div>
|
|
<Button variant="outline" onClick={turnRight}>
|
|
<RotateCw className="h-4 w-4" />
|
|
</Button>
|
|
</div>
|
|
</CardContent>
|
|
</Card>
|
|
|
|
{/* Head Control */}
|
|
<Card>
|
|
<CardHeader>
|
|
<CardTitle className="flex items-center gap-2">
|
|
<Eye className="h-4 w-4" />
|
|
Head Control
|
|
</CardTitle>
|
|
</CardHeader>
|
|
<CardContent className="space-y-4">
|
|
<div className="space-y-2">
|
|
<Label>Head Yaw: {headYaw[0].toFixed(2)} rad</Label>
|
|
<Slider
|
|
value={headYaw}
|
|
onValueChange={setHeadYaw}
|
|
max={2.09}
|
|
min={-2.09}
|
|
step={0.1}
|
|
/>
|
|
</div>
|
|
<div className="space-y-2">
|
|
<Label>Head Pitch: {headPitch[0].toFixed(2)} rad</Label>
|
|
<Slider
|
|
value={headPitch}
|
|
onValueChange={setHeadPitch}
|
|
max={0.51}
|
|
min={-0.67}
|
|
step={0.1}
|
|
/>
|
|
</div>
|
|
<Button onClick={moveHead} className="w-full">
|
|
Move Head
|
|
</Button>
|
|
</CardContent>
|
|
</Card>
|
|
|
|
{/* Emergency Stop */}
|
|
<Card>
|
|
<CardHeader>
|
|
<CardTitle className="flex items-center gap-2 text-red-600">
|
|
<AlertTriangle className="h-4 w-4" />
|
|
Emergency
|
|
</CardTitle>
|
|
</CardHeader>
|
|
<CardContent>
|
|
<Button
|
|
onClick={stopMovement}
|
|
variant="destructive"
|
|
size="lg"
|
|
className="w-full"
|
|
>
|
|
<Square className="mr-2 h-4 w-4" />
|
|
EMERGENCY STOP
|
|
</Button>
|
|
</CardContent>
|
|
</Card>
|
|
</div>
|
|
</TabsContent>
|
|
|
|
<TabsContent value="sensors" className="space-y-4">
|
|
<div className="grid grid-cols-1 gap-4 md:grid-cols-2 lg:grid-cols-3">
|
|
{Object.entries(sensorData).map(([topic, data]) => (
|
|
<Card key={topic}>
|
|
<CardHeader>
|
|
<CardTitle className="text-sm">
|
|
{topic
|
|
.split("/")
|
|
.pop()
|
|
?.replace(/_/g, " ")
|
|
.toUpperCase()}
|
|
</CardTitle>
|
|
</CardHeader>
|
|
<CardContent>
|
|
<pre className="max-h-32 overflow-auto rounded bg-gray-100 p-2 text-xs">
|
|
{JSON.stringify(data, null, 2)}
|
|
</pre>
|
|
</CardContent>
|
|
</Card>
|
|
))}
|
|
{Object.keys(sensorData).length === 0 && (
|
|
<Alert>
|
|
<AlertTriangle className="h-4 w-4" />
|
|
<AlertDescription>
|
|
No sensor data received yet. Make sure the robot is
|
|
connected and publishing data.
|
|
</AlertDescription>
|
|
</Alert>
|
|
)}
|
|
</div>
|
|
</TabsContent>
|
|
|
|
<TabsContent value="status" className="space-y-4">
|
|
<Card>
|
|
<CardHeader>
|
|
<CardTitle className="flex items-center gap-2">
|
|
<Activity className="h-4 w-4" />
|
|
Robot Status
|
|
</CardTitle>
|
|
</CardHeader>
|
|
<CardContent>
|
|
{robotStatus ? (
|
|
<div className="space-y-4">
|
|
<div className="grid grid-cols-2 gap-4">
|
|
<div>
|
|
<Label>Robot Info</Label>
|
|
<pre className="mt-1 rounded bg-gray-100 p-2 text-xs">
|
|
{JSON.stringify(robotStatus, null, 2)}
|
|
</pre>
|
|
</div>
|
|
{jointStates && (
|
|
<div>
|
|
<Label>Joint States</Label>
|
|
<div className="mt-1 max-h-64 overflow-auto rounded bg-gray-100 p-2 text-xs">
|
|
<div>Joints: {jointStates.name?.length || 0}</div>
|
|
<div>
|
|
Last Update: {new Date().toLocaleTimeString()}
|
|
</div>
|
|
{jointStates.name
|
|
?.slice(0, 10)
|
|
.map((name: string, i: number) => (
|
|
<div
|
|
key={name}
|
|
className="flex justify-between"
|
|
>
|
|
<span>{name}:</span>
|
|
<span>
|
|
{jointStates.position?.[i]?.toFixed(3) ||
|
|
"N/A"}
|
|
</span>
|
|
</div>
|
|
))}
|
|
{(jointStates.name?.length || 0) > 10 && (
|
|
<div className="text-gray-500">
|
|
... and {(jointStates.name?.length || 0) - 10}{" "}
|
|
more
|
|
</div>
|
|
)}
|
|
</div>
|
|
</div>
|
|
)}
|
|
</div>
|
|
</div>
|
|
) : (
|
|
<Alert>
|
|
<AlertTriangle className="h-4 w-4" />
|
|
<AlertDescription>
|
|
No robot status data received. Check that the NAO robot
|
|
is connected and the naoqi_driver is running.
|
|
</AlertDescription>
|
|
</Alert>
|
|
)}
|
|
</CardContent>
|
|
</Card>
|
|
</TabsContent>
|
|
|
|
<TabsContent value="logs" className="space-y-4">
|
|
<Card>
|
|
<CardHeader>
|
|
<CardTitle>Communication Logs</CardTitle>
|
|
<CardDescription>
|
|
Real-time log of ROS bridge communication
|
|
</CardDescription>
|
|
</CardHeader>
|
|
<CardContent>
|
|
<div className="h-64 overflow-auto rounded bg-black p-4 font-mono text-xs text-green-400">
|
|
{logs.map((log, index) => (
|
|
<div key={index}>{log}</div>
|
|
))}
|
|
<div ref={logsEndRef} />
|
|
</div>
|
|
<Button
|
|
onClick={() => setLogs([])}
|
|
variant="outline"
|
|
className="mt-2"
|
|
>
|
|
Clear Logs
|
|
</Button>
|
|
</CardContent>
|
|
</Card>
|
|
</TabsContent>
|
|
</Tabs>
|
|
)}
|
|
|
|
{connectionStatus !== "connected" && (
|
|
<Alert>
|
|
<AlertTriangle className="h-4 w-4" />
|
|
<AlertDescription>
|
|
Connect to ROS bridge to start controlling the robot. Make sure
|
|
the NAO integration is running:
|
|
<br />
|
|
<code className="mt-2 block rounded bg-gray-100 p-2">
|
|
ros2 launch nao6_hristudio.launch.py nao_ip:=nao.local
|
|
password:=robolab
|
|
</code>
|
|
</AlertDescription>
|
|
</Alert>
|
|
)}
|
|
</div>
|
|
</PageLayout>
|
|
);
|
|
}
|