robotics-dev
Version:
Robotics.dev P2P SDK client for robot control
581 lines (499 loc) • 23.1 kB
JavaScript
const io = require('socket.io-client');
const nodeDataChannel = require('node-datachannel');
const { v4: uuidv4 } = require('uuid');
const PNG = require('pngjs').PNG;
// RealSense D435 specific constants
const DEPTH_WIDTH = 424; // Fixed width for RealSense D435
const DEPTH_HEIGHT = 240; // Fixed height for RealSense D435
const MIN_DEPTH = 100; // 100mm minimum depth
const MAX_DEPTH = 10000; // 10000mm maximum depth
const DEPTH_SCALE = 1.5; // Scale factor to correct depth values
// PNG signature bytes
const PNG_SIGNATURE = Buffer.from([0x89, 0x50, 0x4E, 0x47, 0x0D, 0x0A, 0x1A, 0x0A]);
// Function to calculate depth from compressed depth data
async function calculateDepth(depthData) {
try {
if (!depthData || !depthData.data) {
console.error(formatLog('No depth data received'));
return null;
}
// Convert the raw data to a Buffer
const rawData = Object.values(depthData.data);
const buffer = Buffer.from(rawData);
// Find the PNG signature in the buffer
const pngStart = buffer.indexOf(PNG_SIGNATURE);
if (pngStart === -1) {
console.error(formatLog('PNG signature not found in data'));
return null;
}
// Extract the PNG data starting from the signature
const pngData = buffer.slice(pngStart);
// Create a new PNG parser
const png = new PNG();
// Parse the PNG data
return new Promise((resolve, reject) => {
png.on('error', (err) => {
console.error(formatLog('PNG parsing error:'), err);
reject(err);
});
png.parse(pngData, (err, data) => {
if (err) {
console.error(formatLog('Error parsing PNG:'), err);
reject(err);
return;
}
// Check for valid PNG dimensions and data
if (png.width === 0 || png.height === 0 || !png.data || png.data.length === 0) {
console.error(formatLog('Parsed PNG has no data or zero dimensions'));
resolve(null);
return;
}
if (png.width !== DEPTH_WIDTH || png.height !== DEPTH_HEIGHT) {
console.error(formatLog(`Unexpected PNG dimensions: ${png.width}x${png.height}`));
resolve(null);
return;
}
// Create a 2D array for depth values
const depthValues = new Array(DEPTH_HEIGHT).fill().map(() => new Array(DEPTH_WIDTH).fill(0));
let validCount = 0;
// Read depth values from PNG data
for (let y = 0; y < png.height; y++) {
for (let x = 0; x < png.width; x++) {
const idx = (y * png.width + x) * 4; // 4 bytes per pixel (RGBA)
if (idx + 1 < png.data.length) {
// Read 16-bit value (little-endian)
const rawValue = (png.data[idx + 1] << 8) | png.data[idx];
// Apply scaling factor to get correct depth
const value = Math.round(rawValue * DEPTH_SCALE);
// Store the value
depthValues[y][x] = value;
if (value >= MIN_DEPTH && value <= MAX_DEPTH) {
validCount++;
}
}
}
}
// Calculate center coordinates
const centerX = Math.floor(DEPTH_WIDTH / 2);
const centerY = Math.floor(DEPTH_HEIGHT / 2);
// Get the center pixel value
const centerDepth = depthValues[centerY][centerX];
// Return just the depth value if valid, otherwise null
resolve(centerDepth >= MIN_DEPTH && centerDepth <= MAX_DEPTH ? centerDepth / 1000.0 : null);
});
});
} catch (error) {
console.error(formatLog('Error calculating depth:'), error);
return null;
}
}
const config = {
iceServers: [
'stun:stun1.l.google.com:19302',
'stun:stun2.l.google.com:19302'
],
maxMessageSize: 16000,
enableIceTcp: true,
portRangeBegin: 5000,
portRangeEnd: 6000,
numDataChannels: 10 // Default number of data channels
};
function formatLog(message) {
const timestamp = new Date().toISOString();
return `[${timestamp}] ${message}`;
}
class RoboticsClient {
constructor() {
this.socket = null;
this.defaultServer = 'wss://robotics.dev';
this.pc = null;
this.dataChannels = new Map(); // Map to store multiple data channels
this.isConnected = false;
this.hasRemoteDescription = false;
this.pendingCandidates = [];
this.setupComplete = false;
this.setupInProgress = false;
this.messages = new Map(); // Store incomplete messages
this.clientId = 'remote-' + Math.random().toString(36).substr(2, 9);
this.robot = null;
this.token = null;
this.currentChannelIndex = 0; // For round-robin channel selection
}
connect(options = {}, callback) {
const server = options.server || this.defaultServer;
this.robot = options.robot;
this.token = options.token;
if (!this.robot || !this.token) {
throw new Error('Robot ID and token are required');
}
console.log(formatLog(`Connecting to robot ${this.robot} at ${server}`));
this.socket = io(server, {
reconnection: true,
rejectUnauthorized: false,
transports: ["websocket", "polling"],
query: {
id: this.clientId,
room: this.robot,
token: this.token
},
auth: { id: this.clientId }
});
this.socket.on('connect', async () => {
console.log(formatLog(`Connected to robotics.dev - Socket ID: ${this.socket.id}`));
console.log(formatLog(`Registering with robot ${this.robot}`));
this.socket.emit('register', {
id: this.clientId,
room: this.robot,
token: this.token
});
await this.setupPeerConnection(callback);
});
this.socket.on('connect_error', (error) => {
console.error(formatLog(`Connection error: ${error.message}`));
});
this.socket.on('disconnect', (reason) => {
console.log(formatLog(`Disconnected: ${reason}`));
});
this.setupSignalingHandlers(callback);
return this.socket.id;
}
setupSignalingHandlers(callback) {
this.socket.on('signal', async (message) => {
try {
if (message.type === 'answer' && message.sourcePeer === this.robot) {
await this.pc.setRemoteDescription(String(message.sdp), message.type);
this.hasRemoteDescription = true;
while (this.pendingCandidates.length > 0) {
const candidate = this.pendingCandidates.shift();
await this.pc.addRemoteCandidate(candidate.candidate, candidate.mid);
}
} else if (message.type === 'candidate' && message.sourcePeer === this.robot) {
const candidate = {
candidate: String(message.candidate),
mid: String(message.mid || "0")
};
if (!this.hasRemoteDescription) {
this.pendingCandidates.push(candidate);
} else {
await this.pc.addRemoteCandidate(candidate.candidate, candidate.mid);
}
}
} catch (error) {
console.error(formatLog(`Signal error: ${error}`));
}
});
this.socket.on('room-info', (info) => {
if (info.peers.includes(this.robot) && !this.isConnected && !this.setupComplete) {
this.setupPeerConnection(callback);
}
});
}
async setupPeerConnection(callback) {
if (this.setupInProgress) return;
this.setupInProgress = true;
try {
if (this.pc) {
this.pc.close();
this.pc = null;
}
this.pc = new nodeDataChannel.PeerConnection('client', config);
this.pc.onStateChange((state) => {
console.log(formatLog(`Connection state: ${state}`));
this.isConnected = state === 'connected';
this.setupComplete = this.isConnected;
this.setupInProgress = !this.isConnected;
});
this.pc.onLocalDescription((sdp, type) => {
this.sendSignal({
type,
sdp: String(sdp)
});
});
this.pc.onLocalCandidate((candidate, mid) => {
if (!candidate) return;
this.sendSignal({
type: 'candidate',
candidate: String(candidate),
mid: String(mid)
});
});
this.setupDataChannel(callback);
await this.pc.setLocalDescription();
} catch (error) {
console.error(formatLog(`Setup failed: ${error}`));
this.setupInProgress = false;
throw error;
}
}
setupDataChannel(callback) {
// Add helper function for image conversion
const convertCompressedImageToBase64 = (imageData) => {
try {
// Handle different possible data structures
let compressedData;
if (Array.isArray(imageData)) {
compressedData = imageData;
} else if (imageData && typeof imageData === 'object') {
if (Array.isArray(imageData.data)) {
compressedData = imageData.data;
} else if (imageData.buffer) {
compressedData = imageData.buffer;
} else if (imageData.data && typeof imageData.data === 'object') {
// Handle the case where data is an object with numeric keys
const dataObj = imageData.data;
// Convert numeric-keyed object to array
compressedData = Object.keys(dataObj)
.sort((a, b) => parseInt(a) - parseInt(b))
.map(key => dataObj[key]);
} else {
console.error(formatLog('Unsupported image data format'));
return null;
}
} else {
console.error(formatLog('Unsupported image data format'));
return null;
}
if (!compressedData || compressedData.length === 0) {
console.error(formatLog('No valid image data found'));
return null;
}
// Convert the array to a Buffer
const buffer = Buffer.from(compressedData);
// Convert Buffer to base64
const base64 = buffer.toString('base64');
// Validate the base64 string
if (base64.length < 100) { // Arbitrary minimum length for a valid image
console.error(formatLog('Generated base64 string too short, likely invalid'));
return null;
}
return base64;
} catch (error) {
console.error(formatLog('Error converting compressed image:'), error);
return null;
}
};
// Create multiple data channels
for (let i = 0; i < config.numDataChannels; i++) {
const channel = this.pc.createDataChannel(`robotics-${i}`);
this.dataChannels.set(i, channel);
channel.onOpen(() => {
console.log(formatLog(`Data channel ${i} opened`));
// Set readyState to 1 when channel opens
channel.readyState = 1;
this.isConnected = true;
});
channel.onMessage(async (data) => {
try {
let message;
// Handle binary data
if (data instanceof ArrayBuffer || Buffer.isBuffer(data)) {
const buffer = Buffer.from(data);
// Extract header information (matching comms.js format)
const index = Number(buffer.readBigUInt64BE(0));
const total = Number(buffer.readBigUInt64BE(8));
const messageId = buffer.slice(16, 52).toString('hex').replace(/(.{8})(.{4})(.{4})(.{4})(.{12})/, '$1-$2-$3-$4-$5');
const chunkData = buffer.slice(52).toString();
// Initialize or get existing message chunks
if (!this.messages.has(messageId)) {
this.messages.set(messageId, {
chunks: new Array(total).fill(null),
received: 0,
total: total
});
}
const messageInfo = this.messages.get(messageId);
messageInfo.chunks[index] = chunkData;
messageInfo.received++;
// If all chunks received, process complete message
if (messageInfo.received === messageInfo.total) {
const completeMessage = messageInfo.chunks.join('');
try {
const dataObj = JSON.parse(completeMessage);
if (dataObj.topic) {
// console.log(formatLog(`📡 ROS Topic: ${dataObj.topic}`));
// Handle camera image messages
if (dataObj.topic === ('/camera/camera/color/image_raw/compressed' || '/camera/camera/color/image_raw') && dataObj.data) {
dataObj.base64Image = dataObj.data;
// const base64Image = await convertCompressedImageToBase64(dataObj.data);
// if (base64Image) {
// dataObj.base64Image = base64Image;
// }
}
if (dataObj.topic === ('/camera2d') && dataObj.data && dataObj.data) {
dataObj.base64Image = dataObj.data;
}
if (dataObj.topic === ('/camera/camera/depth/image_rect_raw/compressedDepth') && dataObj.data) {
const depth = await calculateDepth(dataObj.data);
if (depth) {
dataObj.depth = depth;
}
}
if (callback) {
callback(dataObj);
}
}
} catch (error) {
console.error(formatLog(`Error parsing complete message: ${error}`));
}
this.messages.delete(messageId);
}
} else {
// Handle legacy string messages
if (typeof data === 'string') {
message = JSON.parse(data);
} else {
const str = new TextDecoder().decode(new Uint8Array(data));
message = JSON.parse(str);
}
// Only process if it's a ROS message with a topic
if (message.topic) {
// console.log(formatLog(`📡 ROS Topic: ${message.topic}`));
// Handle camera image messages
if (message.topic.includes('camera') && message.data) {
const base64Image = await convertCompressedImageToBase64(message.data);
if (base64Image) {
message.base64Image = base64Image;
}
}
if (message.topic === ('/camera2d') && message.data && message.data.data) {
message.base64Image = message.data.data;
}
if (callback) {
callback(message);
}
}
}
} catch (error) {
console.error(formatLog(`Message handling error: ${error.message}`));
// Try to handle raw message
if (callback && typeof data === 'string') {
callback({ data: data });
}
}
});
channel.onError((error) => {
console.error(formatLog(`Data channel ${i} error: ${error}`));
// Set readyState to 3 (CLOSED) on error
channel.readyState = 3;
this.isConnected = false;
});
channel.onClosed(() => {
console.log(formatLog(`Data channel ${i} closed`));
// Set readyState to 3 (CLOSED) when channel closes
channel.readyState = 3;
this.isConnected = false;
});
// Initialize readyState to 0 (CONNECTING)
channel.readyState = 0;
}
}
// Get next available channel in round-robin fashion
getNextChannel() {
const channel = this.dataChannels.get(this.currentChannelIndex);
this.currentChannelIndex = (this.currentChannelIndex + 1) % config.numDataChannels;
return channel;
}
sendCompressedMessage(msg) {
try {
// Debug log all channels and their states
// console.log(formatLog('Checking channel states:'));
// this.dataChannels.forEach((channel, index) => {
// console.log(formatLog(`Channel ${index} state: ${channel ? channel.readyState : 'null'}`));
// });
// Get open channels - readyState 1 is OPEN in node-datachannel
const openChannels = Array.from(this.dataChannels.values())
.filter(channel => channel && channel.readyState === 1);
// console.log(formatLog(`Found ${openChannels.length} open channels`));
if (openChannels.length > 0) {
try {
// Simple JSON stringify of the message
const message = JSON.stringify(msg);
// Use round-robin selection
const channel = openChannels[this.currentChannelIndex];
// console.log(formatLog(`Using channel ${this.currentChannelIndex} for sending`));
channel.sendMessage(message);
// Update channel index for next message
this.currentChannelIndex = (this.currentChannelIndex + 1) % openChannels.length;
return; // Success! Exit early
} catch (error) {
console.error(formatLog(`[P2P] Data channel send failed: ${error}`));
// Fall through to socket
}
}
// Fallback to socket.io if no channels or send failed
if (this.socket) {
console.log(formatLog('Falling back to socket.io for message delivery'));
this.socket.emit('message', { robot: this.robot, token: this.token, data: msg });
} else {
throw new Error('No communication channels available');
}
} catch (error) {
console.error(formatLog(`Send error: ${error}`));
throw error;
}
}
sendSignal(message) {
const signalMessage = {
...message,
targetPeer: this.robot,
sourcePeer: this.clientId,
room: this.robot
};
this.socket.emit('signal', signalMessage);
}
twist(robot, twist) {
if (!robot || !twist) {
throw new Error('Robot and twist payload are required');
}
const message = {
topic: "twist",
robot: robot,
twist: twist
};
if (this.isConnected) {
this.sendCompressedMessage(message);
} else if (this.socket) {
this.socket.emit('twist', { robot, token: this.token, twist });
} else {
throw new Error('No connection available');
}
}
speak(robot, text) {
if (!robot || !text) {
throw new Error('Robot and text payload are required');
}
const message = {
topic: "speak",
robot: robot,
text: text
};
if (this.isConnected) {
this.sendCompressedMessage(message);
} else if (this.socket) {
this.socket.emit('speak', { robot, token: this.token, text });
} else {
throw new Error('No connection available');
}
}
disconnect() {
this.dataChannels.forEach(channel => {
if (channel) {
channel.close();
}
});
this.dataChannels.clear();
if (this.pc) {
this.pc.close();
this.pc = null;
}
if (this.socket) {
this.socket.disconnect();
this.socket = null;
}
this.isConnected = false;
this.setupComplete = false;
}
}
// Create singleton instance
const robotics = new RoboticsClient();
module.exports = robotics;