@robotical/roboticaljs
Version:
Javascript/TS library for Robotical products
377 lines • 16.1 kB
JavaScript
;
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// RICJS
// Communications Library
//
// Rob Dobson & Chris Greening 2020-2022
// (C) 2020-2022
//
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Object.defineProperty(exports, "__esModule", { value: true });
exports.RICROSSerial = exports.ROSTOPIC_V2_CAMERA = exports.ROSTOPIC_V2_ROBOT_STATUS = exports.ROSTOPIC_V2_ADDONS = exports.ROSTOPIC_V2_POWER_STATUS = exports.ROSTOPIC_V2_ACCEL = exports.ROSTOPIC_V2_SMART_SERVOS = exports.ROSCameraData = exports.ROSSerialRobotStatus = exports.ROSSerialRGBT = exports.ROSSerialAddOnStatusList = exports.ROSSerialAddOnStatus = exports.ROSSerialPowerStatus = exports.ROSSerialIMU = exports.ROSSerialSmartServos = void 0;
const raftjs_1 = require("@robotical/raftjs");
class ROSSerialSmartServos {
constructor() {
this.smartServos = [];
}
}
exports.ROSSerialSmartServos = ROSSerialSmartServos;
class ROSSerialIMU {
constructor() {
this.accel = { x: 0, y: 0, z: 0 };
}
}
exports.ROSSerialIMU = ROSSerialIMU;
class ROSSerialPowerStatus {
constructor() {
this.powerStatus = {
battRemainCapacityPercent: 0,
battTempDegC: 0,
battRemainCapacityMAH: 0,
battFullCapacityMAH: 0,
battCurrentMA: 0,
power5VOnTimeSecs: 0,
power5VIsOn: false,
powerUSBIsConnected: false,
battInfoValid: false,
powerUSBIsValid: false,
powerFlags: 0,
};
}
}
exports.ROSSerialPowerStatus = ROSSerialPowerStatus;
class ROSSerialAddOnStatus {
constructor() {
this.id = 0;
this.deviceTypeID = 0;
this.whoAmI = "";
this.name = "";
this.status = 0;
this.vals = {};
}
}
exports.ROSSerialAddOnStatus = ROSSerialAddOnStatus;
class ROSSerialAddOnStatusList {
constructor() {
this.addons = new Array();
}
}
exports.ROSSerialAddOnStatusList = ROSSerialAddOnStatusList;
class ROSSerialRGBT {
constructor(r, g, b, t) {
this.r = 0;
this.g = 0;
this.b = 0;
this.t = 0;
this.r = r;
this.g = g;
this.b = b;
this.t = t;
}
toString() {
return `R:${this.r} G:${this.g} B:${this.b} T:${this.t}`;
}
}
exports.ROSSerialRGBT = ROSSerialRGBT;
class ROSSerialRobotStatus {
constructor() {
this.robotStatus = {
flags: 0,
isMoving: false,
isPaused: false,
isFwUpdating: false,
workQCount: 0,
heapFree: 0,
heapMin: 0,
pixRGBT: [],
loopMsAvg: 0,
loopMsMax: 0,
wifiRSSI: 0,
bleRSSI: 0,
};
}
}
exports.ROSSerialRobotStatus = ROSSerialRobotStatus;
class ROSCameraData {
constructor() {
this.cameraData = {
unixTimeMs: 0,
sinceStartMs: 0,
imageData: new Uint8Array(0),
imageWidth: 0,
imageHeight: 0,
imageFormat: 0,
imageQuality: 0,
frameTimeMs: 0,
imageCount: 0
};
}
}
exports.ROSCameraData = ROSCameraData;
// ROSSerial ROSTopics
exports.ROSTOPIC_V2_SMART_SERVOS = 120;
exports.ROSTOPIC_V2_ACCEL = 121;
exports.ROSTOPIC_V2_POWER_STATUS = 122;
exports.ROSTOPIC_V2_ADDONS = 123;
exports.ROSTOPIC_V2_ROBOT_STATUS = 124;
exports.ROSTOPIC_V2_CAMERA = 200;
class RICROSSerial {
static decode(rosSerialMsg, startPos, commsStats, addOnManager, ricStateInfo, frameTimeMs) {
// ROSSerial message format
const RS_MSG_MIN_LENGTH = 8;
const RS_MSG_LEN_LOW_POS = 2;
const RS_MSG_LEN_HIGH_POS = 3;
const RS_MSG_TOPIC_ID_LOW_POS = 5;
const RS_MSG_TOPIC_ID_HIGH_POS = 6;
const RS_MSG_PAYLOAD_POS = 7;
// Max payload length
const MAX_VALID_PAYLOAD_LEN = 200000;
// Update stats
commsStats.updateROSSerialRxRate(rosSerialMsg.length, frameTimeMs);
// Payload may contain multiple ROSSerial messages
let msgPos = startPos;
const topicIDs = new Array();
for (;;) {
const remainingMsgLen = rosSerialMsg.length - msgPos;
// RaftLog.debug('ROSSerial Decode ' + remainingMsgLen);
if (remainingMsgLen < RS_MSG_MIN_LENGTH)
break;
// Extract header
const payloadLength = rosSerialMsg[msgPos + RS_MSG_LEN_LOW_POS] +
rosSerialMsg[msgPos + RS_MSG_LEN_HIGH_POS] * 256;
const topicID = rosSerialMsg[msgPos + RS_MSG_TOPIC_ID_LOW_POS] +
rosSerialMsg[msgPos + RS_MSG_TOPIC_ID_HIGH_POS] * 256;
// RaftLog.debug('ROSSerial ' + payloadLength + ' topic ' + topicID);
// Check max length
if (payloadLength < 0 || payloadLength > MAX_VALID_PAYLOAD_LEN)
break;
// Check min length
if (rosSerialMsg.length < payloadLength + RS_MSG_MIN_LENGTH)
break;
// Extract payload
const payload = rosSerialMsg.slice(msgPos + RS_MSG_PAYLOAD_POS, msgPos + RS_MSG_PAYLOAD_POS + payloadLength);
// RaftLog.debug('ROSSerial ' + RaftUtils.bufferToHex(payload));
// we need to register the static addons here in case
// marty only has static addons (and so the rostopic_v2_addons case
// never runs)
let allAdons = { addons: [] };
const staticAddons = addOnManager.getProcessedStaticAddons();
for (const staticAddon of staticAddons) {
allAdons.addons.push(staticAddon);
}
if (commsStats._msgAddOnPub === 0) {
// we set the static addons only if we don't have any other addons
// the _msgAddOnPub is incremented in the rostopic_v2_addons case
// (when we get addons from marty)
// otherwise, the static addons will be set along with the regular addons (below)
ricStateInfo.addOnInfo = allAdons;
ricStateInfo.addOnInfoValidMs = Date.now();
}
// Record topic ID
topicIDs.push(topicID);
// Update state info
switch (topicID) {
case exports.ROSTOPIC_V2_SMART_SERVOS:
// Smart Servos
ricStateInfo.smartServos = this.extractSmartServos(payload);
ricStateInfo.smartServosValidMs = Date.now();
commsStats.recordSmartServos();
break;
case exports.ROSTOPIC_V2_ACCEL:
// Accelerometer
ricStateInfo.imuData = this.extractAccel(payload);
ricStateInfo.imuDataValidMs = Date.now();
commsStats.recordIMU();
break;
case exports.ROSTOPIC_V2_POWER_STATUS:
// Power Status
ricStateInfo.power = this.extractPowerStatus(payload);
ricStateInfo.powerValidMs = Date.now();
commsStats.recordPowerStatus();
break;
case exports.ROSTOPIC_V2_ADDONS:
// Addons
allAdons = this.extractAddOnStatus(payload, addOnManager);
for (const staticAddon of staticAddons) {
allAdons.addons.push(staticAddon);
}
ricStateInfo.addOnInfo = allAdons;
ricStateInfo.addOnInfoValidMs = Date.now();
commsStats.recordAddOnPub();
break;
case exports.ROSTOPIC_V2_ROBOT_STATUS:
// Robot Status
ricStateInfo.robotStatus = this.extractRobotStatus(payload);
ricStateInfo.robotStatusValidMs = Date.now();
commsStats.recordRobotStatus();
break;
case exports.ROSTOPIC_V2_CAMERA:
// Camera
// RaftLog.debug('Camera ' + payloadLength);
ricStateInfo.cameraData = this.extractCameraData(payload, frameTimeMs);
ricStateInfo.cameraDataValidMs = Date.now();
commsStats.recordOtherTopic();
break;
default:
// Unknown topic
commsStats.recordOtherTopic();
break;
}
// Move msgPos on
msgPos += RS_MSG_PAYLOAD_POS + payloadLength + 1;
// RaftLog.debug('MsgPos ' + msgPos);
}
return topicIDs;
}
static extractSmartServos(buf) {
// Each group of attributes for a servo is a fixed size
const ROS_SMART_SERVOS_ATTR_GROUP_BYTES = 6;
const numGroups = Math.floor(buf.length / ROS_SMART_SERVOS_ATTR_GROUP_BYTES);
const msg = { smartServos: [] };
let bufPos = 0;
for (let i = 0; i < numGroups; i++) {
const servoId = buf[bufPos];
const servoPos = raftjs_1.RaftUtils.getBEInt16FromBuf(buf, bufPos + 1);
const servoCurrent = raftjs_1.RaftUtils.getBEUint16FromBuf(buf, bufPos + 3);
const servoStatus = buf[bufPos + 5];
bufPos += ROS_SMART_SERVOS_ATTR_GROUP_BYTES;
msg.smartServos.push({
id: servoId,
pos: servoPos,
current: servoCurrent,
status: servoStatus,
});
}
return msg;
}
static extractAccel(buf) {
// Three accelerometer floats
const x = raftjs_1.RaftUtils.getBEFloatFromBuf(buf);
const y = raftjs_1.RaftUtils.getBEFloatFromBuf(buf.slice(4));
const z = raftjs_1.RaftUtils.getBEFloatFromBuf(buf.slice(8));
return { accel: { x: x / 1024, y: y / 1024, z: z / 1024 } };
}
static extractPowerStatus(buf) {
// Power indicator values
// RaftLog.debug(`PowerStatus ${RaftUtils.bufferToHex(buf)}`);
const remCapPC = raftjs_1.RaftUtils.getBEUint8FromBuf(buf, 0);
const tempDegC = raftjs_1.RaftUtils.getBEUint8FromBuf(buf, 1);
const remCapMAH = raftjs_1.RaftUtils.getBEUint16FromBuf(buf, 2);
const fullCapMAH = raftjs_1.RaftUtils.getBEUint16FromBuf(buf, 4);
const currentMA = raftjs_1.RaftUtils.getBEInt16FromBuf(buf, 6);
const power5VOnTimeSecs = raftjs_1.RaftUtils.getBEUint16FromBuf(buf, 8);
const powerFlags = raftjs_1.RaftUtils.getBEUint16FromBuf(buf, 10);
const isOnUSBPower = (powerFlags & 0x0001) != 0;
const is5VOn = (powerFlags & 0x0002) != 0;
const isBattInfoValid = (powerFlags & 0x0004) == 0;
const isUSBPowerInfoValid = (powerFlags & 0x0008) == 0;
return {
powerStatus: {
battRemainCapacityPercent: remCapPC,
battTempDegC: tempDegC,
battRemainCapacityMAH: remCapMAH,
battFullCapacityMAH: fullCapMAH,
battCurrentMA: currentMA,
power5VOnTimeSecs: power5VOnTimeSecs,
power5VIsOn: is5VOn,
powerUSBIsConnected: isOnUSBPower && isUSBPowerInfoValid,
battInfoValid: isBattInfoValid,
powerUSBIsValid: isUSBPowerInfoValid,
powerFlags: powerFlags,
},
};
}
static extractAddOnStatus(buf, addOnManager) {
// RaftLog.debug(`AddOnRawData ${RaftUtils.bufferToHex(buf)}`);
// Each group of attributes for a add-on is a fixed size
const ROS_ADDON_ATTR_GROUP_BYTES = 12;
const numGroups = Math.floor(buf.length / ROS_ADDON_ATTR_GROUP_BYTES);
const msg = { addons: [] };
let bufPos = 0;
for (let i = 0; i < numGroups; i++) {
const addOnId = buf[bufPos];
const status = buf[bufPos + 1];
const addOnData = buf.slice(bufPos + 2, bufPos + 12);
bufPos += ROS_ADDON_ATTR_GROUP_BYTES;
const addOnRec = addOnManager.processPublishedData(addOnId, status, addOnData);
if (addOnRec !== null) {
msg.addons.push(addOnRec);
}
}
return msg;
}
static extractRGBT(buf, offset) {
return new ROSSerialRGBT(buf[offset], buf[offset + 1], buf[offset + 2], buf[offset + 3]);
}
static extractRobotStatus(buf) {
const flags = raftjs_1.RaftUtils.getBEUint8FromBuf(buf, 0);
const workQCount = raftjs_1.RaftUtils.getBEUint8FromBuf(buf, 1);
let heapFree = 0;
let heapMin = 0;
let pixRGBT1 = new ROSSerialRGBT(0, 0, 0, 0);
let pixRGBT2 = new ROSSerialRGBT(0, 0, 0, 0);
let pixRGBT3 = new ROSSerialRGBT(0, 0, 0, 0);
let loopMsAvg = 0;
let loopMsMax = 0;
// RaftLog.debug(`RobotStatus ${buf.length} ${RaftUtils.bufferToHex(buf)} ${flags} ${workQCount} ${heapFree} ${heapMin} ${pixRGBT1.toString()} ${pixRGBT2.toString()} ${pixRGBT3.toString()} ${loopMsAvg} ${loopMsMax}`);
let wifiRSSI = 0;
let bleRSSI = 0;
if (buf.length >= 24) {
heapFree = raftjs_1.RaftUtils.getBEUint32FromBuf(buf, 2);
heapMin = raftjs_1.RaftUtils.getBEUint32FromBuf(buf, 6);
pixRGBT1 = RICROSSerial.extractRGBT(buf, 10);
pixRGBT2 = RICROSSerial.extractRGBT(buf, 14);
pixRGBT3 = RICROSSerial.extractRGBT(buf, 18);
loopMsAvg = raftjs_1.RaftUtils.getBEUint8FromBuf(buf, 22);
loopMsMax = raftjs_1.RaftUtils.getBEUint8FromBuf(buf, 23);
// RaftLog.debug(`RobotStatus ${buf.length} ${RaftUtils.bufferToHex(buf)} ${flags} ${workQCount} ${heapFree} ${heapMin} ${pixRGBT1.toString()} ${pixRGBT2.toString()} ${pixRGBT3.toString()} ${loopMsAvg} ${loopMsMax}`);
if (buf.length >= 26) {
wifiRSSI = raftjs_1.RaftUtils.getBEInt8FromBuf(buf, 24);
bleRSSI = raftjs_1.RaftUtils.getBEInt8FromBuf(buf, 25);
}
}
return {
robotStatus: {
flags: flags,
isMoving: (flags & 0x01) != 0,
isPaused: (flags & 0x02) != 0,
isFwUpdating: (flags & 0x04) != 0,
workQCount: workQCount,
heapFree: heapFree,
heapMin: heapMin,
pixRGBT: [pixRGBT1, pixRGBT2, pixRGBT3],
loopMsAvg: loopMsAvg,
loopMsMax: loopMsMax,
wifiRSSI: wifiRSSI,
bleRSSI: bleRSSI,
},
};
}
static extractCameraData(buf, frameTimeMs) {
raftjs_1.RaftLog.verbose(`CameraData ${buf.length} ${raftjs_1.RaftUtils.bufferToHex(buf.slice(0, 30))}`);
const unixTime = raftjs_1.RaftUtils.getBEUint64FromBuf(buf, 0);
const sinceStartMs = raftjs_1.RaftUtils.getBEUint32FromBuf(buf, 8);
const imageWidth = raftjs_1.RaftUtils.getBEUint16FromBuf(buf, 12);
const imageHeight = raftjs_1.RaftUtils.getBEUint16FromBuf(buf, 14);
const imageFormat = raftjs_1.RaftUtils.getBEUint8FromBuf(buf, 16);
const imageQuality = raftjs_1.RaftUtils.getBEUint8FromBuf(buf, 17);
const imageCount = raftjs_1.RaftUtils.getBEUint32FromBuf(buf, 18);
const imageData = buf.slice(22);
return {
cameraData: {
unixTimeMs: unixTime,
sinceStartMs: sinceStartMs,
imageWidth: imageWidth,
imageHeight: imageHeight,
imageFormat: imageFormat,
imageQuality: imageQuality,
imageData: imageData,
frameTimeMs: frameTimeMs,
imageCount: imageCount
}
};
}
}
exports.RICROSSerial = RICROSSerial;
//# sourceMappingURL=RICROSSerial.js.map