@robdobsn/raftjs 1.1.1

package/examples/dashboard/src/SystemTypeCog/CogStateInfo.ts

@@ -0,0 +1,144 @@
+import { time } from "console";
+import RaftLog from "../../../../src/RaftLog";
+
+export interface IMUStateInfo {
+    gx: number;
+    gy: number;
+    gz: number;
+    ax: number;
+    ay: number;
+    az: number;
+    tsMs: number;
+}
+
+export interface DeviceLastTsState {
+    lastMs: number;
+    offsetMs: number;
+}
+
+export interface DeviceMsgJsonElem {
+    _t: string; // Device type
+    _o: string; // Device name
+    x: string; // Hex encoded message values
+}
+
+export interface DeviceMsgJsonBus {
+    [devAddr: string]: DeviceMsgJsonElem;
+}
+  
+export interface DeviceMsgJson {
+    [busName: string]: DeviceMsgJsonBus;
+}
+
+export class CogStateInfo {
+
+    LSM6DS: IMUStateInfo = {gx: 0, gy: 0, gz: 0, ax: 0, ay: 0, az: 0, tsMs: 0};
+
+    // Last timestamp for each device
+    private _deviceLastTs: {[devName: string]: DeviceLastTsState} = {};
+    
+    updateFromMsg(rxMsg: Uint8Array, frameTimeMs: number): Array<string> {
+
+        // RaftLog.info(`CogStateInfo: updateFromMsg: rxMsg: ${rxMsg} frameTimeMs: ${frameTimeMs}`);
+
+        // Convert Uint8Array to string
+        const decoder = new TextDecoder('utf-8');
+        const jsonString = decoder.decode(rxMsg.slice(2));
+
+        // Debug
+        // RaftLog.info(`CogStateInfo: updateFromMsg: jsonString: ${jsonString}`);
+
+        // Parse JSON string to TypeScript object
+        const deviceMsg: DeviceMsgJson = JSON.parse(jsonString);
+
+        // Debug
+        // for (const busName in deviceMsg) {
+        //     for (const devAddr in deviceMsg[busName]) {
+        //         for (const attrGroupName in deviceMsg[busName][devAddr]) {
+        //             RaftLog.info(`CogStateInfo: updateFromMsg: busName ${busName} devAddr ${devAddr} attrGroupName: ${attrGroupName}`);
+        //             RaftLog.info(`CogStateInfo: updateFromMsg: attrValue: ${deviceMsg[busName][devAddr][attrGroupName]}`);
+        //         }
+        //     }
+        // }
+
+        // Iterate over values and extract
+        for (const busName in deviceMsg) {
+            for (const devAddr in deviceMsg[busName]) {
+
+                // Validate
+                if ((deviceMsg[busName][devAddr].x === undefined) || (deviceMsg[busName][devAddr]._t === undefined)) {
+                    RaftLog.warn(`CogStateInfo: updateFromMsg: Invalid message`);
+                    continue;
+                }
+
+                // Extract message and device type
+                const msgHex = deviceMsg[busName][devAddr].x;
+                const devType = deviceMsg[busName][devAddr]._t;
+
+                // Check if message is empty
+                if (msgHex === '') {
+                    RaftLog.warn(`CogStateInfo: updateFromMsg: Empty message`);
+                    continue;
+                }
+
+                // Convert the hex string to an arraybuffer by converting each pair of hex chars to a byte
+                const msgBytes = this.hexToBytes(msgHex);
+
+                // Convert to a Buffer
+                const msgBuffer = Buffer.from(msgBytes);
+
+                // Extract timestamp
+                let timestamp = msgBuffer.readUInt16BE(0);
+                if (this._deviceLastTs[devType] === undefined) {
+                    this._deviceLastTs[devType] = {lastMs: 0, offsetMs: 0};
+                }
+                const devTsState = this._deviceLastTs[devType];
+                if (timestamp < devTsState.lastMs) {
+                    devTsState.offsetMs += 0x10000;
+                }
+                devTsState.lastMs = timestamp;
+                timestamp += devTsState.offsetMs;
+                let msgBufferOffset = 2;
+                // RaftLog.info(`CogStateInfo: updateFromMsg: timestamp: ${timestamp}`);
+
+                // Handle device types
+                switch (devType) {
+                    case 'LSM6DS':
+                        // RaftLog.info(`CogStateInfo: updateFromMsg: IMU`);
+
+                        // LSD6DS IMU data is coded as 16-bit signed integers little-endian
+                        // gx, gy, gz, ax, ay, az
+                        this.LSM6DS = {
+                            gx: msgBuffer.readInt16LE(msgBufferOffset) / 16.384,
+                            gy: msgBuffer.readInt16LE(msgBufferOffset + 2) / 16.384,
+                            gz: msgBuffer.readInt16LE(msgBufferOffset + 4) / 16.384,
+                            ax: msgBuffer.readInt16LE(msgBufferOffset + 6) / 8192,
+                            ay: msgBuffer.readInt16LE(msgBufferOffset + 8) / 8192,
+                            az: msgBuffer.readInt16LE(msgBufferOffset + 10) / 8192,
+                            tsMs: timestamp
+                        };
+
+                        // Debug
+                        // RaftLog.info(`CogStateInfo: updateFromMsg: LSM6DS: gx: ${this.LSM6DS.gx.toFixed(2)} gy: ${this.LSM6DS.gy.toFixed(2)} gz: ${this.LSM6DS.gz.toFixed(2)} ax: ${this.LSM6DS.ax.toFixed(2)} ay: ${this.LSM6DS.ay.toFixed(2)} az: ${this.LSM6DS.az.toFixed(2)} tsMs: ${this.LSM6DS.tsMs}`);
+
+
+
+                        // const imu = deviceMsg[busName][devAddr] as DeviceMsgJsonElem;
+                        // this.imu = {x: imu.x as number, y: imu.y as number, z: imu.z as number, lastMs: frameTimeMs};
+                        break;
+                }
+            }
+        }
+
+        return [];
+
+    }
+
+    private hexToBytes(hex: string): Uint8Array {
+        const bytes = new Uint8Array(hex.length / 2);
+        for (let i = 0; i < bytes.length; i++) {
+            bytes[i] = parseInt(hex.substr(i * 2, 2), 16);
+        }
+        return bytes;
+    }    
+  }

