UNPKG

@robotical/ricjs

Version:

Javascript/TS library for Robotical RIC

779 lines (647 loc) 24.6 kB
///////////////////////////////////////////////////////////////////////////////////////////////////////////////// // // RICJS // Communications Library // // Rob Dobson & Chris Greening 2020-2022 // (C) 2020-2022 // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////// import RICChannel from "./RICChannel"; import RICChannelWebBLE from "./RICChannelWebBLE"; import RICMsgHandler, { RICMsgResultCode } from "./RICMsgHandler"; import RICChannelWebSocket from "./RICChannelWebSocket"; import RICChannelWebSerial from "./RICChannelWebSerial"; import RICLEDPatternChecker from "./RICLEDPatternChecker"; import RICCommsStats from "./RICCommsStats"; import { RICEventFn, RICFileDownloadFn, RICLedLcdColours, RICOKFail, RICStateInfo, RICFileSendType } from "./RICTypes"; import RICAddOnManager from "./RICAddOnManager"; import RICSystem from "./RICSystem"; import RICFileHandler from "./RICFileHandler"; import RICStreamHandler from "./RICStreamHandler"; import { ROSSerialAddOnStatusList, ROSSerialIMU, ROSSerialMagneto, ROSSerialPowerStatus, ROSSerialRobotStatus, ROSSerialSmartServos } from "./RICROSSerial"; import RICUtils from "./RICUtils"; import RICLog from "./RICLog"; import { RICConnEvent, RICConnEventNames } from "./RICConnEvents"; import RICUpdateManager from "./RICUpdateManager"; import { RICUpdateEvent, RICUpdateEventNames } from "./RICUpdateEvents"; import RICServoFaultDetector from "./RICServoFaultDetector"; export default class RICConnector { // Channel private _ricChannel: RICChannel | null = null; // Channel connection method and locator private _channelConnMethod = ""; private _channelConnLocator: string | object = ""; // Comms stats private _commsStats: RICCommsStats = new RICCommsStats(); // Latest data from servos, IMU, etc private _ricStateInfo: RICStateInfo = new RICStateInfo(); // Add-on Manager private _addOnManager = new RICAddOnManager(); // Message handler private _ricMsgHandler: RICMsgHandler = new RICMsgHandler( this._commsStats, this._addOnManager, ); // RICSystem private _ricSystem: RICSystem = new RICSystem(this._ricMsgHandler, this._addOnManager); // LED Pattern checker private _ledPatternChecker: RICLEDPatternChecker = new RICLEDPatternChecker(); private _ledPatternTimeoutMs = 10000; private _ledPatternRefreshTimer: ReturnType<typeof setTimeout> | null = null; // Subscription rate private _subscribeRateHz = 10; // Connection performance checker private readonly _testConnPerfBlockSize = 500; private readonly _testConnPerfNumBlocks = 20; private readonly _connPerfRsltDelayMs = 4000; // Retry connection if lost private _retryIfLostEnabled = true; private _retryIfLostForSecs = 10; private _retryIfLostIsConnected = false; private _retryIfLostDisconnectTime: number | null = null; private readonly _retryIfLostRetryDelayMs = 500; // File handler private _ricFileHandler: RICFileHandler = new RICFileHandler( this._ricMsgHandler, this._commsStats, ); // Stream handler private _ricStreamHandler: RICStreamHandler = new RICStreamHandler( this._ricMsgHandler, this._commsStats, this ); // Update manager private _ricUpdateManager: RICUpdateManager | null = null; // Event listener private _onEventFn: RICEventFn | null = null; // RICServoFaultDetector for detecting servo faults public ricServoFaultDetector: RICServoFaultDetector = new RICServoFaultDetector(this._ricMsgHandler, this._ricStateInfo); constructor() { // Debug RICLog.debug('RICConnector starting up'); } setupUpdateManager(appVersion: string, appUpdateURL: string, firmwareBaseURL: string, fileDownloader: RICFileDownloadFn): void { // Setup update manager const firmwareTypeStrForMainFw = 'main'; this._ricUpdateManager = new RICUpdateManager( this._ricMsgHandler, this._ricFileHandler, this._ricSystem, this._onUpdateEvent.bind(this), firmwareTypeStrForMainFw, appVersion, fileDownloader, appUpdateURL, firmwareBaseURL, this._ricChannel ); } configureFileHandler(fileBlockSize: number, batchAckSize: number){ this._ricFileHandler.setRequestedFileBlockSize(fileBlockSize); this._ricFileHandler.setRequestedBatchAckSize(batchAckSize); } setEventListener(onEventFn: RICEventFn): void { this._onEventFn = onEventFn; } isConnected() { // Check if connected const isConnected = this._retryIfLostIsConnected || (this._ricChannel ? this._ricChannel.isConnected() : false); return isConnected; } // Set retry channel mode setRetryConnectionIfLost(enableRetry: boolean, retryForSecs: number): void { this._retryIfLostEnabled = enableRetry; this._retryIfLostForSecs = retryForSecs; if (!this._retryIfLostEnabled) { this._retryIfLostIsConnected = false; } RICLog.debug(`setRetryConnectionIfLost ${enableRetry} retry for ${retryForSecs}`); } getConnMethod(): string { return this._channelConnMethod; } getAddOnManager(): RICAddOnManager { return this._addOnManager; } getRICSystem(): RICSystem { return this._ricSystem; } getRICState(): RICStateInfo { return this._ricStateInfo; } getCommsStats(): RICCommsStats { return this._commsStats; } getRICMsgHandler(): RICMsgHandler { return this._ricMsgHandler; } getRICChannel(): RICChannel | null { return this._ricChannel; } getRICUpdateManager(): RICUpdateManager | null { return this._ricUpdateManager; } getConnLocator(): any | null { return this._ricChannel ? this._ricChannel.getConnectedLocator() : null; } pauseConnection(pause = true){ if (this._ricChannel) this._ricChannel.pauseConnection(pause); } /** * Connect to a RIC * * @param {string} method - can be "WebBLE" or "WebSocket" * @param {string | object} locator - either a string (WebSocket URL) or an object (WebBLE) * @returns Promise<boolean> * */ async connect(method: string, locator: string | object): Promise<boolean> { // Ensure disconnected try { await this.disconnect(); } catch (err) { // Ignore } // Check connection method let connMethod = ""; if (method === 'WebBLE' && typeof locator === 'object') { // Create channel this._ricChannel = new RICChannelWebBLE(); connMethod = 'WebBLE'; } else if (((method === 'WebSocket') || (method === 'wifi')) && (typeof locator === 'string')) { // Create channel this._ricChannel = new RICChannelWebSocket(); connMethod = 'WebSocket'; } else if (((method === 'WebSerial'))) { this._ricChannel = new RICChannelWebSerial(); connMethod = 'WebSerial'; } RICLog.debug(`connecting with connMethod ${connMethod}`); // Check channel established let connOk = false; if (this._ricChannel !== null) { // Connection method and locator this._channelConnMethod = connMethod; this._channelConnLocator = locator; // Set message handler this._ricChannel.setMsgHandler(this._ricMsgHandler); this._ricChannel.setOnConnEvent(this.onConnEvent.bind(this)); // Message handling in and out this._ricMsgHandler.registerForResults(this); this._ricMsgHandler.registerMsgSender(this._ricChannel); // Connect try { // Event this.onConnEvent(RICConnEvent.CONN_CONNECTING_RIC); // Connect connOk = await this._connectToChannel(); } catch (err) { RICLog.error('RICConnector.connect - error: ' + err); } // Events if (connOk) { this.onConnEvent(RICConnEvent.CONN_CONNECTED_RIC); } else { // Failed Event this.onConnEvent(RICConnEvent.CONN_CONNECTION_FAILED); } // Subscribe for updates if required if (this._ricChannel.requiresSubscription()) { try { await this.subscribeForUpdates(true); RICLog.info(`connect subscribed for updates`); } catch (error: unknown) { RICLog.warn(`connect subscribe for updates failed ${error}`) } } // configure file handler this.configureFileHandler(this._ricChannel.fhFileBlockSize(), this._ricChannel.fhBatchAckSize()); } else { this._channelConnMethod = ""; } return connOk; } async disconnect(): Promise<void> { // Disconnect this._retryIfLostIsConnected = false; if (this._ricChannel) { // await this.sendRICRESTMsg("bledisc", {}); this._ricChannel.disconnect(); } } // Mark: Tx Message handling ----------------------------------------------------------------------------------------- /** * * sendRICRESTMsg * @param commandName command API string * @param params parameters (simple name value pairs only) to parameterize trajectory * @returns Promise<RICOKFail> * */ async sendRICRESTMsg(commandName: string, params: object): Promise<RICOKFail> { try { // Format the paramList as query string const paramEntries = Object.entries(params); let paramQueryStr = ''; for (const param of paramEntries) { if (paramQueryStr.length > 0) paramQueryStr += '&'; paramQueryStr += param[0] + '=' + param[1]; } // Format the url to send if (paramQueryStr.length > 0) commandName += '?' + paramQueryStr; return await this._ricMsgHandler.sendRICRESTURL<RICOKFail>(commandName); } catch (error) { RICLog.warn(`runCommand failed ${error}`); return new RICOKFail(); } } // Mark: Rx Message handling ----------------------------------------------------------------------------------------- onRxReply( msgHandle: number, msgRsltCode: RICMsgResultCode, msgRsltJsonObj: object | null, ): void { RICLog.verbose( `onRxReply msgHandle ${msgHandle} rsltCode ${msgRsltCode} obj ${JSON.stringify( msgRsltJsonObj, )}`, ); } onRxUnnumberedMsg(msgRsltJsonObj: { [key: string]: number | string }): void { RICLog.verbose( `onRxUnnumberedMsg rsltCode obj ${JSON.stringify(msgRsltJsonObj)}`, ); // Inform the file handler if ('okto' in msgRsltJsonObj) { this._ricFileHandler.onOktoMsg(msgRsltJsonObj.okto as number); } else if ('sokto' in msgRsltJsonObj) { this._ricStreamHandler.onSoktoMsg(msgRsltJsonObj.sokto as number); } } // Mark: Published data handling ----------------------------------------------------------------------------------------- onRxOtherROSSerialMsg(topicID: number, payload: Uint8Array): void { RICLog.debug(`onRxOtherROSSerialMsg topicID ${topicID} payload ${RICUtils.bufferToHex(payload)}`); } onRxSmartServo(smartServos: ROSSerialSmartServos): void { // RICLog.verbose(`onRxSmartServo ${JSON.stringify(smartServos)}`); this._ricStateInfo.smartServos = smartServos; this._ricStateInfo.smartServosValidMs = Date.now(); } onRxIMU(imuData: ROSSerialIMU): void { // RICLog.verbose(`onRxIMU ${JSON.stringify(imuData)}`); this._ricStateInfo.imuData = imuData; this._ricStateInfo.imuDataValidMs = Date.now(); } onRxMagneto(magnetoData: ROSSerialMagneto): void { // RICLog.verbose(`onRxMagneto ${JSON.stringify(magnetoData)}`); this._ricStateInfo.magnetoData = magnetoData; this._ricStateInfo.magnetoDataValidMs = Date.now(); } onRxPowerStatus(powerStatus: ROSSerialPowerStatus): void { // RICLog.verbose(`onRxPowerStatus ${JSON.stringify(powerStatus)}`); this._ricStateInfo.power = powerStatus; this._ricStateInfo.powerValidMs = Date.now(); } onRxAddOnPub(addOnInfo: ROSSerialAddOnStatusList): void { // RICLog.verbose(`onRxAddOnPub ${JSON.stringify(addOnInfo)}`); this._ricStateInfo.addOnInfo = addOnInfo; this._ricStateInfo.addOnInfoValidMs = Date.now(); } onRobotStatus(robotStatus: ROSSerialRobotStatus): void { // RICLog.verbose(`onRobotStatus ${JSON.stringify(robotStatus)}`); this._ricStateInfo.robotStatus = robotStatus; this._ricStateInfo.robotStatusValidMs = Date.now(); } getRICStateInfo(): RICStateInfo { return this._ricStateInfo; } // Mark: Check correct RIC ----------------------------------------------------------------------------------------- /** * Start checking correct RIC using LED pattern * * @param {string} ricToConnectTo - RIC to connect to * @return boolean - true if started ok * */ async checkCorrectRICStart(ricLedLcdColours: RICLedLcdColours): Promise<boolean> { // Set colour pattern checker colours const randomColours = this._ledPatternChecker.setup(ricLedLcdColours); // Start timer to repeat checking LED pattern RICLog.debug(`checkCorrectRICStart: starting LED pattern checker`); if (!await this._checkCorrectRICRefreshLEDs()) { return false; } // Event this.onConnEvent(RICConnEvent.CONN_VERIFYING_CORRECT_RIC, randomColours); // Start timer to repeat sending of LED pattern // This is because RIC's LED pattern override times out after a while // so has to be refreshed periodically this._ledPatternRefreshTimer = setInterval(async () => { RICLog.verbose(`checkCorrectRICStart: loop`); if (!this._checkCorrectRICRefreshLEDs()) { RICLog.debug('checkCorrectRICStart no longer active - clearing timer'); this._clearLedPatternRefreshTimer(); } }, this._ledPatternTimeoutMs / 2.1); return true; } /** * Stop checking correct RIC * * @return void * */ async checkCorrectRICStop(confirmCorrectRIC: boolean): Promise<boolean> { // Stop refreshing LED pattern on RIC this._clearLedPatternRefreshTimer(); // Stop the LED pattern checker if connected if (this.isConnected()) { await this._ledPatternChecker.clearRICColors(this._ricMsgHandler); } // Check correct if (!confirmCorrectRIC) { // Event this.onConnEvent(RICConnEvent.CONN_REJECTED_RIC); // Indicate as rejected if we're not connected or if user didn't confirm return false; } // Event this.onConnEvent(RICConnEvent.CONN_VERIFIED_CORRECT_RIC); return true; } /** * Refresh LED pattern on RIC * * @return boolean - true if checking still active * */ async _checkCorrectRICRefreshLEDs(): Promise<boolean> { // Check LED pattern is active if (!this._ledPatternChecker.isActive()) { return false; } // Check connected RICLog.debug(`_verificationRepeat getting isConnected`); if (!this.isConnected()) { console.warn('_verificationRepeat not connected'); return false; } // Repeat the LED pattern (RIC times out the LED override after ~10 seconds) RICLog.debug(`_verificationRepeat setting pattern`); return await this._ledPatternChecker.setRICColors(this._ricMsgHandler, this._ledPatternTimeoutMs); } _clearLedPatternRefreshTimer(): void { if (this._ledPatternRefreshTimer) { clearInterval(this._ledPatternRefreshTimer); this._ledPatternRefreshTimer = null; } } // Mark: Marty system info ------------------------------------------------------------------------------------ /** * Get information Marty system * * @return void * */ async retrieveMartySystemInfo(): Promise<boolean> { // Retrieve system info try { const retrieveResult = await this._ricSystem.retrieveInfo(); return retrieveResult; } catch (err) { RICLog.error(`retrieveMartySystemInfo: error ${err}`); } return false; } // Mark: RIC Subscription to Updates -------------------------------------------------------------------------------- /** * * subscribeForUpdates * @param enable - true to send command to enable subscription (false to remove sub) * @returns Promise<void> * */ async subscribeForUpdates(enable: boolean): Promise<void> { try { const subscribeDisable = '{"cmdName":"subscription","action":"update",' + '"pubRecs":[' + `{"name":"MultiStatus","rateHz":0,}` + '{"name":"PowerStatus","rateHz":0},' + `{"name":"AddOnStatus","rateHz":0}` + ']}'; const subscribeEnable = '{"cmdName":"subscription","action":"update",' + '"pubRecs":[' + `{"name":"MultiStatus","rateHz":${this._subscribeRateHz.toString()}}` + `{"name":"PowerStatus","rateHz":1.0},` + `{"name":"AddOnStatus","rateHz":${this._subscribeRateHz.toString()}}` + ']}'; const ricResp = await this._ricMsgHandler.sendRICRESTCmdFrame<RICOKFail>(enable ? subscribeEnable : subscribeDisable); // Debug RICLog.debug(`subscribe enable/disable returned ${JSON.stringify(ricResp)}`); } catch (error: unknown) { RICLog.warn(`getRICCalibInfo Failed subscribe for updates ${error}`); } } async sendFile(fileName: string, fileContents: Uint8Array, progressCallback: ((sent: number, total: number, progress: number) => void) | undefined, ): Promise<boolean> { return this._ricFileHandler.fileSend(fileName, RICFileSendType.RIC_NORMAL_FILE, fileContents, progressCallback); } // Mark: Streaming -------------------------------------------------------------------------------- streamAudio(streamContents: Uint8Array, clearExisting: boolean, duration: number): void { if (this._ricStreamHandler && this.isConnected()) { this._ricStreamHandler.streamAudio(streamContents, clearExisting, duration); } } isStreamStarting() { return this._ricStreamHandler.isStreamStarting(); } setLegacySoktoMode(legacyMode: boolean){ return this._ricStreamHandler.setLegacySoktoMode(legacyMode); } // Mark: Connection performance-------------------------------------------------------------------------- parkmiller_next(seed: number) { const hi = Math.round(16807 * (seed & 0xffff)); let lo = Math.round(16807 * (seed >> 16)); lo += (hi & 0x7fff) << 16; lo += hi >> 15; if (lo > 0x7fffffff) lo -= 0x7fffffff; return lo; } async checkConnPerformance(): Promise<number | undefined> { // Send random blocks of data - these will be ignored by RIC - but will still be counted for performance // evaluation let prbsState = 1; const testData = new Uint8Array(this._testConnPerfBlockSize); for (let i = 0; i < this._testConnPerfNumBlocks; i++) { testData.set([0, (i >> 24) & 0xff, (i >> 16) & 0xff, (i >> 8) & 0xff, i & 0xff, 0x1f, 0x9d, 0xf4, 0x7a, 0xb5]); for (let j = 10; j < this._testConnPerfBlockSize; j++) { prbsState = this.parkmiller_next(prbsState); testData[j] = prbsState & 0xff; } if (this._ricChannel) { await this._ricChannel.sendTxMsg(testData, false); } } // Wait a little to allow RIC to process the data await new Promise(resolve => setTimeout(resolve, this._connPerfRsltDelayMs)); // Get performance const blePerf = await this._ricSystem.getSysModInfoBLEMan(); if (blePerf) { console.log(`startConnPerformanceCheck timer rate = ${blePerf.tBPS}BytesPS`); return blePerf.tBPS; } else { throw new Error('checkConnPerformance: failed to get BLE performance'); } } // Mark: Connection event -------------------------------------------------------------------------- onConnEvent(eventEnum: RICConnEvent, data: object | string | null | undefined = undefined): void { // Handle information clearing on disconnect switch (eventEnum) { case RICConnEvent.CONN_DISCONNECTED_RIC: // Disconnect time this._retryIfLostDisconnectTime = Date.now(); // Check if retry required if (this._retryIfLostIsConnected && this._retryIfLostEnabled) { // Indicate connection disrupted if (this._onEventFn) { this._onEventFn("conn", RICConnEvent.CONN_ISSUE_DETECTED, RICConnEventNames[RICConnEvent.CONN_ISSUE_DETECTED]); } // Retry connection this._retryConnection(); // Don't allow disconnection to propagate until retries have occurred return; } // Invalidate connection details this._ricSystem.invalidate(); break; } // Notify if (this._onEventFn) { this._onEventFn("conn", eventEnum, RICConnEventNames[eventEnum], data); } } _retryConnection(): void { // Check timeout if ((this._retryIfLostDisconnectTime !== null) && (Date.now() - this._retryIfLostDisconnectTime < this._retryIfLostForSecs * 1000)) { // Set timer to try to reconnect setTimeout(async () => { // Try to connect const isConn = await this._connectToChannel(); if (!isConn) { this._retryConnection(); } else { // No longer retrying this._retryIfLostDisconnectTime = null; // Indicate connection problem resolved if (this._onEventFn) { this._onEventFn("conn", RICConnEvent.CONN_ISSUE_RESOLVED, RICConnEventNames[RICConnEvent.CONN_ISSUE_RESOLVED]); } } }, this._retryIfLostRetryDelayMs); } else { // No longer connected after retry timeout this._retryIfLostIsConnected = false; // Indicate disconnection if (this._onEventFn) { this._onEventFn("conn", RICConnEvent.CONN_DISCONNECTED_RIC, RICConnEventNames[RICConnEvent.CONN_DISCONNECTED_RIC]); } // Invalidate connection details this._ricSystem.invalidate(); } } async _connectToChannel(): Promise<boolean> { // Connect try { if (this._ricChannel) { const connected = await this._ricChannel.connect(this._channelConnLocator); if (connected) { this._retryIfLostIsConnected = true; return true; } } } catch (error) { RICLog.error(`RICConnector.connect() error: ${error}`); } return false; } // Mark: OTA Update ----------------------------------------------------------------------------------------- _onUpdateEvent(eventEnum: RICUpdateEvent, data: object | string | null | undefined = undefined): void { // Notify if (this._onEventFn) { this._onEventFn("ota", eventEnum, RICUpdateEventNames[eventEnum], data); } } async otaUpdateCheck(): Promise<RICUpdateEvent> { if (!this._ricUpdateManager) return RICUpdateEvent.UPDATE_NOT_CONFIGURED; return await this._ricUpdateManager.checkForUpdate(this._ricSystem.getCachedSystemInfo()); } async otaUpdateStart(): Promise<RICUpdateEvent> { if (!this._ricUpdateManager) return RICUpdateEvent.UPDATE_NOT_CONFIGURED; return await this._ricUpdateManager.firmwareUpdate(); } async otaUpdateCancel(): Promise<void> { if (!this._ricUpdateManager) return; return await this._ricUpdateManager.firmwareUpdateCancel(); } // Mark: Set AddOn config ----------------------------------------------------------- /** * * setAddOnConfig - set a specified add-on's configuration * @param serialNo used to identify the add-on * @param newName name to refer to add-on by * @returns Promise<RICOKFail> * */ async setAddOnConfig(serialNo: string, newName: string): Promise<RICOKFail> { try { const msgRslt = await this._ricMsgHandler.sendRICRESTURL<RICOKFail>( `addon/set?SN=${serialNo}&name=${newName}`, ); return msgRslt; } catch (error) { return new RICOKFail(); } } /** * deleteAddOn - remove an addon from the addonlist on RIC * @param serialNo used to identify the add-on * @returns Promise<RICOKFail> */ async deleteAddOn(serialNo: string): Promise<RICOKFail> { try { const msgRslt = await this._ricMsgHandler.sendRICRESTURL<RICOKFail>( `addon/del?SN=${serialNo}`, ); return msgRslt; } catch (error) { return new RICOKFail(); } } // Mark: Identify AddOn ----------------------------------------------------------- /** * * identifyAddOn - send the 'identify' command to a specified add-on * @param name used to identify the add-on * @returns Promise<RICOKFail> * */ async identifyAddOn(name: string): Promise<RICOKFail> { try { const msgRslt = await this._ricMsgHandler.sendRICRESTURL<RICOKFail>( `elem/${name}/json?cmd=raw&hexWr=F8`, ); return msgRslt; } catch (error) { return new RICOKFail(); } } }