Spaces:
Running
Running
File size: 10,195 Bytes
18b0fa5 3aea7c6 18b0fa5 3aea7c6 18b0fa5 3aea7c6 18b0fa5 |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 |
import { Robot, type ManagedJointState } from "./Robot.svelte";
import type {
MasterDriver,
SlaveDriver,
DriverJointState,
MasterDriverConfig,
SlaveDriverConfig
} from "$lib/types/robotDriver";
import { MockSequenceMaster, DEMO_SEQUENCES } from "./drivers/MockSequenceMaster";
import { MockSlave } from "./drivers/MockSlave";
import { USBSlave } from "./drivers/USBSlave";
import { RemoteServerMaster } from "./drivers/RemoteServerMaster";
import { RemoteServerSlave } from "./drivers/RemoteServerSlave";
import { USBMaster } from "./drivers/USBMaster";
import { createRobot } from "@/components/3d/robot/URDF/createRobot.svelte";
import type { RobotUrdfConfig } from "$lib/types/urdf";
import {
getCommunicationConfig,
getRobotPollingConfig,
getDataProcessingConfig
} from "$lib/configs/performanceConfig";
/**
* Central manager for all robots with master-slave architecture
*
* Masters: Command sources (remote servers, scripts, manual control)
* Slaves: Execution targets (physical robots, simulators)
*/
export class RobotManager {
private _robots = $state<Robot[]>([]);
// Reactive getters
get robots(): Robot[] {
return this._robots;
}
get robotCount(): number {
return this._robots.length;
}
get robotsWithMaster(): Robot[] {
return this._robots.filter((robot) => robot.master !== undefined);
}
get robotsWithSlaves(): Robot[] {
return this._robots.filter((robot) => robot.slaves.length > 0);
}
/**
* Create a new robot from URDF configuration
*/
async createRobot(id: string, urdfConfig: RobotUrdfConfig): Promise<Robot> {
// Check if robot already exists
if (this._robots.find((r) => r.id === id)) {
throw new Error(`Robot with ID ${id} already exists`);
}
// Create robot state from URDF
const robotState = await createRobot(urdfConfig);
// Create managed robot
const robot = new Robot(id, robotState);
// Add to reactive array
this._robots.push(robot);
console.log(`Created robot ${id}. Total robots: ${this._robots.length}`);
return robot;
}
/**
* Get robot by ID
*/
getRobot(id: string): Robot | undefined {
return this._robots.find((r) => r.id === id);
}
/**
* Remove a robot
*/
async removeRobot(id: string): Promise<void> {
const robotIndex = this._robots.findIndex((r) => r.id === id);
if (robotIndex === -1) return;
const robot = this._robots[robotIndex];
// Move to rest position before removal if has connected slaves
if (robot.connectedSlaves.length > 0) {
try {
console.log(`Removing robot ${id}: moving to rest position first`);
await robot.moveToRestPosition(3000);
await new Promise((resolve) => setTimeout(resolve, 500));
} catch (error) {
console.warn(`Failed to move robot ${id} to rest position before removal:`, error);
}
}
// Clean up robot resources
await robot.destroy();
// Remove from reactive array
this._robots.splice(robotIndex, 1);
console.log(`Removed robot ${id}. Remaining robots: ${this._robots.length}`);
}
// ============= MASTER MANAGEMENT =============
/**
* Connect a master driver to a robot
*/
async connectMaster(robotId: string, masterConfig: MasterDriverConfig): Promise<void> {
const robot = this._robots.find((r) => r.id === robotId);
if (!robot) {
throw new Error(`Robot ${robotId} not found`);
}
// Create master driver instance
const master = this.createMaster(masterConfig, robotId);
// Connect the master
await master.connect();
// Attach to robot
await robot.setMaster(master);
console.log(`Master ${master.name} connected to robot ${robotId}`);
}
/**
* Disconnect a robot's master
*/
async disconnectMaster(robotId: string): Promise<void> {
const robot = this._robots.find((r) => r.id === robotId);
if (!robot) {
throw new Error(`Robot ${robotId} not found`);
}
await robot.removeMaster();
console.log(`Master disconnected from robot ${robotId}. Manual control restored.`);
}
// ============= SLAVE MANAGEMENT =============
/**
* Connect a slave driver to a robot
*/
async connectSlave(robotId: string, slaveConfig: SlaveDriverConfig): Promise<void> {
const robot = this._robots.find((r) => r.id === robotId);
if (!robot) {
throw new Error(`Robot ${robotId} not found`);
}
// Create slave driver instance
const slave = this.createSlave(slaveConfig, robot);
// Add to robot (this handles connection and initialization)
await robot.addSlave(slave);
console.log(`Slave ${slave.name} connected to robot ${robotId}`);
}
/**
* Disconnect a slave driver from a robot
*/
async disconnectSlave(robotId: string, slaveId: string): Promise<void> {
const robot = this._robots.find((r) => r.id === robotId);
if (!robot) {
throw new Error(`Robot ${robotId} not found`);
}
await robot.removeSlave(slaveId);
console.log(`Slave ${slaveId} disconnected from robot ${robotId}`);
}
// ============= CONVENIENCE METHODS =============
/**
* Connect demo sequence master to a robot
*/
async connectDemoSequences(robotId: string, loopMode: boolean = true): Promise<void> {
const config: MasterDriverConfig = {
type: "mock-sequence",
sequences: DEMO_SEQUENCES,
autoStart: true,
loopMode
};
await this.connectMaster(robotId, config);
}
/**
* Connect mock slave to a robot
*/
async connectMockSlave(robotId: string, simulateLatency: number = 50): Promise<void> {
const config: SlaveDriverConfig = {
type: "mock-slave",
simulateLatency,
simulateErrors: false,
responseDelay: 20
};
await this.connectSlave(robotId, config);
}
/**
* Connect USB slave to a robot (when implemented)
*/
async connectUSBSlave(robotId: string, port?: string): Promise<void> {
const config: SlaveDriverConfig = {
type: "usb-slave",
port,
baudRate: 115200
};
await this.connectSlave(robotId, config);
}
/**
* Connect remote server slave to a robot
*/
async connectRemoteServerSlave(
robotId: string,
url: string = "ws://localhost:8080",
apiKey?: string,
targetRobotId?: string
): Promise<void> {
const config: SlaveDriverConfig = {
type: "remote-server-slave",
url,
apiKey,
robotId: targetRobotId || robotId // Use targetRobotId if provided, otherwise use local robotId
};
await this.connectSlave(robotId, config);
}
/**
* Connect USB master to a robot
*/
async connectUSBMaster(
robotId: string,
options: { port?: string; baudRate?: number; pollInterval?: number; smoothing?: boolean } = {}
): Promise<void> {
const config: MasterDriverConfig = {
type: "usb-master",
port: options.port,
baudRate: options.baudRate || getCommunicationConfig().USB_BAUD_RATE,
pollInterval: options.pollInterval || getRobotPollingConfig().USB_MASTER_POLL_INTERVAL_MS,
smoothing: options.smoothing ?? getDataProcessingConfig().ENABLE_SMOOTHING
};
await this.connectMaster(robotId, config);
}
/**
* Get detailed robot status
*/
getRobotStatus(robotId: string):
| {
id: string;
hasActiveMaster: boolean;
masterName?: string;
manualControlEnabled: boolean;
connectedSlaves: number;
totalSlaves: number;
lastCommandSource: string;
}
| undefined {
const robot = this._robots.find((r) => r.id === robotId);
if (!robot) return undefined;
return {
id: robot.id,
hasActiveMaster: robot.controlState.hasActiveMaster,
masterName: robot.controlState.masterName,
manualControlEnabled: robot.manualControlEnabled,
connectedSlaves: robot.connectedSlaves.length,
totalSlaves: robot.slaves.length,
lastCommandSource: robot.controlState.lastCommandSource
};
}
/**
* Get joint states from all robots
*/
getAllJointStates(): { robotId: string; joints: ManagedJointState[] }[] {
return this._robots.map((robot) => ({
robotId: robot.id,
joints: robot.joints
}));
}
/**
* Clean up all robots
*/
async destroy(): Promise<void> {
const cleanupPromises = this._robots.map((robot) => robot.destroy());
await Promise.allSettled(cleanupPromises);
this._robots.length = 0;
}
// ============= DRIVER FACTORIES =============
/**
* Create a master driver instance
*/
private createMaster(config: MasterDriverConfig, robotId: string): MasterDriver {
switch (config.type) {
case "mock-sequence":
return new MockSequenceMaster(config);
case "remote-server":
return new RemoteServerMaster(config, robotId);
case "script-player":
// TODO: Implement ScriptPlayerMaster
throw new Error("Script player master not implemented yet");
case "usb-master":
return new USBMaster(config);
default: {
// TypeScript exhaustiveness check
const _exhaustive: never = config;
throw new Error(
`Unknown master driver type: ${(_exhaustive as unknown as { type: string }).type}`
);
}
}
}
/**
* Create a slave driver instance
*/
private createSlave(config: SlaveDriverConfig, robot: Robot): SlaveDriver {
// Convert robot joints to driver joint states
const driverJointStates: DriverJointState[] = robot.joints.map((joint) => ({
name: joint.name,
servoId: joint.servoId || 0,
type: joint.urdfJoint.type as "revolute" | "continuous",
virtualValue: joint.virtualValue,
realValue: joint.realValue,
limits: joint.urdfJoint.limit
? {
lower: joint.urdfJoint.limit.lower,
upper: joint.urdfJoint.limit.upper,
velocity: joint.urdfJoint.limit.velocity,
effort: joint.urdfJoint.limit.effort
}
: undefined
}));
switch (config.type) {
case "mock-slave":
return new MockSlave(config, driverJointStates);
case "usb-slave":
return new USBSlave(config, driverJointStates);
case "simulation-slave":
// TODO: Implement SimulationSlave
throw new Error("Simulation slave driver not implemented yet");
case "remote-server-slave":
return new RemoteServerSlave(config, driverJointStates);
default: {
// TypeScript exhaustiveness check
const _exhaustive: never = config;
throw new Error(
`Unknown slave driver type: ${(_exhaustive as unknown as { type: string }).type}`
);
}
}
}
}
// Global robot manager instance
export const robotManager = new RobotManager();
|