package/examples/dashboard/src/SystemTypeCog/SystemTypeCog.ts

@@ -0,0 +1,77 @@
+import { RaftSubscribeForUpdatesCBType, RaftSystemType } from "../../../../src/RaftSystemType";
+import { RaftEventFn, RaftLog, RaftOKFail, RaftPublishEvent, RaftPublishEventNames, RaftSystemUtils } from "../../../../src/main";
+import { CogStateInfo } from "./CogStateInfo";
+
+export default class SystemTypeCog implements RaftSystemType {
+    nameForDialogs = "Robotical Cog";
+    defaultWiFiHostname = "Cog";
+    firmwareDestName = "ricfw";
+    normalFileDestName = "fs";
+    connectorOptions = {wsSuffix: "wsjson"};
+
+    // Latest data from servos, IMU, etc
+    private _stateInfo: CogStateInfo = new CogStateInfo();
+    getStateInfo(): CogStateInfo {
+      return this._stateInfo;
+    }
+
+    // Event handler
+    private _onEvent: RaftEventFn | null = null;
+
+    // Raft system utils
+    private _systemUtils: RaftSystemUtils | null = null;
+
+    // Setup
+    setup(systemUtils: RaftSystemUtils, onEvent: RaftEventFn | null): void {
+      this._systemUtils = systemUtils;
+      this._onEvent = onEvent;
+    }
+
+    // Subscribe for updates
+    subscribeForUpdates: RaftSubscribeForUpdatesCBType | null = async (systemUtils: RaftSystemUtils, enable: boolean) => {
+      // Subscription rate
+      const subscribeRateHz = 0.1;
+      try {
+        const subscribeDisable = '{"cmdName":"subscription","action":"update",' +
+          '"pubRecs":[' +
+          `{"name":"devjson","rateHz":0,}` +
+          ']}';
+        const subscribeEnable = '{"cmdName":"subscription","action":"update",' +
+          '"pubRecs":[' +
+          `{"name":"devjson","trigger":"timeorchange","rateHz":${subscribeRateHz.toString()}}` +
+          ']}';
+
+        const msgHandler = systemUtils.getMsgHandler();
+        const ricResp = await msgHandler.sendRICRESTCmdFrame<RaftOKFail>(
+          enable ? subscribeEnable : subscribeDisable
+        );
+
+        // Debug
+        RaftLog.debug(`subscribe enable/disable returned ${JSON.stringify(ricResp)}`);
+      } catch (error: unknown) {
+        RaftLog.warn(`getRICCalibInfo Failed subscribe for updates ${error}`);
+      }
+    };
+
+    // Invalidate state
+    stateIsInvalid(): void {};
+
+    // Other message type
+    rxOtherMsgType(payload: Uint8Array, frameTimeMs: number) {
+
+      // RICLog.debug(`rxOtherMsgType payload ${RICUtils.bufferToHex(payload)}`);
+      RaftLog.verbose(`rxOtherMsgType payloadLen ${payload.length}`);
+      const topicIDs = this._stateInfo.updateFromMsg(payload, frameTimeMs);
+
+      // Call event handler if registered
+      if (this._onEvent) {
+        this._onEvent("pub", RaftPublishEvent.PUBLISH_EVENT_DATA, RaftPublishEventNames[RaftPublishEvent.PUBLISH_EVENT_DATA],
+          {
+            topicIDs: topicIDs,
+            payload: payload,
+            frameTimeMs: frameTimeMs
+          });
+      }
+
+    };
+  }

