@robotical/ricjs
Version:
Javascript/TS library for Robotical RIC
702 lines (605 loc) • 21.4 kB
text/typescript
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// RICJS
// Communications Library
//
// Rob Dobson & Chris Greening 2020-2022
// (C) 2020-2022
//
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
import RICCommsStats from './RICCommsStats';
import { RICMsgTrackInfo } from './RICMsgTrackInfo';
import RICLog from './RICLog';
import RICUtils from './RICUtils';
import {
RICROSSerial,
ROSSerialIMU,
ROSSerialSmartServos,
ROSSerialPowerStatus,
ROSSerialAddOnStatusList,
ROSSerialRobotStatus,
ROSSerialMagneto,
} from './RICROSSerial';
import {
PROTOCOL_RICREST,
RICSERIAL_MSG_NUM_POS,
RICSERIAL_PAYLOAD_POS,
RICSERIAL_PROTOCOL_POS,
RICREST_REST_ELEM_CODE_POS,
RICREST_HEADER_PAYLOAD_POS,
} from './RICProtocolDefs';
import RICMiniHDLC from './RICMiniHDLC';
import RICAddOnManager from './RICAddOnManager';
import { RICReportMsg } from './RICTypes';
// Protocol enums
export enum RICRESTElemCode {
RICREST_ELEM_CODE_URL,
RICREST_ELEM_CODE_CMDRESPJSON,
RICREST_ELEM_CODE_BODY,
RICREST_ELEM_CODE_COMMAND_FRAME,
RICREST_ELEM_CODE_FILEBLOCK,
}
export enum RICCommsMsgTypeCode {
MSG_TYPE_COMMAND,
MSG_TYPE_RESPONSE,
MSG_TYPE_PUBLISH,
MSG_TYPE_REPORT,
}
export enum RICCommsMsgProtocol {
MSG_PROTOCOL_ROSSERIAL,
MSG_PROTOCOL_RESERVED_1,
MSG_PROTOCOL_RICREST,
}
// Message results
export enum RICMsgResultCode {
MESSAGE_RESULT_TIMEOUT,
MESSAGE_RESULT_OK,
MESSAGE_RESULT_FAIL,
MESSAGE_RESULT_UNKNOWN,
}
export interface RICMessageResult {
onRxReply(
msgHandle: number,
msgRsltCode: RICMsgResultCode,
msgRsltJsonObj: object | null,
): void;
onRxUnnumberedMsg(msgRsltJsonObj: object): void;
onRxSmartServo(smartServos: ROSSerialSmartServos): void;
onRxIMU(imuData: ROSSerialIMU): void;
onRxMagneto(magnetoData: ROSSerialMagneto): void;
onRxPowerStatus(powerStatus: ROSSerialPowerStatus): void;
onRxAddOnPub(addOnInfo: ROSSerialAddOnStatusList): void;
onRobotStatus(robotStatus: ROSSerialRobotStatus): void;
onRxOtherROSSerialMsg(topicID: number, payload: Uint8Array): void;
}
export interface RICMessageSender {
sendTxMsg(
msg: Uint8Array,
sendWithResponse: boolean,
): Promise<boolean>;
sendTxMsgNoAwait(
msg: Uint8Array,
sendWithResponse: boolean,
): Promise<boolean>;
}
export default class RICMsgHandler {
// Message numbering and tracking
private _currentMsgNumber = 1;
private _currentMsgHandle = 1;
private _msgTrackInfos: Array<RICMsgTrackInfo> = new Array<RICMsgTrackInfo>(
RICMsgTrackInfo.MAX_MSG_NUM + 1,
);
private _msgTrackTimerMs = 50;
private _msgTrackLastCheckIdx = 0;
// report message callback dictionary. Add a callback to subscribe to report messages
private _reportMsgCallbacks = new Map<string, (report: RICReportMsg) => void>();
// Interface to inform of message results
private _msgResultHandler: RICMessageResult | null = null;
// Interface to send messages
private _msgSender: RICMessageSender | null = null;
// Comms stats
private _commsStats: RICCommsStats;
// RICMiniHDLC - handles part of RICSerial protocol
private _miniHDLC: RICMiniHDLC;
// Add-on manager
private _addOnManager: RICAddOnManager;
// Constructor
constructor(commsStats: RICCommsStats, addOnManager: RICAddOnManager) {
this._commsStats = commsStats;
this._addOnManager = addOnManager;
RICLog.debug('RICMsgHandler constructor');
// Message tracking
for (let i = 0; i < this._msgTrackInfos.length; i++) {
this._msgTrackInfos[i] = new RICMsgTrackInfo();
}
// Timer for checking messages
setTimeout(async () => {
this._onMsgTrackTimer(true);
}, this._msgTrackTimerMs);
// HDLC used to encode/decode the RICREST protocol
this._miniHDLC = new RICMiniHDLC();
this._miniHDLC.setOnRxFrame(this._onHDLCFrameDecode.bind(this));
}
registerForResults(msgResultHandler: RICMessageResult) {
this._msgResultHandler = msgResultHandler;
}
registerMsgSender(RICMessageSender: RICMessageSender) {
this._msgSender = RICMessageSender;
}
handleNewRxMsg(rxMsg: Uint8Array): void {
this._miniHDLC.addRxBytes(rxMsg);
// RICLog.verbose(`handleNewRxMsg len ${rxMsg.length} ${RICUtils.bufferToHex(rxMsg)}`)
}
reportMsgCallbacksSet(callbackName: string, callback: (report: RICReportMsg) => void): void {
this._reportMsgCallbacks.set(callbackName, callback);
}
reportMsgCallbacksDelete(callbackName: string) {
this._reportMsgCallbacks.delete(callbackName);
}
_onHDLCFrameDecode(rxMsg: Uint8Array): void {
// Add to stats
this._commsStats.msgRx();
// Validity
if (rxMsg.length < RICSERIAL_PAYLOAD_POS) {
this._commsStats.msgTooShort();
return;
}
// RICLog.verbose(`_onHDLCFrameDecode len ${rxMsg.length}`);
// Decode the RICFrame header
const rxMsgNum = rxMsg[RICSERIAL_MSG_NUM_POS] & 0xff;
const rxProtocol = rxMsg[RICSERIAL_PROTOCOL_POS] & 0x3f;
const rxMsgType = (rxMsg[RICSERIAL_PROTOCOL_POS] >> 6) & 0x03;
// Decode payload
if (rxProtocol == PROTOCOL_RICREST) {
RICLog.verbose(
`_onHDLCFrameDecode RICREST rx msgNum ${rxMsgNum} msgDirn ${rxMsgType} ${RICUtils.bufferToHex(
rxMsg,
)}`,
);
// Extract payload
const ricRestElemCode =
rxMsg[RICSERIAL_PAYLOAD_POS + RICREST_REST_ELEM_CODE_POS] & 0xff;
if (
ricRestElemCode == RICRESTElemCode.RICREST_ELEM_CODE_URL ||
ricRestElemCode == RICRESTElemCode.RICREST_ELEM_CODE_CMDRESPJSON ||
ricRestElemCode == RICRESTElemCode.RICREST_ELEM_CODE_COMMAND_FRAME
) {
// These are all text-based messages
const restStr = RICUtils.getStringFromBuffer(
rxMsg,
RICSERIAL_PAYLOAD_POS + RICREST_HEADER_PAYLOAD_POS,
rxMsg.length - RICSERIAL_PAYLOAD_POS - RICREST_HEADER_PAYLOAD_POS - 1,
);
RICLog.verbose(
`_onHDLCFrameDecode RICREST rx elemCode ${ricRestElemCode} ${restStr}`,
);
// Check message types
if (rxMsgType == RICCommsMsgTypeCode.MSG_TYPE_RESPONSE) {
// Handle response messages
this._handleResponseMessages(restStr, rxMsgNum);
} else if (rxMsgType == RICCommsMsgTypeCode.MSG_TYPE_REPORT) {
// Handle report messages
this._handleReportMessages(restStr);
}
} else {
const binMsgLen = rxMsg.length - RICSERIAL_PAYLOAD_POS - RICREST_HEADER_PAYLOAD_POS;
RICLog.debug(
`_onHDLCFrameDecode RICREST rx binary message elemCode ${ricRestElemCode} len ${binMsgLen}`,
);
}
} else if (rxProtocol == RICCommsMsgProtocol.MSG_PROTOCOL_ROSSERIAL) {
// Extract ROSSerial messages - decoded messages returned via _msgResultHandler
RICROSSerial.decode(
rxMsg,
RICSERIAL_PAYLOAD_POS,
this._msgResultHandler,
this._commsStats,
this._addOnManager,
);
} else {
RICLog.warn(`_onHDLCFrameDecode unsupported protocol ${rxProtocol}`);
}
}
_handleResponseMessages(restStr: string, rxMsgNum: number): void {
try {
let msgRsltCode = RICMsgResultCode.MESSAGE_RESULT_UNKNOWN;
const msgRsltJsonObj = JSON.parse(restStr);
if ('rslt' in msgRsltJsonObj) {
const rsltStr = msgRsltJsonObj.rslt.toLowerCase();
if (rsltStr === 'ok') {
RICLog.verbose(
`_handleResponseMessages RICREST rslt Ok ${rxMsgNum == 0 ? "unnumbered" : "msgNum " + rxMsgNum.toString()} resp ${msgRsltJsonObj.rslt}`,
);
msgRsltCode = RICMsgResultCode.MESSAGE_RESULT_OK;
} else if (rsltStr === 'fail') {
msgRsltCode = RICMsgResultCode.MESSAGE_RESULT_FAIL;
RICLog.warn(
`_handleResponseMessages RICREST rslt fail ${rxMsgNum == 0 ? "unnumbered" : "msgNum " + rxMsgNum.toString()} resp ${restStr}`,
);
} else {
RICLog.warn(
`_handleResponseMessages RICREST rslt not recognized ${rxMsgNum == 0 ? "unnumbered" : "msgNum " + rxMsgNum.toString()}resp ${restStr}`,
);
}
} else {
RICLog.warn(
`_handleResponseMessages RICREST response doesn't contain rslt ${rxMsgNum == 0 ? "unnumbered" : "msgNum " + rxMsgNum.toString()}resp ${restStr}`,
);
}
// Handle matching of request and response
this.msgTrackingRxRespMsg(rxMsgNum, msgRsltCode, msgRsltJsonObj);
} catch (excp: unknown) {
if (excp instanceof Error) {
RICLog.warn(
`_handleResponseMessages Failed to parse JSON ${rxMsgNum == 0 ? "unnumbered" : "msgNum " + rxMsgNum.toString()} JSON STR ${restStr} resp ${excp.toString()}`,
);
}
}
}
_handleReportMessages(restStr: string): void {
try {
const reportMsg: RICReportMsg = JSON.parse(restStr);
reportMsg.timeReceived = Date.now();
RICLog.debug(`_handleReportMessages ${JSON.stringify(reportMsg)}`);
this._reportMsgCallbacks.forEach((callback) => callback(reportMsg));
} catch (excp: unknown) {
if (excp instanceof Error) {
RICLog.warn(
`_handleReportMessages Failed to parse JSON report ${excp.toString()}`,
);
}
}
}
async sendRICRESTURL<T>(
cmdStr: string,
msgTimeoutMs: number | undefined = undefined,
): Promise<T> {
// Send
return this.sendRICREST(
cmdStr,
RICRESTElemCode.RICREST_ELEM_CODE_URL,
msgTimeoutMs,
);
}
async sendRICRESTCmdFrame<T>(
cmdStr: string,
msgTimeoutMs: number | undefined = undefined,
): Promise<T> {
// Send
return this.sendRICREST(
cmdStr,
RICRESTElemCode.RICREST_ELEM_CODE_COMMAND_FRAME,
msgTimeoutMs,
);
}
async sendRICREST<T>(
cmdStr: string,
ricRESTElemCode: RICRESTElemCode,
msgTimeoutMs: number | undefined = undefined,
): Promise<T> {
// Put cmdStr into buffer
const cmdStrTerm = new Uint8Array(cmdStr.length + 1);
RICUtils.addStringToBuffer(cmdStrTerm, cmdStr, 0);
cmdStrTerm[cmdStrTerm.length - 1] = 0;
// Send
return this.sendRICRESTBytes(
cmdStrTerm,
ricRESTElemCode,
true,
msgTimeoutMs,
);
}
async sendRICRESTBytes<T>(
cmdBytes: Uint8Array,
ricRESTElemCode: RICRESTElemCode,
withResponse: boolean,
msgTimeoutMs: number | undefined = undefined,
): Promise<T> {
// Form message
const cmdMsg = new Uint8Array(cmdBytes.length + RICREST_HEADER_PAYLOAD_POS);
cmdMsg[RICREST_REST_ELEM_CODE_POS] = ricRESTElemCode;
cmdMsg.set(cmdBytes, RICREST_HEADER_PAYLOAD_POS);
// Send
return this.sendMsgAndWaitForReply<T>(
cmdMsg,
RICCommsMsgTypeCode.MSG_TYPE_COMMAND,
RICCommsMsgProtocol.MSG_PROTOCOL_RICREST,
withResponse,
msgTimeoutMs,
);
}
async sendMsgAndWaitForReply<T>(
msgPayload: Uint8Array,
msgDirection: RICCommsMsgTypeCode,
msgProtocol: RICCommsMsgProtocol,
withResponse: boolean,
msgTimeoutMs: number | undefined,
): Promise<T> {
// Check there is a sender
if (!this._msgSender) {
throw new Error('sendMsgAndWaitForReply failed no sender');
}
// Frame the message
const framedMsg = this.frameCommsMsg(msgPayload, msgDirection, msgProtocol, true);
if (!framedMsg) {
throw new Error('sendMsgAndWaitForReply failed to frame message');
}
// Debug
// RICLog.verbose(
// `sendMsgAndWaitForReply ${RICUtils.bufferToHex(framedMsg)}`,
// );
// Return a promise that will be resolved when a reply is received or timeout occurs
const promise = new Promise<T>((resolve, reject) => {
// Update message tracking
this.msgTrackingTxCmdMsg<T>(
framedMsg,
withResponse,
msgTimeoutMs,
resolve,
reject,
);
this._currentMsgHandle++;
});
return promise;
}
frameCommsMsg(
msgPayload: Uint8Array,
msgDirection: RICCommsMsgTypeCode,
msgProtocol: RICCommsMsgProtocol,
isNumbered: boolean,
): Uint8Array {
// Header
const msgBuf = new Uint8Array(
msgPayload.length + RICSERIAL_PAYLOAD_POS,
);
msgBuf[0] = isNumbered ? this._currentMsgNumber & 0xff : 0;
msgBuf[1] = (msgDirection << 6) + msgProtocol;
// Payload
msgBuf.set(msgPayload, RICSERIAL_PAYLOAD_POS);
// Wrap into HDLC
return this._miniHDLC.encode(msgBuf);
}
msgTrackingTxCmdMsg<T>(
msgFrame: Uint8Array,
withResponse: boolean,
msgTimeoutMs: number | undefined,
resolve: (arg: T) => void,
reject: (reason: Error) => void,
): void {
// Record message re-use of number
if (this._msgTrackInfos[this._currentMsgNumber].msgOutstanding) {
this._commsStats.recordMsgNumCollision();
}
// Set tracking info
this._msgTrackInfos[this._currentMsgNumber].set(
true,
msgFrame,
withResponse,
this._currentMsgHandle,
msgTimeoutMs,
resolve,
reject,
);
// Debug
RICLog.verbose(
`msgTrackingTxCmdMsg msgNum ${this._currentMsgNumber
} msg ${RICUtils.bufferToHex(msgFrame)} sanityCheck ${this._msgTrackInfos[this._currentMsgNumber].msgOutstanding
}`,
);
// RICLog.debug(
// `msgTrackingTxCmdMsg msgNum ${this._currentMsgNumber} msgLen ${msgFrame.length}}`,
// );
// Stats
this._commsStats.msgTx();
// Bump msg number
if (this._currentMsgNumber == RICMsgTrackInfo.MAX_MSG_NUM) {
this._currentMsgNumber = 1;
} else {
this._currentMsgNumber++;
}
}
msgTrackingRxRespMsg(
msgNum: number,
msgRsltCode: RICMsgResultCode,
msgRsltJsonObj: object,
) {
// Check message number
if (msgNum == 0) {
// Callback on unnumbered message
if (this._msgResultHandler !== null)
this._msgResultHandler.onRxUnnumberedMsg(msgRsltJsonObj);
return;
}
if (msgNum > RICMsgTrackInfo.MAX_MSG_NUM) {
RICLog.warn('msgTrackingRxRespMsg msgNum > 255');
return;
}
if (!this._msgTrackInfos[msgNum].msgOutstanding) {
RICLog.warn(`msgTrackingRxRespMsg unmatched msgNum ${msgNum}`);
this._commsStats.recordMsgNumUnmatched();
return;
}
// Handle message
RICLog.verbose(
`msgTrackingRxRespMsg Message response received msgNum ${msgNum}`,
);
this._commsStats.recordMsgResp(
Date.now() - this._msgTrackInfos[msgNum].msgSentMs,
);
this._msgCompleted(msgNum, msgRsltCode, msgRsltJsonObj);
}
_msgCompleted(
msgNum: number,
msgRsltCode: RICMsgResultCode,
msgRsltObj: object | null,
) {
// Lookup message in tracking
const msgHandle = this._msgTrackInfos[msgNum].msgHandle;
this._msgTrackInfos[msgNum].msgOutstanding = false;
// Check if message result handler should be informed
if (this._msgResultHandler !== null) {
this._msgResultHandler.onRxReply(msgHandle, msgRsltCode, msgRsltObj);
}
// Handle reply
// if (msgRsltCode === RICMsgResultCode.MESSAGE_RESULT_OK) {
const resolve = this._msgTrackInfos[msgNum].resolve;
if (resolve) {
// eslint-disable-next-line @typescript-eslint/no-explicit-any
RICLog.debug(`_msgCompleted resolve ${msgRsltCode} ${JSON.stringify(msgRsltObj)}`);
(resolve as ((arg: object | null) => void))(msgRsltObj);
}
// } else {
// const reject = this._msgTrackInfos[msgNum].reject;
// if (reject) {
// // eslint-disable-next-line @typescript-eslint/no-explicit-any
// try {
// RICLog.debug(`_msgCompleted reject rsltCode ${msgRsltCode}`);
// // (reject as any)(new Error(`Message failed msgNum ${msgNum} rslt ${msgRsltCode}`));
// } catch (excp: unknown) {
// RICLog.warn(`_msgCompleted reject ${excp}`);
// }
// }
// }
// No longer waiting for reply
this._msgTrackInfos[msgNum].resolve = null;
this._msgTrackInfos[msgNum].reject = null;
}
// Check message timeouts
async _onMsgTrackTimer(chainRecall: boolean): Promise<void> {
if (this._msgSender !== null) {
// Handle message tracking
for (let loopIdx = 0; loopIdx < this._msgTrackInfos.length; loopIdx++) {
// Index to check
const checkIdx = this._msgTrackLastCheckIdx;
this._msgTrackLastCheckIdx = (checkIdx + 1) % this._msgTrackInfos.length;
// Check if message is outstanding
if (!this._msgTrackInfos[checkIdx].msgOutstanding) continue;
// Get message timeout and ensure valid
let msgTimeoutMs = this._msgTrackInfos[checkIdx].msgTimeoutMs;
if (msgTimeoutMs === undefined) {
msgTimeoutMs = RICMsgTrackInfo.MSG_RESPONSE_TIMEOUT_MS;
}
// Check for timeout (or never sent)
if ((this._msgTrackInfos[checkIdx].retryCount === 0) || (Date.now() > this._msgTrackInfos[checkIdx].msgSentMs + msgTimeoutMs)) {
// Debug
RICLog.debug(`msgTrackTimer msgNum ${checkIdx} ${this._msgTrackInfos[checkIdx].retryCount === 0 ? 'first send' : 'timeout - retrying'}`);
// RICLog.verbose(`msgTrackTimer msg ${RICUtils.bufferToHex(this._msgTrackInfos[i].msgFrame)}`);
// Handle timeout (or first send)
if (this._msgTrackInfos[checkIdx].retryCount < RICMsgTrackInfo.MSG_RETRY_COUNT) {
this._msgTrackInfos[checkIdx].retryCount++;
try {
// Send the message
if (!await this._msgSender.sendTxMsg(
this._msgTrackInfos[checkIdx].msgFrame,
this._msgTrackInfos[checkIdx].withResponse)) {
RICLog.warn(`msgTrackTimer Message send failed msgNum ${checkIdx}`);
this._msgCompleted(checkIdx, RICMsgResultCode.MESSAGE_RESULT_FAIL, null);
this._commsStats.recordMsgNoConnection();
}
// Message sent ok so break here
break;
} catch (error: unknown) {
RICLog.warn(`Retry message failed ${error}`);
}
this._commsStats.recordMsgRetry();
this._msgTrackInfos[checkIdx].msgSentMs = Date.now();
} else {
RICLog.warn(
`msgTrackTimer TIMEOUT msgNum ${checkIdx} after ${RICMsgTrackInfo.MSG_RETRY_COUNT} retries`,
);
this._msgCompleted(checkIdx, RICMsgResultCode.MESSAGE_RESULT_TIMEOUT, null);
this._commsStats.recordMsgTimeout();
}
}
}
}
// Call again if required
if (chainRecall) {
setTimeout(async () => {
this._onMsgTrackTimer(true);
}, this._msgTrackTimerMs);
}
}
encodeFileStreamBlock(blockContents: Uint8Array,
blockStart: number,
streamID: number): Uint8Array {
// Create entire message buffer (including protocol wrappers)
const msgBuf = new Uint8Array(
blockContents.length + 4 + RICREST_HEADER_PAYLOAD_POS + RICSERIAL_PAYLOAD_POS,
);
let msgBufPos = 0;
// RICSERIAL protocol
msgBuf[msgBufPos++] = 0; // not numbered
msgBuf[msgBufPos++] =
(RICCommsMsgTypeCode.MSG_TYPE_COMMAND << 6) +
RICCommsMsgProtocol.MSG_PROTOCOL_RICREST;
// RICREST protocol
msgBuf[msgBufPos++] = RICRESTElemCode.RICREST_ELEM_CODE_FILEBLOCK;
// Buffer header
msgBuf[msgBufPos++] = streamID & 0xff;
msgBuf[msgBufPos++] = (blockStart >> 16) & 0xff;
msgBuf[msgBufPos++] = (blockStart >> 8) & 0xff;
msgBuf[msgBufPos++] = blockStart & 0xff;
// Copy block info
msgBuf.set(blockContents, msgBufPos);
return msgBuf;
}
async sendFileBlock(
blockContents: Uint8Array,
blockStart: number
): Promise<boolean> {
const msgBuf = this.encodeFileStreamBlock(blockContents, blockStart, 0);
// // Debug
// RICLog.debug(
// `sendFileBlock frameLen ${msgBuf.length} start ${blockStart} end ${blockEnd} len ${blockLen}`,
// );
// Send
try {
// Send
if (this._msgSender) {
// Wrap into HDLC
const framedMsg = this._miniHDLC.encode(msgBuf);
// Send
return this._msgSender.sendTxMsg(
framedMsg,
true,
// Platform.OS === 'ios',
);
}
} catch (error: unknown) {
RICLog.warn(`RICMsgHandler sendFileBlock error${error}`);
}
return false;
}
async sendStreamBlock(
blockContents: Uint8Array,
blockStart: number,
streamID: number,
): Promise<boolean> {
// Ensure any waiting messages are sent first
await this._onMsgTrackTimer(false);
// Encode message
const msgBuf = this.encodeFileStreamBlock(blockContents, blockStart, streamID);
// // Debug
// RICLog.debug(
// `sendStreamBlock frameLen ${msgBuf.length} start ${blockStart} end ${blockEnd} len ${blockLen}`,
// );
// Send
try {
// Send
if (this._msgSender) {
// Wrap into HDLC
const framedMsg = this._miniHDLC.encode(msgBuf);
// Send
return await this._msgSender.sendTxMsg(
framedMsg,
true,
// Platform.OS === 'ios',
);
}
} catch (error: unknown) {
RICLog.warn(`RICMsgHandler sendStreamBlock error${error}`);
}
return false;
}
}