From 22c537fa9575c8399e384fb8229b424453d45aeb Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Tue, 16 Jul 2024 15:58:14 -0700 Subject: [PATCH] Wait until key topics have a publisher --- src/pages/robot/tsx/index.tsx | 4 +- src/pages/robot/tsx/robot.tsx | 87 ++++++++++++++++++++++++++-- src/pages/robot/tsx/videostreams.tsx | 5 +- 3 files changed, 90 insertions(+), 6 deletions(-) diff --git a/src/pages/robot/tsx/index.tsx b/src/pages/robot/tsx/index.tsx index 7fc7fe4a..916c84bc 100644 --- a/src/pages/robot/tsx/index.tsx +++ b/src/pages/robot/tsx/index.tsx @@ -58,7 +58,7 @@ connection = new WebRTCConnection({ onMessage: handleMessage, onConnectionEnd: disconnectFromRobot, }); -robot.setOnRosConnectCallback(() => { +robot.setOnRosConnectCallback(async () => { robot.subscribeToVideo({ topicName: "/navigation_camera/image_raw/rotated/compressed", callback: navigationStream.updateImage, @@ -83,6 +83,8 @@ robot.setOnRosConnectCallback(() => { robot.getJointLimits(); connection.joinRobotRoom(); + + return Promise.resolve(); }); robot.connect(); diff --git a/src/pages/robot/tsx/robot.tsx b/src/pages/robot/tsx/robot.tsx index be21cbcd..ae61db6c 100644 --- a/src/pages/robot/tsx/robot.tsx +++ b/src/pages/robot/tsx/robot.tsx @@ -30,7 +30,7 @@ export class Robot extends React.Component { private ros: ROSLIB.Ros; private readonly rosURL = "wss://localhost:9090"; private rosReconnectTimerID?: ReturnType; - private onRosConnectCallback?: () => void; + private onRosConnectCallback?: () => Promise; private jointLimits: { [key in ValidJoints]?: [number, number] } = {}; private jointState?: ROSJointState; private poseGoal?: ROSLIB.ActionGoal; @@ -101,7 +101,7 @@ export class Robot extends React.Component { this.stretchToolCallback = props.stretchToolCallback; } - setOnRosConnectCallback(callback: () => void) { + setOnRosConnectCallback(callback: () => Promise) { this.onRosConnectCallback = callback; } @@ -117,8 +117,19 @@ export class Robot extends React.Component { this.ros.on("connection", async () => { console.log("Connected to ROS."); - await this.onConnect(); - if (this.onRosConnectCallback) this.onRosConnectCallback(); + // We check that bidirectional communications with ROS are working, and + // that some key topics have publishers (which are indicative of all + // required nodes being loaded). This is because ROSbridge matches the + // QoS of publishers, so without publishers there is likely to be a + // QoS mismatch. + let isConnected = await this.checkROSConnection(); + if (isConnected) { + await this.onConnect(); + if (this.onRosConnectCallback) await this.onRosConnectCallback(); + } else { + console.log("Required ROS nodes are not yet loaded. Reconnecting."); + this.reconnect(); + } }); this.ros.on("error", (error) => { console.log("Error connecting to ROS:", error); @@ -142,6 +153,74 @@ export class Robot extends React.Component { } } + async checkROSConnection( + required_topics: string[] = [ + "/camera/color/image_raw/rotated/compressed", + "/gripper_camera/image_raw/cropped/compressed", + "/navigation_camera/image_raw/rotated/compressed", + "/stretch/joint_states", + ], + timeout_ms: number = 5000, + ): Promise { + let numRequiredTopicsWithPublisher = 0; + let isResolved = false; + console.log("Checking ROS connection..."); + return new Promise(async (resolve) => { + if (this.ros.isConnected) { + for (let topic of required_topics) { + // Verify that the topic has a publisher + this.ros.getPublishers( + topic, + // Success callback + (publishers: string[]) => { + if (publishers.length > 0) { + console.log("Got a publisher on topic", topic); + numRequiredTopicsWithPublisher += 1; + if (numRequiredTopicsWithPublisher === required_topics.length) { + console.log("Got publishers on all required topics."); + isResolved = true; + resolve(true); + } + } else { + console.log("No publisher on topic", topic); + isResolved = true; + resolve(false); + } + }, + // Failure callback + (error) => { + console.log( + "Error in getting publishers for topic", + topic, + error, + ); + isResolved = true; + resolve(false); + }, + ); + } + resolve( + await new Promise((resolve) => + setTimeout(() => { + if (!isResolved) { + if (numRequiredTopicsWithPublisher < required_topics.length) { + console.log( + "Timed out with at least one required topic not having publishers.", + ); + resolve(false); + } + } + }, timeout_ms), + ), + ); + } else { + console.log("ROS is not connected."); + isResolved = true; + resolve(false); + } + }); + } + async onConnect() { console.log("onConnect"); this.subscribeToJointState(); diff --git a/src/pages/robot/tsx/videostreams.tsx b/src/pages/robot/tsx/videostreams.tsx index 7e23ad52..b8a24a61 100644 --- a/src/pages/robot/tsx/videostreams.tsx +++ b/src/pages/robot/tsx/videostreams.tsx @@ -87,7 +87,7 @@ export class VideoStream extends React.Component { ); } if (!this.imageReceived) { - let { width, height, data } = jpeg.decode( + let { width, height } = jpeg.decode( Uint8Array.from(atob(message.data), (c) => c.charCodeAt(0)), true, ); @@ -112,11 +112,14 @@ export class VideoStream extends React.Component { start() { if (!this.started) { + console.log("Starting video stream", this.streamName); if (!this.canvas.current) throw "Video stream canvas null"; this.outputVideoStream = this.canvas.current.captureStream(this.fps); this.video.srcObject = this.outputVideoStream; this.drawVideo(); this.started = true; + } else { + console.log("Video stream already started", this.streamName); } }