robotics
Version:
Robotics.dev P2P ROS2 robot controller CLI with ROS telemetry and video streaming
590 lines (509 loc) • 20.5 kB
JavaScript
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;
// const { io } = require('socket.io-client');
// const LZString = require('lz-string');
// 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 LZString = require('lz-string');
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: 16384,
enableIceTcp: true,
portRangeBegin: 5000,
portRangeEnd: 6000
};
// 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;
}
async start() {
console.log(formatLog('Starting P2P server...'));
await this.connect();
// Keep connection alive
setInterval(() => {
if (!this.socket?.connected) {
console.log(formatLog('Server disconnected, reconnecting...'));
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, //this.serverId,
token: apiToken
},
auth: { id: this.serverId }
});
this.socket.on('connect', () => {
console.log(formatLog(`Connected to signaling server: ${this.socket.id}`));
speak("Robot connected.");
});
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);
}
await this.connections.get(sourcePeer).handleSignal(message);
} catch (error) {
console.error(formatLog(`Signal error: ${error.stack || error}`));
}
});
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', () => {
console.log(formatLog('Disconnected from signaling server'));
});
} catch (error) {
console.error(formatLog(`Connection error: ${error}`));
} finally {
this.isReconnecting = false;
}
}
cleanup() {
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.pc = new nodeDataChannel.PeerConnection(peerId, {
...config,
enableIceTcp: true,
enableIceUdpMux: false, // Disable UDP mux
iceTransportPolicy: 'all',
portRangeBegin: 0, // Allow any port
portRangeEnd: 65535,
maxMessageSize: 262144,
enableIceTrickle: true, // Enable trickle ICE
iceRole: 'controlled' // Add explicit ICE role
});
// Set up data channel with matching configuration
this.dataChannel = this.pc.createDataChannel('chat', {
ordered: true,
protocol: 'chat',
negotiated: true,
id: 1,
maxRetransmits: null,
maxPacketLifeTime: null
});
this.setupDataChannel(this.dataChannel);
this.hasRemoteDescription = false;
this.candidateQueue = [];
this.dataChannelOpen = false;
this.setupHandlers();
this.maxChunkSize = 15 * 1024;
this.messages = new Map();
this.subscribers = []; // Track subscriber handles for cleanup
this.currentDataChannel = null;
}
sendSignal(message) {
try {
const signalMessage = {
...message,
targetPeer: this.peerId,
sourcePeer: 'robot'
};
this.socket.emit('signal', signalMessage);
console.log(formatLog(`Sent ${message.type} to ${this.peerId}`));
} catch (error) {
console.error(formatLog(`Failed to send signal: ${error}`));
}
}
async handleSignal(message) {
try {
if (message.type === 'offer') {
console.log(formatLog(`Processing offer from ${this.peerId}`));
console.log(formatLog('Original SDP:', message.sdp));
// Reset state
this.candidateQueue = [];
this.hasRemoteDescription = false;
try {
let sdp = message.sdp;
// Extract ICE parameters and fingerprint
const iceUfragMatch = sdp.match(/a=ice-ufrag:([^\r\n]+)/)?.[1];
const icePwdMatch = sdp.match(/a=ice-pwd:([^\r\n]+)/)?.[1];
const fingerprintMatch = sdp.match(/a=fingerprint:sha-256\s+([^\r\n]+)/)?.[1];
if (!iceUfragMatch || !icePwdMatch || !fingerprintMatch) {
throw new Error('Missing required ICE or fingerprint parameters');
}
// Build SDP in exact format
const sdpLines = [
'v=0',
`o=- ${Date.now()} 1 IN IP4 0.0.0.0`,
's=-',
't=0 0',
'm=application 9 DTLS/SCTP 5000', // Match Python format
'c=IN IP4 0.0.0.0',
'a=mid:0',
`a=ice-ufrag:${iceUfragMatch}`,
`a=ice-pwd:${icePwdMatch}`,
`a=fingerprint:sha-256 ${fingerprintMatch}`,
'a=sctp-port:5000',
'a=max-message-size:262144',
'a=setup:active' // Always use active as responder
];
const formattedSdp = sdpLines.join('\r\n') + '\r\n';
console.log(formatLog('Formatted SDP:', formattedSdp));
await this.pc.setRemoteDescription(formattedSdp, message.type);
console.log(formatLog('Remote description set'));
this.hasRemoteDescription = true;
await this.pc.setLocalDescription();
console.log(formatLog('Local description set, sending answer'));
// Process queued candidates if any
while (this.candidateQueue.length > 0) {
const candidate = this.candidateQueue.shift();
if (candidate && candidate.candidate) {
await this.pc.addRemoteCandidate(candidate.candidate, candidate.mid || '0');
console.log(formatLog('Added queued ICE candidate'));
}
}
} catch (e) {
console.error(formatLog(`SDP error: ${e}`));
console.log(formatLog('Failed SDP:\n' + message.sdp));
throw e;
}
} else if (message.type === 'candidate') {
const candidate = {
candidate: String(message.candidate || ''),
mid: String(message.mid || '0')
};
if (!this.hasRemoteDescription) {
console.log(formatLog('Queueing candidate for later'));
this.candidateQueue.push(candidate);
} else {
try {
await this.pc.addRemoteCandidate(candidate.candidate, candidate.mid);
console.log(formatLog('Added ICE candidate'));
} catch (e) {
console.warn(formatLog(`Invalid candidate, queueing: ${e}`));
this.candidateQueue.push(candidate);
}
}
}
} catch (error) {
console.error(formatLog(`Signal handling error: ${error}`));
}
}
sendCompressedMessage(dc, msg) {
try {
const compressed = LZString.compressToBase64(JSON.stringify(msg));
const compressedSize = compressed.length;
const messageId = uuidv4();
const totalChunks = Math.ceil(compressedSize / this.maxChunkSize);
for (let i = 0; i < totalChunks; i++) {
const chunk = compressed.slice(i * this.maxChunkSize, (i + 1) * this.maxChunkSize);
const messageChunk = {
chunk,
index: i,
total: totalChunks,
messageId
};
dc.sendMessage(JSON.stringify(messageChunk));
}
} catch (error) {
console.error(formatLog(`Send error: ${error}`));
}
}
setupHandlers() {
this.pc.onLocalDescription((sdp, type) => {
this.sendSignal({
type,
sdp: String(sdp)
});
});
this.pc.onLocalCandidate((candidate, mid) => {
if (candidate) {
this.sendSignal({
type: 'candidate',
candidate: String(candidate),
mid: String(mid)
});
}
});
this.pc.onStateChange((state) => {
console.log(formatLog(`Connection state for ${this.peerId}: ${state}`));
});
this.pc.onDataChannel((dc) => {
console.log(formatLog(`Data channel from ${this.peerId}: ${dc.getLabel()}`));
this.setupDataChannel(dc);
if(firstRun){
this.initRcl(dc);
}
});
}
setupDataChannel(dc) {
console.log(formatLog('Setting up responder data channel'));
this.currentDataChannel = dc;
dc.onOpen(() => {
console.log(formatLog(`📡 Data channel opened with label: ${dc.getLabel()}`));
this.dataChannelOpen = true;
this.isDataChannelReady = true;
this.setupRosSubscriptions(dc); // Setup subscriptions when channel opens
if(firstRun){
this.initRcl(dc);
}
});
dc.onMessage((msg) => {
try {
console.log(formatLog(`📥 Received: ${msg}`));
var dataObj
try{
dataObj = JSON.parse(msg);
} catch(error){
dataObj = msg //message is not JSON
}
if(typeof dataObj == "object"){
if(dataObj.topic === "twist"){
velocityPub.publish(dataObj.twist);
}
if(dataObj.topic === "speak"){
speak(dataObj.text);
}
}
// // Send immediate response using compression
// const response = `Echo: ${msg}`;
// console.log(formatLog(`📤 Sending compressed response: ${response}`));
// this.sendCompressedMessage(dc, response);
// // Schedule next message
// setTimeout(() => {
// if (dc.readyState === 'open' && this.isDataChannelReady) {
// const nextMsg = `Message from server #${++this.messageCount}`;
// console.log(formatLog(`📤 Sending next compressed: ${nextMsg}`));
// this.sendCompressedMessage(dc, nextMsg);
// }
// }, 2000);
} catch (e) {
console.error(formatLog(`Message handling error: ${e}`));
}
});
dc.onError((e) => console.error(formatLog(`Data channel error: ${e}`)));
dc.onClosed(() => {
console.log(formatLog('Data channel closed'));
this.cleanupSubscriptions(); // Cleanup subscriptions when channel closes
});
}
cleanupSubscriptions() {
this.subscribers.forEach(subscriber => {
try {
if (subscriber && typeof subscriber.dispose === 'function') {
subscriber.dispose();
}
} catch (error) {
console.warn(formatLog(`Error cleaning up subscriber: ${error}`));
}
});
this.subscribers = [];
}
setupRosSubscriptions(dc) {
if (!rosNode) {
console.error('ROS node not initialized');
return;
}
this.cleanupSubscriptions(); // Cleanup any existing subscriptions
if (rosTopics.length > 0) {
console.log('Subscribing to ROS topics:', rosTopics);
rosTopics.forEach(({type, topic}) => {
const subscriber = rosNode.createSubscription(type, topic, (msg) => {
if (dc.isOpen()) { // Check if data channel is open before sending
try {
var modTopic;
if(topic === `/robot${robotId.replace(/-/g, "")}/camera2d`){
modTopic = '/camera2d';
} else if (topic === `/robot${robotId.replace(/-/g, "")}/odom`){
modTopic = '/odom';
} else {
modTopic = topic;
}
this.sendCompressedMessage(dc, {
robotId: robotId,
topic: modTopic,
data: msg
});
} catch (error) {
console.warn(formatLog(`Failed to send message on topic ${topic}: ${error}`));
}
}
});
this.subscribers.push(subscriber); // Store subscriber for cleanup
console.log(`Subscribed to ${topic} with message type ${type}`);
});
} else {
console.log('No ROS topics identified.');
}
}
initRcl(dc) {
try {
firstRun = false;
rclnodejs.init().then(() => {
rosNode = new rclnodejs.Node('robotics_dev_node');
velocityPub = rosNode.createPublisher('geometry_msgs/msg/Twist', `${cmdVel}`);
this.setupRosSubscriptions(dc); // Setup subscriptions when ROS node is initialized
rosNode.spin();
});
} catch (error) {
console.error(formatLog(`RCL error: ${error}`));
}
}
cleanup() {
// Clean up subscribers before closing connection
this.cleanupSubscriptions();
if (this.pc) this.pc.close();
if (this.socket) this.socket.close();
}
}
// Main execution
async function main() {
const server = new P2PServer();
process.on('SIGINT', () => {
console.log(formatLog('Shutting down...'));
server.cleanup();
rclnodejs.shutdown();
// socket.close();
nodeDataChannel.cleanup();
process.exit(0);
});
await server.start();
}
main().catch(error => {
console.error(formatLog(`Fatal error: ${error}`));
process.exit(1);
});