package/examples/dashboard/src/SystemTypeMarty/RICAddOn.ts

@@ -0,0 +1,70 @@
+import { RICHWElem } from "./RICHWElem";
+
+/**
+ * RICAddOn
+ * 
+ * @description
+ * Information about an add-on
+ * 
+ * @field name: string - Name of add-on
+ * @field SN: string - Serial number
+ * @field poll: string - polling type ("status")
+ * @field pollRd: string - hex data most recently read
+ * @field pollHz: number - rate of polling
+ */
+export type RICAddOn = {
+  name: string;
+  SN: string;
+  poll: string;
+  pollRd: number;
+  pollHz: number;
+};
+
+export class RICConfiguredAddOns {
+  req = '';
+  rslt = 'ok';
+  addons: Array<RICAddOn> = [];
+}
+
+/**
+ * AddOnElemAndConfig
+ * 
+ * @description
+ * Carrier of information about an add-on combining
+ * the add-on element and the add-on configuration
+ * 
+ * @field addOnConfig: RICAddOn - Add-on configuration
+ * @field hwElemRec: RICHWElem - Add-on element
+ * @field elemIdx: number - Index of the add-on element
+ */
+export class AddOnElemAndConfig {
+  constructor(
+    addOnConfig: RICAddOn | null,
+    hwElemRec: RICHWElem | null,
+    elemIdx: number,
+  ) {
+    this.isConfigured = addOnConfig !== null;
+    this.isConnected = hwElemRec !== null;
+    if (addOnConfig != null) {
+      this.SN = addOnConfig.SN;
+      this.name = addOnConfig.name;
+    } else if (hwElemRec != null) {
+      this.SN = hwElemRec.SN;
+      this.name = hwElemRec.name;
+    }
+    this.addOnConfig = addOnConfig;
+    this.hwElemRec = hwElemRec;
+    this.id = elemIdx.toString();
+  }
+
+  // Fields from config (stored in RIC NVS using addon REST API)
+  addOnConfig: RICAddOn | null = null;
+  // Fields from HWElem (from hwstatus command)
+  hwElemRec: RICHWElem | null = null;
+  // Fields allocated when combining records
+  name = '';
+  SN = '';
+  id = '0';
+  isConnected = false;
+  isConfigured = false;
+}

package/examples/dashboard/src/SystemTypeMarty/RICAddOnBase.ts

