robotics
Version:
Robotics.dev P2P ROS2 robot controller CLI with ROS telemetry and video streaming
1,130 lines (996 loc) • 124 kB
JavaScript
process.on('uncaughtException', (err) => {
console.error('An uncaught exception occurred:', err);
});
process.on('unhandledRejection', (reason, promise) => {
console.error('Unhandled rejection:', reason);
});
/**
* NON-QUEUING MESSAGE SYSTEM
*
* This implementation eliminates message queuing to reduce latency and ensure
* only the most current ROS messages are sent. Key improvements:
*
* 1. No Message Queuing: ROS messages are not queued - only the latest message
* for each topic is kept and sent immediately
* 2. Priority System: High-priority topics (camera, odometry, cmd_vel) are sent
* immediately, while lower-priority topics have a minimal delay
* 3. Immediate Sending: Messages are sent as soon as they arrive, not batched
* 4. Memory Management: Stale pending messages are automatically cleaned up
* 5. Performance Monitoring: Optional logging of message frequencies
*
* Configuration options in messageConfig:
* - enableNonQueuing: Enable/disable the new system
* - highPriorityTopics: List of topics to prioritize
* - pendingMessageTimeout: Timeout for stale messages (ms)
* - lowPriorityDelay: Delay for low priority messages (ms)
* - performanceLogging: Enable performance metrics
*/
// Add memory monitoring
let memoryCheckInterval;
function startMemoryMonitoring() {
memoryCheckInterval = setInterval(() => {
const memUsage = process.memoryUsage();
const memUsageMB = {
rss: Math.round(memUsage.rss / 1024 / 1024),
heapUsed: Math.round(memUsage.heapUsed / 1024 / 1024),
heapTotal: Math.round(memUsage.heapTotal / 1024 / 1024),
external: Math.round(memUsage.external / 1024 / 1024)
};
console.log(formatLog(`Memory usage: RSS=${memUsageMB.rss}MB, Heap=${memUsageMB.heapUsed}MB/${memUsageMB.heapTotal}MB, External=${memUsageMB.external}MB`));
// Force garbage collection if memory usage is high
if (memUsageMB.heapUsed > 500) { // 500MB threshold
console.log(formatLog('High memory usage detected, forcing garbage collection'));
if (global.gc) {
global.gc();
}
}
}, 30000); // Check every 30 seconds
}
// Global storage for latest camera data (independent of P2P connections)
const globalCameraData = new Map(); // topic -> { data: base64, timestamp: Date.now() }
// Cleanup old camera data periodically
setInterval(() => {
const now = Date.now();
let cleanedCount = 0;
for (const [topic, data] of globalCameraData.entries()) {
if (now - data.timestamp > 10000) { // Remove data older than 10 seconds
globalCameraData.delete(topic);
cleanedCount++;
}
}
if (cleanedCount > 0) {
console.log(formatLog(`🧹 Cleaned up ${cleanedCount} old camera data entries`));
}
}, 30000); // Check every 30 seconds
function stopMemoryMonitoring() {
if (memoryCheckInterval) {
clearInterval(memoryCheckInterval);
memoryCheckInterval = null;
}
}
import {createRequire } from "module";
const require = createRequire(import.meta.url);
import Configstore from 'configstore';
import { fetchRobotConfig, resolveTopic, toClientTopic, getCmdVelTopic, fetchIceServers } from './ros-topics.js';
const configs = new Configstore('robotics');
const robotId = configs.get('ROBOT_ID');
const apiToken = configs.get('API_TOKEN');
const { exec } = require('child_process');
const rclnodejs = require('rclnodejs');
const sharp = require('sharp');
var velocityPub;
var firstRun = true;
// Checks for --server and if it has a value
const serverIndex = process.argv.indexOf('--server');
let serverValue;
if (serverIndex > -1) {
serverValue = process.argv[serverIndex + 1];
if(!serverValue.startsWith('ws')){
serverValue = 'ws://' + serverValue
}
}
const serverUrl = (serverValue || 'wss://robotics.dev');
console.log('Server:', `${serverUrl}`);
//const socket = io('http://192.168.0.6:3001');
//const socket = io('https://robotics.dev');
// const socket = io(server);
const nodeDataChannel = require('node-datachannel');
const io = require('socket.io-client');
const http = require('http');
const { v4: uuidv4 } = require('uuid');
const PEER_ID = 'peer2'; // Always responder
// node-datachannel specific configuration
const config = {
iceServers: [
'stun:stun1.l.google.com:19302',
'stun:stun2.l.google.com:19302',
'stun:stun3.l.google.com:19302',
'stun:stun4.l.google.com:19302'
],
maxMessageSize: 16000,
enableIceTcp: true,
portRangeBegin: 5000,
portRangeEnd: 6000,
numDataChannels: 10 // Reduced back to 10 for better stability
};
// NEW: Non-queuing message system configuration
const messageConfig = {
enableNonQueuing: true, // RE-ENABLED with improvements
highPriorityTopics: [
'/camera2d',
'/camera/camera/color/image_raw',
'/camera/camera/color/image_raw/compressed',
'/camera/camera/depth/image_rect_raw/compressedDepth',
'/odom',
'cmd_vel'
], // Topics to prioritize for faster sending
pendingMessageTimeout: 2000, // Reduced timeout for faster cleanup (ms)
lowPriorityDelay: 5, // 5ms delay for low priority messages to allow high priority messages first
performanceLogging: false // Disable performance logging to reduce overhead
};
// Validate configuration before creating connection
if (!config.iceServers || !config.iceServers.length) {
throw new Error('Invalid ICE server configuration');
}
// Fetch robot details
var rosTopics = [];
var rosNamespace = false;
var cmdVel = '/cmd_vel';
var camera2dTopic = '/camera2d';
var odomTopic = '/odom';
const portalServer = serverUrl.replace(/^wss:\/\//, 'https://').replace(/^ws:\/\//, 'http://').replace(/\/$/, '');
let activeIceServers = config.iceServers;
async function loadRobotDetails() {
const robotConfig = await fetchRobotConfig(robotId, apiToken, portalServer);
rosNamespace = robotConfig.rosNamespace;
cmdVel = getCmdVelTopic(robotId, robotConfig);
camera2dTopic = resolveTopic('camera2d', robotId, rosNamespace);
odomTopic = resolveTopic('odom', robotId, rosNamespace);
rosTopics = robotConfig.rosTopics ? [...robotConfig.rosTopics] : [];
const hasCamera2d = rosTopics.some(t => t.topic === camera2dTopic);
if (!hasCamera2d) {
rosTopics.push({ type: 'std_msgs/msg/String', topic: camera2dTopic });
}
activeIceServers = await fetchIceServers(apiToken, portalServer);
}
// Add ROS_TOPICS=[{"type":"nav_msgs/msg/Odometry","topic":"/odom"}] to ~/robotics.env file
await loadRobotDetails();
//speak("Initializing robot.");
function speak(msg){
exec(`espeak "${msg}"`)
}
function checkServerStatus() {
return new Promise((resolve, reject) => {
var uptimeUrl;
uptimeUrl = serverUrl.replace('wss://', 'https://');
uptimeUrl = serverUrl.replace('ws://', 'http://');
const request = http.get(uptimeUrl, (response) => {
if (response.statusCode === 200) {
resolve(true);
} else {
reject(new Error(`Unexpected status code: ${response.statusCode}`));
}
}).on('error', () => {
reject(new Error('Signaling server is not running. Please start it first with:\n\nnode signaling-server.js'));
});
request.setTimeout(5000, () => {
request.destroy();
reject(new Error('Server check timed out'));
});
});
}
function formatLog(message) {
const timestamp = new Date().toISOString();
return `[${timestamp}] ${message}`;
}
let rosNode; // Add this at the top level with other global variables
class P2PServer {
constructor() {
this.connections = new Map();
this.socket = null;
this.serverId = robotId;
this.isReconnecting = false;
this.reconnectAttempts = 0;
this.maxReconnectAttempts = 5;
this.reconnectDelay = 2000;
this.pendingReconnections = new Map();
this.connectionAttempts = new Map(); // Track connection attempts per peer
// Initialize ROS camera subscriptions immediately
this.initStandaloneRosCameraSubscriptions();
}
async start() {
console.log(formatLog('Starting P2P server...'));
await this.connect();
// Keep connection alive with exponential backoff
setInterval(() => {
if (!this.socket?.connected) {
console.log(formatLog('Server disconnected, attempting to reconnect...'));
this.connect();
}
}, 2000);
}
// Initialize ROS camera subscriptions independently of P2P connections
async initStandaloneRosCameraSubscriptions() {
try {
console.log(formatLog('Initializing standalone ROS camera subscriptions...'));
// Initialize RCL if not already done
if (!rosNode) {
console.log(formatLog('Initializing RCL for standalone camera subscriptions...'));
await rclnodejs.init();
rosNode = new rclnodejs.Node('robotics_dev_node');
velocityPub = rosNode.createPublisher('geometry_msgs/msg/Twist', `${cmdVel}`);
console.log(formatLog('ROS node created for standalone camera subscriptions'));
}
// Set up camera topic subscriptions immediately
const cameraTopics = [
{ topic: camera2dTopic, type: 'std_msgs/msg/String' },
{ topic: '/camera/camera/color/image_raw', type: 'sensor_msgs/msg/Image' },
{ topic: '/camera/camera/color/image_raw/compressed', type: 'sensor_msgs/msg/CompressedImage' }
];
console.log(formatLog('Setting up standalone camera subscriptions...'));
cameraTopics.forEach(({ topic, type }) => {
try {
console.log(formatLog(`Setting up standalone subscription for ${topic} with type ${type}`));
const subscriber = rosNode.createSubscription(type, topic, async (msg) => {
console.log(formatLog(`📸 Standalone ROS camera message received on topic: ${topic}`));
try {
let modTopic = toClientTopic(topic, robotId, rosNamespace);
// Handle different camera topic types
if (topic === '/camera/camera/color/image_raw/compressed' || topic === '/camera/camera/color/image_raw') {
// Convert image to base64
try {
let imageData;
if (topic === '/camera/camera/color/image_raw/compressed') {
imageData = msg.data || msg;
} else {
if (msg.data && msg.encoding) {
console.log(formatLog(`Processing standalone raw image: ${msg.width}x${msg.height}, encoding: ${msg.encoding}`));
imageData = msg.data;
} else {
console.warn(formatLog('Invalid standalone raw image message format'));
return;
}
}
// Convert to base64 using Sharp
let base64Image;
try {
console.log(formatLog(`🔄 Starting standalone Sharp conversion for ${msg.width}x${msg.height} image`));
const sharpPromise = sharp(imageData, {
raw: {
width: msg.width,
height: msg.height,
channels: 3
}
}).jpeg({ quality: 80 }).toBuffer();
const jpegBuffer = await Promise.race([
sharpPromise,
new Promise((_, reject) =>
setTimeout(() => reject(new Error('Sharp conversion timeout')), 5000)
)
]);
base64Image = jpegBuffer.toString('base64');
console.log(formatLog(`✅ Standalone Sharp conversion successful: ${jpegBuffer.length} bytes -> ${base64Image.length} chars base64`));
} catch (sharpError) {
console.error(formatLog(`❌ Standalone Sharp conversion failed: ${sharpError}`));
// Fallback conversion
if (imageData instanceof Uint8Array) {
base64Image = Buffer.from(imageData).toString('base64');
} else if (Buffer.isBuffer(imageData)) {
base64Image = imageData.toString('base64');
} else {
base64Image = Buffer.from(imageData).toString('base64');
}
console.log(formatLog(`🔄 Using standalone fallback conversion: ${base64Image.length} chars`));
}
// Store in global camera data
globalCameraData.set(modTopic, {
data: base64Image,
timestamp: Date.now()
});
console.log(formatLog(`💾 Stored standalone camera data in global storage for topic: ${modTopic} (${base64Image.length} chars)`));
} catch (error) {
console.error(formatLog(`Error in standalone image conversion: ${error}`));
}
} else if (topic === camera2dTopic) {
// Handle camera2d topic (already base64)
if (msg && typeof msg === 'string' && msg.length > 100) {
globalCameraData.set(modTopic, {
data: msg,
timestamp: Date.now()
});
console.log(formatLog(`💾 Stored standalone camera2d data in global storage (${msg.length} chars)`));
}
}
} catch (error) {
console.error(formatLog(`Error processing standalone camera message: ${error}`));
}
});
// Store subscriber for cleanup
if (!this.standaloneSubscribers) {
this.standaloneSubscribers = [];
}
this.standaloneSubscribers.push(subscriber);
console.log(formatLog(`✅ Successfully set up standalone subscription for ${topic}`));
} catch (err) {
console.error(formatLog(`Error setting up standalone subscription for ${topic}: ${err}`));
}
});
// Start ROS node spin if not already running
if (rosNode && !rosNode.isSpinning) {
console.log(formatLog('Starting standalone ROS node spin...'));
rosNode.spin();
console.log(formatLog('Standalone ROS node is now running'));
}
} catch (error) {
console.error(formatLog(`Error initializing standalone ROS camera subscriptions: ${error}`));
}
}
async connect() {
if (this.isReconnecting) return;
this.isReconnecting = true;
try {
if (this.socket) {
this.socket.disconnect();
}
this.socket = io(serverUrl, {
query: {
id: this.serverId,
robot: robotId,
token: apiToken
},
auth: { id: this.serverId },
reconnection: true,
reconnectionAttempts: Infinity,
reconnectionDelay: this.reconnectDelay,
reconnectionDelayMax: 10000,
timeout: 20000
});
this.socket.on('connect', () => {
console.log(formatLog(`Connected to signaling server: ${this.socket.id}`));
speak("Robot connected.");
this.reconnectAttempts = 0;
this.reconnectDelay = 2000;
loadRobotDetails().catch(err => {
console.error('Error refreshing robot config on connect:', err);
});
// Notify server about existing connections
this.connections.forEach((connection, peerId) => {
if (connection.connectionState === 'connected') {
this.socket.emit('peer-status', {
peerId: peerId,
status: 'connected'
});
}
});
// Process any queued signals for all connections
this.connections.forEach(connection => {
connection.processQueuedSignals();
});
// Process any pending reconnections
this.pendingReconnections.forEach((timestamp, peerId) => {
if (Date.now() - timestamp < 30000) {
this.socket.emit('peer-reconnect', { peerId });
}
});
this.pendingReconnections.clear();
});
this.socket.on('signal', async (message) => {
try {
const sourcePeer = message.sourcePeer ||
(message.auth && message.auth.id);
if (!sourcePeer) {
console.error(formatLog('Missing source peer ID'));
return;
}
const msgType = String(message.type || '').toLowerCase();
console.log(formatLog(`Received ${msgType} from ${sourcePeer}`));
// Every browser offer must get a fresh PeerConnection. Reusing a
// failed/closed PC (or one mid robot-side reconnect) yields no answer.
if (msgType === 'offer') {
if (this.connections.has(sourcePeer)) {
console.log(formatLog(`Replacing existing connection for peer: ${sourcePeer}`));
try {
this.connections.get(sourcePeer).cleanup();
} catch (e) {
console.warn(formatLog(`Error cleaning old connection: ${e.message}`));
}
this.connections.delete(sourcePeer);
}
// Drop other stale peers that are no longer connected (leak prevention)
for (const [peerId, conn] of this.connections.entries()) {
if (peerId !== sourcePeer &&
(conn.connectionState === 'failed' ||
conn.connectionState === 'closed' ||
conn.connectionState === 'disconnected')) {
try { conn.cleanup(); } catch (_) {}
this.connections.delete(peerId);
console.log(formatLog(`Pruned stale connection for peer: ${peerId}`));
}
}
console.log(formatLog(`Creating new connection for peer: ${sourcePeer}`));
const connection = new P2PConnection(sourcePeer, this.socket);
if (!connection.pc) {
console.error(formatLog(`Failed to create PeerConnection for ${sourcePeer}`));
return;
}
this.connections.set(sourcePeer, connection);
this.connectionAttempts.set(sourcePeer, 0);
} else if (!this.connections.has(sourcePeer)) {
console.log(formatLog(`Creating new connection for peer: ${sourcePeer}`));
const connection = new P2PConnection(sourcePeer, this.socket);
this.connections.set(sourcePeer, connection);
this.connectionAttempts.set(sourcePeer, 0);
}
message.type = msgType;
await this.connections.get(sourcePeer).handleSignal(message);
} catch (error) {
console.error(formatLog(`Signal error: ${error.stack || error}`));
}
});
this.socket.on('peer-reconnect', async (data) => {
const { peerId } = data;
console.log(formatLog(`Received reconnection request from peer: ${peerId}`));
if (this.connections.has(peerId)) {
const connection = this.connections.get(peerId);
const attempts = this.connectionAttempts.get(peerId) || 0;
if (attempts < 5) { // Limit reconnection attempts per peer
this.connectionAttempts.set(peerId, attempts + 1);
this.pendingReconnections.set(peerId, Date.now());
await connection.reconnect();
} else {
console.log(formatLog(`Max reconnection attempts reached for peer ${peerId}`));
this.connections.delete(peerId);
this.connectionAttempts.delete(peerId);
}
}
});
this.socket.on('peer-status', (data) => {
const { peerId, status } = data;
console.log(formatLog(`Peer ${peerId} status: ${status}`));
if (this.connections.has(peerId)) {
const connection = this.connections.get(peerId);
if (status === 'disconnected' && connection.connectionState === 'connected') {
connection.handleReconnect();
}
}
});
this.socket.on("twist", (data, callback) => {
// console.log("TWIST:", data);
try{
velocityPub.publish(data);
} catch(error){
exec(`ros2 topic pub --once ${cmdVel} geometry_msgs/Twist "{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}"`);
console.log('aborted', error);
}
});
this.socket.on("speak", (data) => {
// console.log("Speak", data);
speak(data);
});
this.socket.on("photo-request", async (data) => {
const { requestId } = data;
console.log(formatLog(`📸 Photo request received with requestId: ${requestId}`));
try {
// Get the latest camera image from any of the three camera topics
let base64Photo = null;
let sourceTopic = null;
// Check each camera topic in order of preference
const cameraTopics = ['/camera2d', '/camera/camera/color/image_raw', '/camera/camera/color/image_raw/compressed'];
// First, check global camera data (independent of P2P connections)
console.log(formatLog(`🔍 Checking global camera data for ${cameraTopics.length} topics`));
console.log(formatLog(`🔍 Available global camera topics: ${Array.from(globalCameraData.keys()).join(', ')}`));
for (const topic of cameraTopics) {
if (globalCameraData.has(topic)) {
const cameraData = globalCameraData.get(topic);
const age = Date.now() - cameraData.timestamp;
console.log(formatLog(`🔍 Found global camera data for ${topic}: age=${age}ms, dataLength=${cameraData.data ? cameraData.data.length : 'undefined'}`));
// Only use data that's less than 5 seconds old
if (age < 5000 && cameraData.data && typeof cameraData.data === 'string' && cameraData.data.length > 100) {
base64Photo = cameraData.data;
sourceTopic = topic;
console.log(formatLog(`📸 Found latest camera image from global storage: ${topic} (${base64Photo.length} chars, age: ${age}ms) - STANDALONE MODE`));
break;
} else {
console.log(formatLog(`⚠️ Global camera data for ${topic} is too old (${age}ms) or invalid`));
}
} else {
console.log(formatLog(`🔍 No global camera data found for topic: ${topic}`));
}
}
// Fallback: Search through all active connections for camera data
if (!base64Photo) {
for (const [peerId, connection] of this.connections) {
if (connection.connectionState === 'connected' && connection.pendingMessages) {
for (const topic of cameraTopics) {
if (connection.pendingMessages.has(topic)) {
const messageData = connection.pendingMessages.get(topic);
const message = messageData.message;
// Check if the message has base64 data
if (message.data && typeof message.data === 'string' && message.data.length > 100) {
base64Photo = message.data;
sourceTopic = topic;
console.log(formatLog(`📸 Found latest camera image from topic: ${topic} (${base64Photo.length} chars) via peer: ${peerId}`));
break;
}
}
}
if (base64Photo) break; // Found data, no need to check other connections
}
}
}
if (base64Photo) {
// Send the photo back to the server with the same requestId
this.socket.emit('photo-response', {
requestId: requestId,
base64: base64Photo
});
console.log(formatLog(`✅ Photo response sent for requestId: ${requestId} from topic: ${sourceTopic}`));
} else {
// No camera data available
this.socket.emit('photo-response', {
requestId: requestId,
error: 'No camera data available'
});
console.log(formatLog(`❌ No camera data available for requestId: ${requestId}`));
}
} catch (error) {
console.error(formatLog(`❌ Error processing photo request: ${error}`));
// Send error response
this.socket.emit('photo-response', {
requestId: requestId,
error: 'Failed to capture photo'
});
}
});
this.socket.on("bash-script", (data, callback) => {
console.log("BASH:", data);
try{
exec(data);
} catch(error){
exec(`ros2 topic pub --once ${cmdVel} geometry_msgs/Twist "{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}"`);
console.log('aborted');
}
});
this.socket.on('disconnect', (reason) => {
console.log(formatLog(`Disconnected from signaling server: ${reason}`));
// Don't cleanup P2P connections on WebSocket disconnect
this.handleReconnect();
});
this.socket.on('connect_error', (error) => {
console.error(formatLog(`Connection error: ${error}`));
this.handleReconnect();
});
} catch (error) {
console.error(formatLog(`Connection error: ${error}`));
this.handleReconnect();
} finally {
this.isReconnecting = false;
}
}
handleReconnect() {
if (this.reconnectAttempts < this.maxReconnectAttempts) {
this.reconnectAttempts++;
this.reconnectDelay = Math.min(this.reconnectDelay * 1.5, 10000); // Exponential backoff with max 10s
console.log(formatLog(`Attempting reconnect in ${this.reconnectDelay}ms (attempt ${this.reconnectAttempts}/${this.maxReconnectAttempts})`));
setTimeout(() => this.connect(), this.reconnectDelay);
} else {
console.error(formatLog('Max reconnection attempts reached. Please check server status.'));
}
}
cleanup() {
// Only cleanup when explicitly requested, not on WebSocket disconnects
this.connections.forEach(conn => conn.cleanup());
this.connections.clear();
if (this.socket) this.socket.close();
// Cleanup standalone subscribers
if (this.standaloneSubscribers) {
this.standaloneSubscribers.forEach(sub => {
if (sub) {
if (typeof sub.destroy === 'function') {
try { sub.destroy(); } catch (e) { console.warn('destroy() error', e); }
} else if (typeof sub.unsubscribe === 'function') {
try { sub.unsubscribe(); } catch (e) { console.warn('unsubscribe() error', e); }
} else if (typeof sub.close === 'function') {
try { sub.close(); } catch (e) { console.warn('close() error', e); }
}
}
});
this.standaloneSubscribers = [];
}
}
}
class P2PConnection {
constructor(peerId, socket) {
if (!peerId) {
throw new Error('Peer ID is required');
}
this.peerId = peerId;
this.socket = socket;
this.initializePeerConnection();
this.dataChannels = new Map();
this.currentChannelIndex = 0;
this.maxChannels = 10;
this.activeChannels = new Set();
this.channelCreationQueue = [];
this.isCreatingChannels = false;
this.hasRemoteDescription = false;
this.candidateQueue = [];
this.dataChannelOpen = false;
this.isDataChannelReady = false;
// setupHandlers() is called from initializePeerConnection()
this.messages = new Map();
this.subscribers = [];
this.currentDataChannel = null;
this.connectionState = 'new';
this.abortSend = false;
this.rosPaused = false;
this.isWebSocketConnected = true;
this.reconnectTimer = null;
this.reconnectAttempts = 0;
this.maxReconnectAttempts = Infinity;
this.lastReconnectAttempt = 0;
this.reconnectCooldown = 2000;
this.isReconnecting = false;
this.iceGatheringState = 'new';
this.signalingState = 'stable';
this.lastSignalTime = Date.now();
this.signalTimeout = 60000;
this.iceRestartTimer = null;
this.iceRestartInterval = 300000;
this.forceIceRestart = false;
this.connectionTimeout = null;
this.activeSends = new Set();
this.channelErrors = new Map();
this.dataChannelRetryTimer = null;
this.dataChannelRetryInterval = 5000;
this.maxDataChannelRetries = Infinity;
this.dataChannelRetryCount = 0;
this.latestMessages = new Map();
this.lastSentTime = new Map();
this.minSendInterval = 33;
this.lastActivityTime = Date.now();
this.keepAliveInterval = 30000;
this.keepAliveTimer = null;
this.lastPingTime = 0;
this.pingInterval = 30000; // 30 seconds between pings
// NEW: Non-queuing message system
this.pendingMessages = new Map(); // topic -> latest message
this.sendingMessages = new Set(); // topics currently being sent
this.messageSendQueue = []; // simple queue for non-ROS messages (like ping)
this.isProcessingQueue = false;
// Memory management
this.messageCleanupTimer = null;
this.messageCleanupInterval = 60000; // Clean up old messages every minute
this.maxMessageAge = 300000; // 5 minutes
this.maxMessagesPerPeer = 100; // Limit messages per peer
this.startMessageCleanup();
// NEW: Aggressive cleanup for non-queuing system
this.aggressiveCleanupTimer = null;
this.aggressiveCleanupInterval = 1000; // Clean up every second
this.startAggressiveCleanup();
}
initializePeerConnection() {
if (this.pc) {
try {
this.pc.close();
} catch (error) {
console.error(formatLog(`Error closing existing peer connection: ${error}`));
}
}
console.log(formatLog('Initializing new peer connection'));
try {
let iceServers = Array.isArray(activeIceServers)
? activeIceServers.filter((s) => typeof s === 'string')
: [];
// node-datachannel rejects browser-style {urls,username,credential} objects.
// Fall back to built-in STUN strings so teleop can still answer.
if (!iceServers.length) {
console.warn(formatLog('No valid ICE strings in activeIceServers — using default STUN'));
iceServers = config.iceServers;
}
this.pc = new nodeDataChannel.PeerConnection(this.peerId, {
iceServers,
maxMessageSize: config.maxMessageSize,
enableIceTcp: true,
portRangeBegin: 0,
portRangeEnd: 65535,
enableIceTrickle: true,
iceRole: 'controlled'
});
// Clear any existing timers
this.clearAllTimers();
// Start keepalive timer
this.keepAliveTimer = setInterval(() => {
if (this.connectionState === 'connected') {
const now = Date.now();
if (now - this.lastPingTime >= this.pingInterval) {
console.log(formatLog('Sending keepalive ping'));
try {
this.queueMessage({ type: 'ping' });
this.lastPingTime = now;
} catch (error) {
console.error(formatLog(`Error sending keepalive: ${error}`));
}
}
}
}, this.pingInterval);
// Handlers must be (re)bound whenever a new native PC is created
this.setupHandlers();
return true;
} catch (error) {
console.error(formatLog(`Error initializing peer connection: ${error}`));
this.pc = null;
return false;
}
}
clearAllTimers() {
if (this.keepAliveTimer) {
clearInterval(this.keepAliveTimer);
this.keepAliveTimer = null;
}
if (this.iceRestartTimer) {
clearInterval(this.iceRestartTimer);
this.iceRestartTimer = null;
}
if (this.dataChannelRetryTimer) {
clearInterval(this.dataChannelRetryTimer);
this.dataChannelRetryTimer = null;
}
if (this.reconnectTimer) {
clearTimeout(this.reconnectTimer);
this.reconnectTimer = null;
}
if (this.connectionTimeout) {
clearTimeout(this.connectionTimeout);
this.connectionTimeout = null;
}
if (this.messageCleanupTimer) {
clearInterval(this.messageCleanupTimer);
this.messageCleanupTimer = null;
}
if (this.aggressiveCleanupTimer) {
clearInterval(this.aggressiveCleanupTimer);
this.aggressiveCleanupTimer = null;
}
}
startMessageCleanup() {
this.messageCleanupTimer = setInterval(() => {
this.cleanupOldMessages();
}, this.messageCleanupInterval);
}
// NEW: Aggressive cleanup for non-queuing system
startAggressiveCleanup() {
this.aggressiveCleanupTimer = setInterval(() => {
this.aggressiveCleanup();
}, this.aggressiveCleanupInterval);
}
aggressiveCleanup() {
if (!messageConfig.enableNonQueuing) return;
const now = Date.now();
let cleanedCount = 0;
// Clean up very old pending messages (older than 500ms)
for (const [topic, messageData] of this.pendingMessages.entries()) {
if (now - messageData.timestamp > 500) {
this.pendingMessages.delete(topic);
cleanedCount++;
}
}
// Clean up stuck sending messages (older than 1 second)
for (const topic of this.sendingMessages) {
const messageData = this.pendingMessages.get(topic);
if (messageData && (now - messageData.timestamp > 1000)) {
this.sendingMessages.delete(topic);
this.pendingMessages.delete(topic);
cleanedCount++;
}
}
if (cleanedCount > 0) {
console.log(formatLog(`Aggressive cleanup: removed ${cleanedCount} stale messages`));
}
}
cleanupOldMessages() {
const now = Date.now();
let cleanedCount = 0;
// Clean up old messages
for (const [messageId, message] of this.messages.entries()) {
if (now - message.timestamp > this.maxMessageAge) {
this.messages.delete(messageId);
cleanedCount++;
}
}
// NEW: Clean up stale pending messages
for (const [topic, messageData] of this.pendingMessages.entries()) {
if (now - messageData.timestamp > messageConfig.pendingMessageTimeout) {
this.pendingMessages.delete(topic);
cleanedCount++;
}
}
// Limit total messages per peer
if (this.messages.size > this.maxMessagesPerPeer) {
const entries = Array.from(this.messages.entries());
entries.sort((a, b) => a[1].timestamp - b[1].timestamp);
const toDelete = entries.slice(0, this.messages.size - this.maxMessagesPerPeer);
toDelete.forEach(([messageId]) => {
this.messages.delete(messageId);
cleanedCount++;
});
}
if (cleanedCount > 0) {
console.log(formatLog(`Cleaned up ${cleanedCount} old messages for peer ${this.peerId}`));
}
}
async retryDataChannel() {
if (this.dataChannelRetryCount >= this.maxDataChannelRetries) {
console.log(formatLog('Max data channel retry attempts reached'));
return;
}
try {
console.log(formatLog(`Attempting to create data channel (attempt ${this.dataChannelRetryCount + 1}/${this.maxDataChannelRetries})`));
const dc = this.pc.createDataChannel(`robotics-${this.currentChannelIndex++}`);
this.setupDataChannel(dc);
this.dataChannelRetryCount++;
} catch (error) {
console.error(formatLog(`Error creating data channel: ${error}`));
}
}
async restartIce() {
if (!this.pc || this.connectionState !== 'connected') return;
try {
// console.log(formatLog('Initiating ICE restart'));
const offer = this.pc.createOffer({ iceRestart: true });
await this.pc.setLocalDescription(offer);
this.forceIceRestart = false;
} catch (error) {
// console.error(formatLog(`Error during ICE restart: ${error}`));
}
}
setupHandlers() {
this.pc.onLocalDescription((sdp, type) => {
console.log(formatLog(`Generated local ${type}`));
console.log(formatLog(`SDP content: ${sdp}`));
// Normalize to lowercase for browser RTCPeerConnection
const signalType = String(type || '').toLowerCase();
if (this.socket?.connected) {
this.sendSignal({
type: signalType,
sdp: String(sdp)
});
} else {
console.log(formatLog('WebSocket disconnected, queuing signal for later'));
this.candidateQueue.push({
type: signalType,
sdp: String(sdp)
});
}
});
this.pc.onLocalCandidate((candidate, mid) => {
if (candidate) {
console.log(formatLog(`Generated local candidate for mid: ${mid || '0'}`));
console.log(formatLog(`Candidate content: ${candidate}`));
if (this.socket?.connected) {
this.sendSignal({
type: 'candidate',
candidate: String(candidate),
mid: String(mid || '0')
});
} else {
console.log(formatLog('WebSocket disconnected, queuing candidate for later'));
this.candidateQueue.push({
type: 'candidate',
candidate: String(candidate),
mid: String(mid || '0')
});
}
}
});
this.pc.onStateChange((state) => {
console.log(formatLog(`Connection state for ${this.peerId}: ${state}`));
const oldState = this.connectionState;
this.connectionState = state;
if (state === 'connected') {
console.log(formatLog('Peer connection established'));
this.dataChannelOpen = true;
this.isDataChannelReady = true;
this.rosPaused = false;
this.reconnectAttempts = 0;
this.isReconnecting = false;
this.lastActivityTime = Date.now();
if (this.reconnectTimer) {
clearTimeout(this.reconnectTimer);
this.reconnectTimer = null;
}
if (this.socket?.connected) {
this.socket.emit('peer-status', {
peerId: this.peerId,
status: 'connected'
});
}
// Create data channel if not already created
if (!this.currentDataChannel) {
console.log(formatLog('Creating data channel after connection established'));
try {
const dc = this.pc.createDataChannel('robotics-0', {
ordered: true,
maxRetransmits: 3,
protocol: 'binary'
});
this.setupDataChannel(dc);
this.activeChannels.add('robotics-0');
console.log(formatLog('Data channel created after connection'));
} catch (error) {
console.error(formatLog(`Error creating data channel after connection: ${error}`));
}
}
} else if (state === 'disconnected' || state === 'failed' || state === 'closed') {
console.log(formatLog(`Peer connection ${state}`));
this.dataChannelOpen = false;
this.isDataChannelReady = false;
this.rosPaused = true;
// Do not call createOffer()-based reconnect here — native PeerConnection
// has no createOffer/createAnswer, and teleop browser is the offerer.
// Wait for a fresh browser offer (handler replaces this connection).
if (this.socket?.connected) {
this.socket.emit('peer-status', {
peerId: this.peerId,
status: 'disconnected'
});
}
}
});
this.pc.onDataChannel((dc) => {
console.log(formatLog(`Data channel from ${this.peerId}: ${dc.getLabel()}`));
try {
this.setupDataChannel(dc);
if(firstRun){
this.initRcl(dc);
}
} catch (error) {
console.error(formatLog(`Error setting up data channel: ${error}`));
}
});
this.pc.onGatheringStateChange((state) => {
console.log(formatLog(`ICE gathering state: ${state}`));
this.iceGatheringState = state;
});
this.pc.onSignalingStateChange((state) => {
console.log(formatLog(`Signaling state: ${state}`));
this.signalingState = state;
});
}
async handleSignal(message) {
try {
console.log(formatLog(`Handling signal of type: ${message.type}`));
console.log(formatLog(`Signal content: ${JSON.stringify(message)}`));
this.lastSignalTime = Date.now();
if (message.type === 'offer') {
console.log(formatLog('Setting remote description (offer)'));
try {
// Set remote description — node-datachannel then generates an
// answer via onLocalDescription (there is no createAnswer()).
const sdp = message.sdp;
console.log(formatLog(`Setting remote SDP: ${sdp}`));
this.pc.setRemoteDescription(sdp, 'offer');
this.hasRemoteDescription = true;
// Process any queued candidates first
if (this.candidateQueue.length > 0) {
console.log(formatLog(`Processing ${this.candidateQueue.length} queued candidates`));
for (const candidate of this.candidateQueue) {
try {
console.log(formatLog(`Adding queued candidate: ${candidate.candidate}`));
await this.pc.addRemoteCandidate(candidate.candidate, candidate.mid || '0');
} catch (e) {
console.warn(formatLog(`Error adding queued candidate: ${e.message}`));
}
}
this.candidateQueue = [];
}
// Answer is sent by onLocalDescription. Do not call createAnswer()
// — it does not exist on the native PeerConnection and throws.
} catch (error) {
console.error(formatLog(`Error in offer handling: ${error}`));
throw error;
}
} else if (message.type === 'answer' || message.type === 'Answer') {
console.log(formatLog('Setting remote description (answer)'));
try {
const sdp = message.sdp;