robotics
Version:
Robotics.dev P2P ROS2 robot controller CLI with ROS telemetry and video streaming
1,111 lines (989 loc) • 67.3 kB
JavaScript
process.on('uncaughtException', (err) => {
console.error('An uncaught exception occurred:', err);
});
process.on('unhandledRejection', (reason, promise) => {
console.error('Unhandled rejection:', reason);
});
import {createRequire } from "module";
const require = createRequire(import.meta.url);
import Configstore from 'configstore';
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');
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
};
// Validate configuration before creating connection
if (!config.iceServers || !config.iceServers.length) {
throw new Error('Invalid ICE server configuration');
}
// Fetch robot details
var rosTopics = [];
var cmdVel = `/robot${robotId.replace(/-/g, "")}/cmd_vel`
async function fetchRobotDetails() {
try {
console.log("ROS Fetch", robotId, apiToken);
if(robotId && apiToken){
const response = await fetch(`https://robotics.dev/robot/${robotId}`, {
headers: {
"Content-Type": "application/json",
"api_token": apiToken
}
});
const data = await response.json();
// console.log('Robot Details:', data);
if(data.cmdVel && data.cmdVel !== ""){
cmdVel = data.cmdVel;
}
if(data.rosTopics){
rosTopics = data.rosTopics;
}
rosTopics.push({"type":"std_msgs/msg/String","topic":`/robot${robotId.replace(/-/g, "")}/camera2d`})
}
} catch (error) {
console.error('Error fetching robot details:', error);
}
}
// Add ROS_TOPICS=[{"type":"nav_msgs/msg/Odometry","topic":"/odom"}] to ~/robotics.env file
await fetchRobotDetails();
//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
}
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);
}
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;
// 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;
}
console.log(formatLog(`Received ${message.type} from ${sourcePeer}`));
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);
}
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("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();
}
}
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;
this.setupHandlers();
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
}
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 {
// Simplified ICE configuration for better compatibility
this.pc = new nodeDataChannel.PeerConnection(this.peerId, {
iceServers: [
'stun:stun.l.google.com:19302'
],
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.sendCompressedMessage({ type: 'ping' });
this.lastPingTime = now;
} catch (error) {
console.error(formatLog(`Error sending keepalive: ${error}`));
}
}
}
}, this.pingInterval);
// Setup handlers
this.setupHandlers();
return true;
} catch (error) {
console.error(formatLog(`Error initializing peer connection: ${error}`));
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;
}
}
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}`));
if (this.socket?.connected) {
this.sendSignal({
type,
sdp: String(sdp)
});
} else {
console.log(formatLog('WebSocket disconnected, queuing signal for later'));
this.candidateQueue.push({
type,
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}`));
if (oldState === 'connected') {
this.dataChannelOpen = false;
this.isDataChannelReady = false;
this.rosPaused = true;
if (!this.isReconnecting) {
console.log(formatLog(`Initiating reconnection due to ${state} state`));
this.handleReconnect();
}
}
}
});
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
const sdp = message.sdp;
console.log(formatLog(`Setting remote SDP: ${sdp}`));
await 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 = [];
}
// Create and send answer
console.log(formatLog('Creating answer...'));
const answer = this.pc.createAnswer();
console.log(formatLog(`Created answer: ${answer}`));
await this.pc.setLocalDescription(answer);
console.log(formatLog('Answer created and set as local description'));
// Create data channel after setting local description
console.log(formatLog('Creating data channel...'));
const dc = this.pc.createDataChannel('robotics-0', {
ordered: true,
maxRetransmits: 3,
protocol: 'binary'
});
console.log(formatLog('Data channel created, setting up handlers...'));
this.setupDataChannel(dc);
this.activeChannels.add('robotics-0');
console.log(formatLog('Data channel setup complete'));
} catch (error) {
console.error(formatLog(`Error in offer handling: ${error}`));
throw error;
}
} else if (message.type === 'answer') {
console.log(formatLog('Setting remote description (answer)'));
try {
const sdp = message.sdp;
console.log(formatLog(`Setting remote SDP: ${sdp}`));
await this.pc.setRemoteDescription(sdp, 'answer');
this.hasRemoteDescription = true;
// Create data channel if not already created
if (!this.currentDataChannel) {
console.log(formatLog('Creating data channel after answer'));
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 answer'));
} catch (error) {
console.error(formatLog(`Error creating data channel after answer: ${error}`));
}
}
} catch (error) {
console.error(formatLog(`Error in answer handling: ${error}`));
throw error;
}
} else if (message.type === 'candidate') {
console.log(formatLog(`Received ICE candidate for mid: ${message.mid}`));
console.log(formatLog(`Candidate content: ${message.candidate}`));
try {
if (this.hasRemoteDescription) {
console.log(formatLog(`Adding remote candidate with mid: ${message.mid || '0'}`));
await this.pc.addRemoteCandidate(message.candidate, message.mid || '0');
} else {
console.log(formatLog('Queuing candidate - no remote description yet'));
this.candidateQueue.push(message);
}
} catch (error) {
console.error(formatLog(`Error in candidate handling: ${error}`));
throw error;
}
}
} catch (error) {
console.error(formatLog(`Error handling signal: ${error}`));
this.handleReconnect();
}
}
sendSignal(message) {
if (!this.socket?.connected) {
console.log(formatLog('WebSocket not connected, queuing signal'));
this.candidateQueue.push(message);
return;
}
this.socket.emit('signal', {
...message,
targetPeer: this.peerId
});
}
// Add method to process queued signals when WebSocket reconnects
processQueuedSignals() {
if (this.candidateQueue.length > 0) {
console.log(formatLog(`Processing ${this.candidateQueue.length} queued signals`));
while (this.candidateQueue.length > 0) {
const signal = this.candidateQueue.shift();
this.sendSignal(signal);
}
}
}
async createDataChannels() {
if (this.isCreatingChannels) return;
this.isCreatingChannels = true;
try {
console.log(formatLog('Creating data channels...'));
// Create initial channels
for (let i = 0; i < this.maxChannels; i++) {
const channelName = `robotics-${i}`;
try {
console.log(formatLog(`Creating data channel: ${channelName}`));
const dc = this.pc.createDataChannel(channelName, {
ordered: true,
maxRetransmits: 3,
protocol: 'binary'
});
this.setupDataChannel(dc);
this.activeChannels.add(channelName);
console.log(formatLog(`Data channel ${channelName} created successfully`));
} catch (error) {
console.error(formatLog(`Error creating data channel ${channelName}: ${error}`));
}
}
} catch (error) {
console.error(formatLog(`Error in createDataChannels: ${error}`));
} finally {
this.isCreatingChannels = false;
}
}
setupVideoStreaming(dc) {
if (!rosNode) {
console.error('ROS node not initialized');
return;
}
this.cleanupSubscriptions();
// Subscribe to camera topic for browser peers
const cameraTopic = `/robot${robotId.replace(/-/g, "")}/camera2d`;
console.log(formatLog(`Setting up video streaming for topic: ${cameraTopic}`));
try {
const subscriber = rosNode.createSubscription('std_msgs/msg/String', cameraTopic, (msg) => {
if (this.rosPaused) return;
if (this.dataChannelOpen && dc.isOpen()) {
try {
const now = Date.now();
const lastSent = this.lastSentTime.get(cameraTopic) || 0;
// Only send if enough time has passed since last send
if (now - lastSent >= this.minSendInterval) {
console.log(formatLog(`Sending video data to browser peer`));
this.sendCompressedMessage({
robotId: robotId,
topic: '/camera2d',
data: msg
});
this.lastSentTime.set(cameraTopic, now);
}
} catch (error) {
console.warn(formatLog(`Failed to send video message: ${error}`));
}
} else {
console.warn(formatLog(`Data channel not open for video streaming`));
}
});
this.subscribers.push(subscriber);
console.log(formatLog(`Successfully subscribed to video topic ${cameraTopic}`));
} catch (err) {
console.error(formatLog(`Error setting up video subscription: ${err}`));
}
}
async sendCompressedMessage(msg) {
try {
const compressed = JSON.stringify(msg);
const compressedSize = compressed.length;
const messageId = uuidv4();
// Use 75% of maxMessageSize for actual data to ensure room for header
const actualChunkSize = Math.floor(config.maxMessageSize * 0.75);
const totalChunks = Math.ceil(compressedSize / actualChunkSize);
const openChannels = Array.from(this.dataChannels.values())
.filter(ch => ch.isOpen() && this.activeChannels.has(ch.getLabel()));
if (openChannels.length === 0) {
console.warn(formatLog('No open channels available. Aborting send.'));
return;
}
this.activeSends.add(messageId);
for (let i = 0; i < totalChunks; i++) {
if (this.abortSend) {
console.warn(formatLog(`Send aborted at chunk ${i + 1} due to disconnect or error.`));
this.activeSends.delete(messageId);
return;
}
const channel = openChannels[i % openChannels.length];
try {
// Create header (8 bytes for index, 8 bytes for total, 36 bytes for messageId)
const header = Buffer.alloc(52);
header.writeBigUInt64BE(BigInt(i), 0);
header.writeBigUInt64BE(BigInt(totalChunks), 8);
header.write(messageId.replace(/-/g, ''), 16, 'hex');
// Get chunk data
const chunkData = compressed.slice(i * actualChunkSize, (i + 1) * actualChunkSize);
const chunkBuffer = Buffer.from(chunkData);
// Combine header and chunk
const messageBuffer = Buffer.concat([header, chunkBuffer]);
if (messageBuffer.length > config.maxMessageSize) {
console.warn(formatLog(`Chunk ${i + 1} too large (${messageBuffer.length} bytes), skipping`));
continue;
}
channel.sendMessageBinary(messageBuffer);
} catch (sendError) {
console.error(formatLog(`Error sending on channel ${channel.getLabel()}: ${sendError}`));
this.channelErrors.set(channel.getLabel(), sendError);
this.activeChannels.delete(channel.getLabel());
if (this.activeChannels.size === 0) {
this.abortSend = true;
this.activeSends.delete(messageId);
return;
}
}
}
this.activeSends.delete(messageId);
} catch (error) {
console.error(formatLog(`Send error: ${error}`));
this.activeSends.clear();
}
}
setupDataChannel(dc) {
console.log(formatLog(`Setting up data channel: ${dc.getLabel()}`));
this.currentDataChannel = dc;
this.dataChannels.set(0, dc); // Store the channel with index 0
try {
console.log(formatLog(`Initial channel state for ${dc.getLabel()}: ${dc.isOpen() ? 'open' : 'closed'}`));
dc.onOpen(() => {
console.log(formatLog(`📡 Data channel opened with label: ${dc.getLabel()}`));
this.dataChannelOpen = true;
this.isDataChannelReady = true;
this.abortSend = false;
this.rosPaused = false;
this.channelErrors.delete(dc.getLabel());
this.activeChannels.add(dc.getLabel());
// Check if this is a browser peer
const isBrowserPeer = this.peerId.startsWith('browser-');
console.log(formatLog(`Peer ${this.peerId} is ${isBrowserPeer ? 'browser' : 'non-browser'}`));
// Setup ROS subscriptions immediately when channel opens
try {
if (isBrowserPeer) {
// For browser peers, prioritize video streaming
console.log(formatLog('Setting up video streaming for browser peer'));
this.setupVideoStreaming(dc);
// Also setup other ROS topics for browser peers
console.log(formatLog('Setting up additional ROS topics for browser peer'));
this.setupRosSubscriptions(dc);
} else {
// For non-browser peers, setup regular ROS subscriptions
console.log(formatLog('Setting up ROS subscriptions for non-browser peer'));
this.setupRosSubscriptions(dc);
}
if(firstRun){
this.initRcl(dc);
}
} catch (setupError) {
console.error(formatLog(`Error in setup after channel open: ${setupError}`));
}
});
dc.onMessage((msg) => {
console.log(formatLog(`📥 Received message on ${dc.getLabel()} from peer ${this.peerId}`));
try {
if (msg instanceof Buffer) {
// Handle binary messages
console.log(formatLog(`📥 Received binary message (${msg.length} bytes) from peer ${this.peerId}`));
// Ensure we have enough bytes for the header
if (msg.length < 52) {
console.warn(formatLog(`Message too short (${msg.length} bytes), expected at least 52 bytes from peer ${this.peerId}`));
return;
}
try {
// Parse binary message header
const index = Number(msg.readBigUInt64BE(0));
const total = Number(msg.readBigUInt64BE(8));
// Validate array lengths
if (isNaN(index) || isNaN(total) || index < 0 || total <= 0 || index >= total) {
console.warn(formatLog(`Invalid message header from peer ${this.peerId}: index=${index}, total=${total}`));
return;
}
const messageId = msg.slice(16, 52).toString('hex').replace(/(.{8})(.{4})(.{4})(.{4})(.{12})/, '$1-$2-$3-$4-$5');
const chunkData = msg.slice(52).toString();
console.log(formatLog(`📥 Processing chunk ${index + 1}/${total} for message ${messageId} from peer ${this.peerId}`));
// Reconstruct message if needed
if (!this.messages.has(messageId)) {
console.log(formatLog(`📥 Starting new message reconstruction for ${messageId} from peer ${this.peerId}`));
this.messages.set(messageId, {
chunks: new Array(total).fill(null),
received: 0,
total: total
});
}
const message = this.messages.get(messageId);
message.chunks[index] = chunkData;
message.received++;
// If all chunks received, process complete message
if (message.received === message.total) {
const completeMessage = message.chunks.join('');
console.log(formatLog(`📥 Complete message received from peer ${this.peerId}: ${completeMessage}`));
try {
// Try to parse the complete message
let dataObj;
try {
// First parse the outer message
const outerObj = JSON.parse(completeMessage);
console.log(formatLog(`📥 Parsed outer message from peer ${this.peerId}: ${JSON.stringify(outerObj)}`));
// Then parse the inner message in the data field if it exists
if (outerObj.data) {
try {
dataObj = JSON.parse(outerObj.data);
console.log(formatLog(`📥 Parsed inner message from peer ${this.peerId}: ${JSON.stringify(dataObj)}`));
} catch (e) {
console.log(formatLog(`Failed to parse inner message from peer ${this.peerId}: ${e.message}`));
dataObj = outerObj; // Use outer object if inner parse fails
}
} else {
dataObj = outerObj; // Use outer object if no data field
}
// Handle twist commands
if (dataObj.topic === "twist") {
console.log(formatLog(`Processing twist command from peer ${this.peerId}: ${JSON.stringify(dataObj.twist)}`));
velocityPub.publish(dataObj.twist);
} else if (dataObj.twist) {
// Direct twist object
console.log(formatLog(`Processing direct twist object from peer ${this.peerId}: ${JSON.stringify(dataObj.twist)}`));
velocityPub.publish(dataObj.twist);
}
// Handle speak commands
if (dataObj.topic === "speak") {
console.log(formatLog(`Processing speak command from peer ${this.peerId}: ${dataObj.text}`));
speak(dataObj.text);
}
} catch (e) {
console.log(formatLog(`Failed to parse message from peer ${this.peerId}: ${e.message}`));
// If parsing fails, try direct command format
if (completeMessage.startsWith('twist:')) {
try {
const twistData = JSON.parse(completeMessage.slice(6));
console.log(formatLog(`Processing direct twist command from peer ${this.peerId}: ${JSON.stringify(twistData)}`));
velocityPub.publish(twistData);
} catch (e) {
console.error(formatLog(`Failed to parse twist command from peer ${this.peerId}: ${e.message}`));
}
} else if (completeMessage.startsWith('speak:')) {
const speakText = completeMessage.slice(6);
console.log(formatLog(`Processing direct speak command from peer ${this.peerId}: ${speakText}`));
speak(speakText);
}
}
} catch (error) {
console.error(formatLog(`Error parsing complete message from peer ${this.peerId}: ${error}`));
}
this.messages.delete(messageId);
} else {
console.log(formatLog(`Received chunk ${index + 1}/${total} for message ${messageId} from peer ${this.peerId}`));
}
} catch (parseError) {
console.error(formatLog(`Error parsing binary message from peer ${this.peerId}: ${parseError}`));
}
} else {
// Handle legacy string messages
console.log(formatLog(`📥 Received legacy message from peer ${this.peerId} (raw): ${msg}`));
console.log(formatLog(`📥 Message type from peer ${this.peerId}: ${typeof msg}`));
console.log(formatLog(`📥 Message length from peer ${this.peerId}: ${msg.length}`));
try {
// Handle the message directly
if (typeof msg === 'string') {
let dataObj;
try {
// First try to parse as JSON
dataObj = JSON.parse(msg);
console.log(formatLog(`📥 Parsed message as JSON from peer ${this.peerId}: ${JSON.stringify(dataObj)}`));
// Handle twist commands
if (dataObj.topic === "twist") {
console.log(formatLog(`Processing twist command from peer ${this.peerId}: ${JSON.stringify(dataObj.twist)}`));
velocityPub.publish(dataObj.twist);
} else if (dataObj.twist) {
// Direct twist object
console.log(formatLog(`Processing direct twist object from peer ${this.peerId}: ${JSON.stringify(dataObj.twist)}`));
velocityPub.publish(dataObj.twist);
}
// Handle speak commands
if (dataObj.topic === "speak") {
console.log(formatLog(`Processing speak command from peer ${this.peerId}: ${dataObj.text}`));
speak(dataObj.text);
}
} catch (e) {
console.log(formatLog(`Failed to parse as JSON from peer ${this.peerId}: ${e.message}`));
// If parsing fails, try direct command format
if (msg.startsWith('twist:')) {
try {
const twistData = JSON.parse(msg.slice(6));
console.log(formatLog(`Processing direct twist command from peer ${this.peerId}: ${JSON.stringify(twistData)}`));
velocityPub.publish(twistData);
} catch (e) {
console.error(formatLog(`Failed to parse twist command from peer ${this.peerId}: ${e.message}`));
}
} else if (msg.startsWith('speak:')) {
const speakText = msg.slice(6);
console.log(formatLog(`Processing direct speak command from peer ${this.peerId}: ${speakText}`));
speak(speakText);
}
}
}
} catch (error) {
console.error(formatLog(`Error processing message from peer ${this.peerId}: ${error.message}`));
}
}
} catch (e) {
console.error(formatLog(`Message handling error from peer ${this.peerId}: ${e}`));
}
});
// Add message handler for binary messages
dc.onMessageBinary((msg) => {
console.log(formatLog(`📥 Received binary message (${msg.length} bytes)`));
try {
// Ensure we have enough bytes for the header
if (msg.length < 52) {
console.warn(formatLog(`Message too short (${msg.length} bytes), expected at least 52 bytes`));
return;
}
// Parse binary message header
const index = Number(msg.readBigUInt64BE(0));
const total = Number(msg.readBigUInt64BE(8));
// Validate array lengths
if (isNaN(index) || isNaN(total) || index < 0 || total <= 0 || index >= total) {
console.warn(formatLog(`Invalid message header: index=${index}, total=${total}`));
return;
}
const messageId = msg.slice(16, 52).toString('hex').replace(/(.{8})(.{4})(.{4})(.{4})(.{12})/, '$1-$2-$3-$4-$5');
const chunkData = msg.slice(52).toString();
// Reconstruct message if needed
if (!this.messages.has(messageId)) {
this.messages.set(messageId, {