@@ -0,0 +1,33 @@
+/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+//
+// RICJS
+// Communications Library
+//
+// Rob Dobson & Chris Greening 2020-2022
+// (C) 2020-2022
+//
+/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+import { RaftReportMsg } from '../../../../src/RaftTypes';
+import { ROSSerialAddOnStatus } from './RICROSSerial';
+
+export default abstract class RICAddOnBase {
+  _name = '';
+  _typeName = '';
+  _whoAmI = "";
+  _whoAmITypeCode = "";
+  _isStatic = false;
+  _initCmd: string | null = null;
+  constructor(name: string, typeName: string, whoAmI: string, whoAmITypeCode: string) {
+    this._name = name;
+    this._typeName = typeName;
+    this._whoAmI = whoAmI;
+    this._whoAmITypeCode = whoAmITypeCode;
+  }
+  abstract processInit(_dataReceived: RaftReportMsg): void;
+  abstract processPublishedData(
+    addOnID: number,
+    statusByte: number,
+    rawData: Uint8Array,
+  ): ROSSerialAddOnStatus;
+}

package/examples/dashboard/src/SystemTypeMarty/RICAddOnManager.ts

@@ -0,0 +1,342 @@
+/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+//
+// RICJS
+// Communications Library
+//
+// Rob Dobson & Chris Greening 2020-2022
+// (C) 2020-2022
+//
+/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+import RaftLog from "../../../../src/RaftLog";
+import RaftMsgHandler from "../../../../src/RaftMsgHandler";
+import { Dictionary, RaftOKFail, RaftReportMsg } from "../../../../src/RaftTypes";
+import RICAddOnBase from "./RICAddOnBase";
+import { RICHWElem } from "./RICHWElem";
+import { ROSSerialAddOnStatus } from "./RICROSSerial";
+
+export type RICAddOnCreator = (
+  name: string,
+  addOnFamily: string,
+  whoAmI: string,
+  whoAmITypeCode: string
+) => RICAddOnBase;
+
+class AddOnFactoryElem {
+  typeName: string;
+  addOnFamily: string;
+  whoAmI: string;
+  factoryFn: RICAddOnCreator;
+  constructor(
+    typeName: string,
+    addOnFamily: string,
+    whoAmI: string,
+    factoryFn: RICAddOnCreator
+  ) {
+    this.addOnFamily = addOnFamily;
+    this.typeName = typeName;
+    this.whoAmI = whoAmI;
+    this.factoryFn = factoryFn;
+  }
+}
+
+export interface RICAddOnRegistry {
+  registerHWElemType(
+    typeName: string,
+    addOnFamily: string,
+    whoAmI: string,
+    factoryFn: RICAddOnCreator
+  ): void;
+}
+
+/**
+ * RICAddOnManager
+ *
+ * @description
+ * Handles the creation and management of RIC Add-Ons
+ *
+ */
+export default class RICAddOnManager implements RICAddOnRegistry {
+
+  // Message handler, etc
+  private _msgHandler: RaftMsgHandler | null = null;
+
+  // Add-on factory map
+  private _addOnFactoryMap: Dictionary<AddOnFactoryElem> = {};
+
+  // Configured add-ons
+  private _configuredAddOns: Dictionary<RICAddOnBase> = {};
+
+  /**
+   * Setup the RIC Add-On Manager
+   */
+  setup(msgHandler: RaftMsgHandler): void {
+    this._msgHandler = msgHandler;
+  }
+
+  /**
+   * 
+   * @param typeName Register HWElem type
+   * @param addOnFamily 
+   * @param whoAmI 
+   * @param factoryFn 
+   */
+  registerHWElemType(
+    typeName: string,
+    addOnFamily: string,
+    whoAmI: string,
+    factoryFn: RICAddOnCreator
+  ): void {
+    RaftLog.debug(`registerHWElemType ${whoAmI} ${typeName}`);
+    const lookupStr = addOnFamily + "_" + whoAmI;
+    this._addOnFactoryMap[lookupStr] = new AddOnFactoryElem(
+      typeName,
+      addOnFamily,
+      whoAmI,
+      factoryFn
+    );
+  }
+
+  // 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<RaftOKFail>
+   *
+   */
+  async setAddOnConfig(serialNo: string, newName: string): Promise<RaftOKFail> {
+    try {
+      if (this._msgHandler) {
+        const msgRslt = await this._msgHandler.sendRICRESTURL<RaftOKFail>(
+          `addon/set?SN=${serialNo}&name=${newName}`,
+        );
+        return msgRslt;
+      }
+    } catch (error) {
+    }
+    return new RaftOKFail();
+  }
+
+  /**
+   * deleteAddOn - remove an addon from the addonlist on RIC
+   * @param serialNo used to identify the add-on
+   * @returns Promise<RaftOKFail>
+   */
+  async deleteAddOn(serialNo: string): Promise<RaftOKFail> {
+    try {
+      if (this._msgHandler) {
+        const msgRslt = await this._msgHandler.sendRICRESTURL<RaftOKFail>(
+          `addon/del?SN=${serialNo}`,
+        );
+        return msgRslt;
+      }
+    } catch (error) {
+    }
+    return new RaftOKFail();
+  }
+
+  // Mark: Identify AddOn -----------------------------------------------------------
+
+  /**
+   *
+   * identifyAddOn - send the 'identify' command to a specified add-on
+   * @param name used to identify the add-on
+   * @returns Promise<RaftOKFail>
+   *
+   */
+  async identifyAddOn(name: string): Promise<RaftOKFail> {
+    try {
+      if (this._msgHandler) {
+        const msgRslt = await this._msgHandler.sendRICRESTURL<RaftOKFail>(
+          `elem/${name}/json?cmd=raw&hexWr=F8`,
+        );
+        return msgRslt;
+      }
+    } catch (error) {
+    }
+    return new RaftOKFail();
+  }
+
+  /**
+   * @function getStaticAddonIds
+   * Get the ids of the add-ons that are static
+   * (their data do not get published from ricjs, eg buspixel ledeyes)
+   * @returns {Array<number>} the ids of the static add-ons
+   */
+  getStaticAddonIds(): Array<number> {
+    // at this point we will create the buspixel addon for the batch 4 ledeye,
+    // as their data is not published the same way the RSAddOn do
+    // to do so, however, we need to know if the batch 4 ledeye is connected
+    const staticAddonIds = [];
+    for (const addOnId in this._configuredAddOns) {
+      const addon = this._configuredAddOns[addOnId];
+      if (addon._isStatic) {
+        staticAddonIds.push(+addOnId);
+      }
+    }
+    return staticAddonIds;
+  }
+
+  /**
+   * @function getStaticAddons
+   * Get the add-ons that are static
+   * (their data do not get published from ricjs, eg buspixel ledeyes)
+   * @returns {Array<RICAddOnBase>} the static add-ons unprocessed
+   */
+  getStaticAddons(): Array<RICAddOnBase> {
+    const staticAddons = [];
+    for (const addOnId in this._configuredAddOns) {
+      const addon = this._configuredAddOns[addOnId];
+      if (addon._isStatic) {
+        staticAddons.push(addon);
+      }
+    }
+    return staticAddons;
+  }
+
+  /**
+   * @function getProcessedStaticAddons
+   * Get the add-ons that are static
+   * (their data do not get published from ricjs, eg buspixel ledeyes)
+   * @returns {Array<ROSSerialAddOnStatus>} the static add-ons processed
+   */
+  getProcessedStaticAddons(): Array<ROSSerialAddOnStatus> {
+    const ids = this.getStaticAddonIds();
+    const staticAddons: ROSSerialAddOnStatus[] = [];
+    ids.forEach((id) => {
+      const processedAddon = this.processPublishedData(
+        id,
+        0,
+        new Uint8Array(0)
+      );
+      if (processedAddon) {
+        staticAddons.push(processedAddon);
+      }
+    });
+    return staticAddons;
+  }
+
+  /**
+   * @function setHWElems
+   * Set the hardware elements from a list of RICHWElem
+   * @param hwElems
+   *
+   */
+  setHWElems(hwElems: Array<RICHWElem>): void {
+    this._configuredAddOns = this.configureAddOns(hwElems);
+  }
+
+  clear(): void {
+    this._configuredAddOns = {};
+  }
+
+  configureAddOns(hwElems: Array<RICHWElem>): Dictionary<RICAddOnBase> {
+    const addOnMap: Dictionary<RICAddOnBase> = {};
+    // Iterate HWElems to find addons
+    for (const hwElem of hwElems) {
+      RaftLog.debug(`configureAddOns whoAmITypeCode ${hwElem.whoAmI}`);
+
+      // Lookup the add-on
+      const lookupStr = hwElem.type + "_" + hwElem.whoAmI;
+      if (lookupStr in this._addOnFactoryMap) {
+        const addOnFactoryElem = this._addOnFactoryMap[lookupStr];
+        const whoAmILen = hwElem.whoAmITypeCode.length;
+        hwElem.whoAmITypeCode = hwElem.whoAmITypeCode.slice(
+          whoAmILen - 2,
+          whoAmILen
+        );
+        const addOn = addOnFactoryElem.factoryFn(
+          hwElem.name,
+          hwElem.type,
+          hwElem.whoAmI,
+          hwElem.whoAmITypeCode
+        );
+        if (addOn !== null) {
+          addOnMap[hwElem.IDNo.toString()] = addOn;
+        }
+      }
+    }
+    return addOnMap;
+  }
+
+  getHWElemTypeStr(
+    whoAmITypeCode: string | undefined,
+    whoAmI: string | undefined
+  ) {
+    RaftLog.debug(`getting type code for ${whoAmITypeCode}`);
+    if (whoAmITypeCode === undefined) {
+      return `Undefined whoamiTypeCode`;
+    }
+    if (whoAmITypeCode in this._addOnFactoryMap) {
+      return this._addOnFactoryMap[whoAmITypeCode].typeName;
+    }
+    return `Unknown (${whoAmI} - ${whoAmITypeCode})`;
+  }
+
+  processPublishedData(
+    addOnID: number,
+    statusByte: number,
+    rawData: Uint8Array
+  ): ROSSerialAddOnStatus | null {
+    // Lookup in map
+    const addOnIdStr = addOnID.toString();
+    if (addOnIdStr in this._configuredAddOns) {
+      const addOnHandler = this._configuredAddOns[addOnIdStr];
+      const data = addOnHandler.processPublishedData(
+        addOnID,
+        statusByte,
+        rawData
+      );
+      return data;
+    }
+    return null;
+  }
+  getIDNoFromName(name: string): string | null {
+    for (const key in this._configuredAddOns) {
+      if (key in this._configuredAddOns) {
+        if (this._configuredAddOns[key]._name == name) return key;
+      }
+    }
+    return null;
+  }
+
+  getInitCmds(): Array<string> {
+    const cmds: Array<string> = [];
+    for (const key in this._configuredAddOns) {
+      if (key in this._configuredAddOns) {
+        const initCmd = this._configuredAddOns[key]._initCmd;
+        if (initCmd) {
+          cmds.push(initCmd);
+        }
+      }
+    }
+    return cmds;
+  }
+
+  processReportMsg(reportMsgs: Array<RaftReportMsg>, timeInitStart: number) {
+    for (const reportID in reportMsgs) {
+      const report = reportMsgs[reportID];
+      //RaftLog.debug(`Report message: ${JSON.stringify(report)}`);
+      if (report.timeReceived && report.timeReceived < timeInitStart) {
+        continue;
+      }
+      if (report.elemName) {
+        let hwElemIDNoStr = "";
+        if (report.IDNo) {
+          hwElemIDNoStr = report.IDNo.toString();
+        } else if (report.elemName) {
+          const maybeIdno = this.getIDNoFromName(report.elemName);
+          if (maybeIdno) {
+            hwElemIDNoStr = maybeIdno;
+          }
+        }
+        if (hwElemIDNoStr.length > 0) {
+          this._configuredAddOns[hwElemIDNoStr].processInit(report);
+        }
+      }
+    }
+  }
+}