@robotical/ricjs-robotical-addons
Version:
JS/TS library for Robotical RIC Addons
473 lines • 18.2 kB
JavaScript
"use strict";
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// RICRoboticalAddOns
// Communications Connector for RIC V2
//
// RIC V2
// Rob Dobson & Chris Greening 2020
// (C) Robotical 2020-2022
//
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Object.defineProperty(exports, "__esModule", { value: true });
exports.CS_AIR_FLAG = exports.CS_TOUCH_FLAG = exports.CS_IRVAL = exports.CS_CLEAR = exports.IRF_AIR_FLAG = exports.IRF_TOUCH_FLAG = exports.IRF_AIR_AMBIENT = exports.IRF_TOUCH_AMBIENT = exports.IRF_AIR_VAL = exports.IRF_TOUCH_VAL = exports.RIC_WHOAMI_TYPE_CODE_ADDON_GRIPSERVO = exports.RIC_WHOAMI_TYPE_CODE_ADDON_NOISE = exports.RIC_WHOAMI_TYPE_CODE_ADDON_LEDEYE = exports.RIC_WHOAMI_TYPE_CODE_ADDON_LEDARM = exports.RIC_WHOAMI_TYPE_CODE_ADDON_LEDFOOT = exports.RIC_WHOAMI_TYPE_CODE_ADDON_IRFOOT = exports.RIC_WHOAMI_TYPE_CODE_ADDON_COLOUR = exports.RIC_WHOAMI_TYPE_CODE_ADDON_LIGHT = exports.RIC_WHOAMI_TYPE_CODE_ADDON_DISTANCE = void 0;
const ricjs_1 = require("@robotical/ricjs");
const estimate_detection_flags_1 = require("./estimate-detection-flags");
// RIC ADDON CODES
exports.RIC_WHOAMI_TYPE_CODE_ADDON_DISTANCE = "VCNL4200";
exports.RIC_WHOAMI_TYPE_CODE_ADDON_LIGHT = "lightsensor";
exports.RIC_WHOAMI_TYPE_CODE_ADDON_COLOUR = "coloursensor";
exports.RIC_WHOAMI_TYPE_CODE_ADDON_IRFOOT = "IRFoot";
exports.RIC_WHOAMI_TYPE_CODE_ADDON_LEDFOOT = "LEDfoot";
exports.RIC_WHOAMI_TYPE_CODE_ADDON_LEDARM = "LEDarm";
exports.RIC_WHOAMI_TYPE_CODE_ADDON_LEDEYE = "LEDeye";
exports.RIC_WHOAMI_TYPE_CODE_ADDON_NOISE = "noisesensor";
exports.RIC_WHOAMI_TYPE_CODE_ADDON_GRIPSERVO = "roboservo3";
// IRF and Colour sensor Val suffixes
exports.IRF_TOUCH_VAL = "Touch_IR";
exports.IRF_AIR_VAL = "Air_IR";
exports.IRF_TOUCH_AMBIENT = "Touch_Ambient";
exports.IRF_AIR_AMBIENT = "Air_Ambient";
exports.IRF_TOUCH_FLAG = "Touch";
exports.IRF_AIR_FLAG = "Air";
exports.CS_CLEAR = "Clear";
exports.CS_IRVAL = "IRVal";
exports.CS_TOUCH_FLAG = "Touch";
exports.CS_AIR_FLAG = "Air";
class RICRoboticalAddOns {
static registerAddOns(addOnRegistry) {
const addOns = {
[exports.RIC_WHOAMI_TYPE_CODE_ADDON_DISTANCE]: [{ "class": RICAddOnDistanceSensor, "typeName": "DistanceSensor", "addonFamily": "RSAddOn" }],
[exports.RIC_WHOAMI_TYPE_CODE_ADDON_LIGHT]: [{ "class": RICAddOnLightSensor, "typeName": "LightSensor", "addonFamily": "RSAddOn" }],
[exports.RIC_WHOAMI_TYPE_CODE_ADDON_COLOUR]: [{ "class": RICAddOnColourSensor, "typeName": "ColourSensor", "addonFamily": "RSAddOn" }],
[exports.RIC_WHOAMI_TYPE_CODE_ADDON_IRFOOT]: [{ "class": RICAddOnIRFoot, "typeName": "IRFoot", "addonFamily": "RSAddOn" }],
[exports.RIC_WHOAMI_TYPE_CODE_ADDON_LEDFOOT]: [{ "class": RICAddOnLEDFoot, "typeName": "DiscoFoot", "addonFamily": "RSAddOn" }],
[exports.RIC_WHOAMI_TYPE_CODE_ADDON_LEDARM]: [{ "class": RICAddOnLEDArm, "typeName": "DiscoArm", "addonFamily": "RSAddOn" }],
// the LEDEye addon we register it for both BusPixel and RSAddOn
// as batch 4 LEDEyes will come up as BusPixel
[exports.RIC_WHOAMI_TYPE_CODE_ADDON_LEDEYE]: [{ "class": RICAddOnLEDEye, "typeName": "DiscoEyes", "addonFamily": "RSAddOn" },
{ "class": RICAddOnLEDEyeBusPix, "typeName": "DiscoEyes", "addonFamily": "BusPixels" }],
[exports.RIC_WHOAMI_TYPE_CODE_ADDON_NOISE]: [{ "class": RICAddOnNoiseSensor, "typeName": "NoiseSensor", "addonFamily": "RSAddOn" }],
[exports.RIC_WHOAMI_TYPE_CODE_ADDON_GRIPSERVO]: [{ "class": RICAddOnGripServo, "typeName": "Gripper", "addonFamily": "RSAddOn" }],
};
for (const [key, arrayValue] of Object.entries(addOns)) {
for (const value of arrayValue) {
addOnRegistry.registerHWElemType(value.typeName, value.addonFamily, key, (name, addOnFamily, whoAmI, whoAmITypeCode) => {
return new value.class(name, addOnFamily, whoAmI, whoAmITypeCode);
});
}
}
}
}
exports.default = RICRoboticalAddOns;
// Format definitions
const ADDON_IRFOOT_FORMAT_DEF = {
fields: [
{
type: ricjs_1.RICDataExtractorVarType.VAR_BOOL,
suffix: exports.IRF_TOUCH_FLAG,
atBit: 8,
bits: 1,
postMult: 1,
postAdd: 0,
},
{
type: ricjs_1.RICDataExtractorVarType.VAR_BOOL,
suffix: exports.IRF_AIR_FLAG,
atBit: 9,
bits: 1,
postMult: 1,
postAdd: 0,
},
{
type: ricjs_1.RICDataExtractorVarType.VAR_UNSIGNED,
suffix: exports.IRF_TOUCH_VAL,
atBit: 16,
bits: 16,
postMult: 1,
postAdd: 0,
},
{
type: ricjs_1.RICDataExtractorVarType.VAR_UNSIGNED,
suffix: exports.IRF_AIR_VAL,
atBit: 32,
bits: 16,
postMult: 1,
postAdd: 0,
},
{
type: ricjs_1.RICDataExtractorVarType.VAR_UNSIGNED,
suffix: exports.IRF_TOUCH_AMBIENT,
atBit: 48,
bits: 16,
postMult: 1,
postAdd: 0,
},
{
type: ricjs_1.RICDataExtractorVarType.VAR_UNSIGNED,
suffix: exports.IRF_AIR_AMBIENT,
atBit: 64,
bits: 16,
postMult: 1,
postAdd: 0,
},
],
};
const ADDON_COLOURSENSOR_FORMAT_DEF = {
fields: [
{
type: ricjs_1.RICDataExtractorVarType.VAR_UNSIGNED,
suffix: exports.CS_CLEAR,
atBit: 8,
bits: 8,
postMult: 1,
postAdd: 0,
littleEndian: false,
},
{
type: ricjs_1.RICDataExtractorVarType.VAR_UNSIGNED,
suffix: 'Red',
atBit: 16,
bits: 8,
postMult: 1.65,
postAdd: 0,
},
{
type: ricjs_1.RICDataExtractorVarType.VAR_UNSIGNED,
suffix: 'Green',
atBit: 24,
bits: 8,
postMult: 1.15,
postAdd: 0,
},
{
type: ricjs_1.RICDataExtractorVarType.VAR_UNSIGNED,
suffix: 'Blue',
atBit: 32,
bits: 8,
postMult: 1.0,
postAdd: 0,
},
{
type: ricjs_1.RICDataExtractorVarType.VAR_BOOL,
suffix: exports.CS_TOUCH_FLAG,
atBit: 48,
bits: 1,
postMult: 1,
postAdd: 0,
},
{
type: ricjs_1.RICDataExtractorVarType.VAR_BOOL,
suffix: exports.CS_AIR_FLAG,
atBit: 49,
bits: 1,
postMult: 1,
postAdd: 0,
},
{
type: ricjs_1.RICDataExtractorVarType.VAR_UNSIGNED,
suffix: exports.CS_IRVAL,
atBit: 56,
bits: 16,
postMult: 1.0,
postAdd: 0,
},
],
};
const ADDON_DISTANCESENSOR_FORMAT_DEF = {
fields: [
{
type: ricjs_1.RICDataExtractorVarType.VAR_UNSIGNED,
suffix: 'Reading',
atBit: 8,
bits: 16,
postMult: 1,
postAdd: 0,
},
],
};
const ADDON_LIGHTSENSOR_FORMAT_DEF = {
fields: [
{
type: ricjs_1.RICDataExtractorVarType.VAR_UNSIGNED,
suffix: 'Reading1',
atBit: 8,
bits: 16,
postMult: 1,
postAdd: 0,
},
{
type: ricjs_1.RICDataExtractorVarType.VAR_UNSIGNED,
suffix: 'Reading2',
atBit: 24,
bits: 16,
postMult: 1,
postAdd: 0,
},
{
type: ricjs_1.RICDataExtractorVarType.VAR_UNSIGNED,
suffix: 'Reading3',
atBit: 40,
bits: 16,
postMult: 1,
postAdd: 0,
},
],
};
const ADDON_NOISESENSOR_FORMAT_DEF = {
fields: [
{
type: ricjs_1.RICDataExtractorVarType.VAR_UNSIGNED,
suffix: 'Smoothed',
atBit: 8,
bits: 16,
postMult: 1,
postAdd: 0,
},
{
type: ricjs_1.RICDataExtractorVarType.VAR_UNSIGNED,
suffix: 'HighestSinceLastReading',
atBit: 24,
bits: 16,
postMult: 1,
postAdd: 0,
},
{
type: ricjs_1.RICDataExtractorVarType.VAR_UNSIGNED,
suffix: 'Raw',
atBit: 40,
bits: 16,
postMult: 1,
postAdd: 0,
},
],
};
/* eslint no-unused-vars: ['error', { argsIgnorePattern: "^_" }] */
/* eslint-disable @typescript-eslint/no-unused-vars */
/*eslint no-empty-function: ["error", { "allow": ["methods"] }]*/
/* eslint-disable @typescript-eslint/no-empty-function */
class RICAddOnGripServo extends ricjs_1.RICAddOnBase {
constructor(name, typeName, whoAmI, whoAmITypeCode) {
super(name, typeName, whoAmI, whoAmITypeCode);
}
processPublishedData(addOnID, statusByte) {
// Status to return
const retStatus = new ricjs_1.ROSSerialAddOnStatus();
// RICLog.debug("RICADDONMANAGER: debugging info");
// Extract data
retStatus.id = addOnID;
retStatus.name = this._name;
retStatus.whoAmI = this._whoAmI;
retStatus.status = statusByte;
return retStatus;
}
processInit(_dataReceived) {
}
}
class RICAddOnLEDFoot extends ricjs_1.RICAddOnBase {
constructor(name, typeName, whoAmI, whoAmITypeCode) {
super(name, typeName, whoAmI, whoAmITypeCode);
}
processPublishedData(addOnID, statusByte) {
// Status to return
const retStatus = new ricjs_1.ROSSerialAddOnStatus();
// RICLog.debug("RICADDONMANAGER: debugging info");
// Extract data
retStatus.id = addOnID;
retStatus.name = this._name;
retStatus.whoAmI = this._whoAmI;
retStatus.status = statusByte;
return retStatus;
}
processInit(_dataReceived) {
}
}
class RICAddOnLEDArm extends ricjs_1.RICAddOnBase {
constructor(name, typeName, whoAmI, whoAmITypeCode) {
super(name, typeName, whoAmI, whoAmITypeCode);
}
processPublishedData(addOnID, statusByte) {
// Status to return
const retStatus = new ricjs_1.ROSSerialAddOnStatus();
// Extract data
retStatus.id = addOnID;
retStatus.name = this._name;
retStatus.whoAmI = this._whoAmI;
retStatus.status = statusByte;
return retStatus;
}
processInit(_dataReceived) {
}
}
class RICAddOnLEDEye extends ricjs_1.RICAddOnBase {
constructor(name, typeName, whoAmI, whoAmITypeCode) {
super(name, typeName, whoAmI, whoAmITypeCode);
}
processPublishedData(addOnID, statusByte) {
// Status to return
const retStatus = new ricjs_1.ROSSerialAddOnStatus();
// Extract data
retStatus.id = addOnID;
retStatus.name = this._name;
retStatus.whoAmI = this._whoAmI;
retStatus.status = statusByte;
return retStatus;
}
processInit(_dataReceived) {
}
}
class RICAddOnLEDEyeBusPix extends ricjs_1.RICAddOnBase {
constructor(name, typeName, whoAmI, whoAmITypeCode) {
super(name, typeName, whoAmI, whoAmITypeCode);
// static add-on is do not publish their data to ROS (e.g, LED eye buspix)
// we use this flag to grab these addons from withing the ricjs library
super._isStatic = true;
}
processPublishedData(addOnID) {
// Status to return
const retStatus = new ricjs_1.ROSSerialAddOnStatus();
// Extract data
retStatus.id = addOnID;
retStatus.name = this._name;
retStatus.whoAmI = this._whoAmI;
retStatus.status = 128;
return retStatus;
}
processInit(_dataReceived) {
}
}
class RICAddOnIRFoot extends ricjs_1.RICAddOnBase {
constructor(name, typeName, whoAmI, whoAmITypeCode) {
super(name, typeName, whoAmI, whoAmITypeCode);
this._dataExtractor = new ricjs_1.RICDataExtractor(name, ADDON_IRFOOT_FORMAT_DEF);
}
processPublishedData(addOnID, statusByte, rawData) {
// Status to return
const retStatus = new ricjs_1.ROSSerialAddOnStatus();
// Extract data
retStatus.id = addOnID;
retStatus.name = this._name;
retStatus.whoAmI = this._whoAmI;
retStatus.status = statusByte;
retStatus.vals = this._dataExtractor.extractData(rawData);
// adding predicted values for Touch and Ground
(0, estimate_detection_flags_1.estimateDetectionFlags)(retStatus, this._whoAmITypeCode);
return retStatus;
}
processInit(_dataReceived) {
}
}
class RICAddOnColourSensor extends ricjs_1.RICAddOnBase {
constructor(name, typeName, whoAmI, whoAmITypeCode) {
super(name, typeName, whoAmI, whoAmITypeCode);
this._dataExtractor = new ricjs_1.RICDataExtractor(name, ADDON_COLOURSENSOR_FORMAT_DEF);
this._initCmd = `elem/${name}/json?cmd=raw&hexWr=104020&numToRd=20&msgKey=1234`;
}
processPublishedData(addOnID, statusByte, rawData) {
// Status to return
const retStatus = new ricjs_1.ROSSerialAddOnStatus();
// Extract data
retStatus.id = addOnID;
retStatus.name = this._name;
retStatus.whoAmI = this._whoAmI;
retStatus.status = statusByte;
retStatus.vals = this._dataExtractor.extractData(rawData);
// adding predicted values for Touch and Ground
(0, estimate_detection_flags_1.estimateDetectionFlags)(retStatus, this._whoAmITypeCode);
return retStatus;
}
setCalibration(cal) {
if (cal.length != 4) {
console.warn(`Colour sensor calibration function called with ${cal.length} parameters, expected 4`);
}
const [c, r, g, b] = cal;
this._dataExtractor._formatDef.fields[0].postMult = c;
this._dataExtractor._formatDef.fields[1].postMult = r;
this._dataExtractor._formatDef.fields[2].postMult = g;
this._dataExtractor._formatDef.fields[3].postMult = b;
ricjs_1.RICLog.debug(`Setting colour Sensor ${this._name} calibration: ${c}, ${r}, ${g}, ${b}`);
this._dataExtractor.preCalcs();
}
getCalibration() {
const c = this._dataExtractor._formatDef.fields[0].postMult ? this._dataExtractor._formatDef.fields[0].postMult : 1.0;
const r = this._dataExtractor._formatDef.fields[1].postMult ? this._dataExtractor._formatDef.fields[1].postMult : 1.0;
const g = this._dataExtractor._formatDef.fields[2].postMult ? this._dataExtractor._formatDef.fields[2].postMult : 1.0;
const b = this._dataExtractor._formatDef.fields[3].postMult ? this._dataExtractor._formatDef.fields[3].postMult : 1.0;
ricjs_1.RICLog.debug(`Colour Sensor calibration: ${c}, ${r}, ${g}, ${b}`);
return [c, r, g, b];
}
processInit(dataReceived) {
ricjs_1.RICLog.debug(`Addon ${this._name} init data received ${JSON.stringify(dataReceived)}`);
if (dataReceived.elemName != this._name) {
ricjs_1.RICLog.warn(`Addon init Rx received for ${this._name} but addon name wrong: ${JSON.stringify(dataReceived)}`);
}
if (dataReceived.hexRd) {
const cal = this.getCalibration();
for (let i = 0; i < 4; i++) {
const calRcv = parseInt(dataReceived.hexRd.substr(4 * i, 4), 16);
if (calRcv > 0) {
cal[i] = 255 / calRcv;
}
}
this.setCalibration(cal);
}
}
}
class RICAddOnDistanceSensor extends ricjs_1.RICAddOnBase {
constructor(name, typeName, whoAmI, whoAmITypeCode) {
super(name, typeName, whoAmI, whoAmITypeCode);
this._dataExtractor = new ricjs_1.RICDataExtractor(name, ADDON_DISTANCESENSOR_FORMAT_DEF);
}
processPublishedData(addOnID, statusByte, rawData) {
// Status to return
const retStatus = new ricjs_1.ROSSerialAddOnStatus();
// Extract data
retStatus.id = addOnID;
retStatus.name = this._name;
retStatus.whoAmI = this._whoAmI;
retStatus.status = statusByte;
retStatus.vals = this._dataExtractor.extractData(rawData);
return retStatus;
}
processInit(_dataReceived) {
}
}
class RICAddOnLightSensor extends ricjs_1.RICAddOnBase {
constructor(name, typeName, whoAmI, whoAmITypeCode) {
super(name, typeName, whoAmI, whoAmITypeCode);
this._dataExtractor = new ricjs_1.RICDataExtractor(name, ADDON_LIGHTSENSOR_FORMAT_DEF);
}
processPublishedData(addOnID, statusByte, rawData) {
// Status to return
const retStatus = new ricjs_1.ROSSerialAddOnStatus();
// Extract data
retStatus.id = addOnID;
retStatus.name = this._name;
retStatus.whoAmI = this._whoAmI;
retStatus.status = statusByte;
retStatus.vals = this._dataExtractor.extractData(rawData);
return retStatus;
}
processInit(_dataReceived) {
}
}
class RICAddOnNoiseSensor extends ricjs_1.RICAddOnBase {
constructor(name, typeName, whoAmI, whoAmITypeCode) {
super(name, typeName, whoAmI, whoAmITypeCode);
this._dataExtractor = new ricjs_1.RICDataExtractor(name, ADDON_NOISESENSOR_FORMAT_DEF);
}
processPublishedData(addOnID, statusByte, rawData) {
// Status to return
const retStatus = new ricjs_1.ROSSerialAddOnStatus();
// Extract data
retStatus.id = addOnID;
retStatus.name = this._name;
retStatus.whoAmI = this._whoAmI;
retStatus.status = statusByte;
retStatus.vals = this._dataExtractor.extractData(rawData);
return retStatus;
}
processInit(_dataReceived) {
}
}
//# sourceMappingURL=RICRoboticalAddOns.js.map