diff --git a/src/core/types/index.ts b/src/core/types/index.ts new file mode 100644 index 000000000..cdd0618fa --- /dev/null +++ b/src/core/types/index.ts @@ -0,0 +1,3 @@ +export type PartialNullable = { + [P in keyof T]?: T[P] | null; +}; \ No newline at end of file diff --git a/src/math/Pose.js b/src/math/Pose.ts similarity index 55% rename from src/math/Pose.js rename to src/math/Pose.ts index adb84eaec..a412f9227 100644 --- a/src/math/Pose.js +++ b/src/math/Pose.ts @@ -2,67 +2,78 @@ * @fileOverview * @author David Gossow - dgossow@willowgarage.com */ +import Vector3, { type IVector3 } from './Vector3.js'; +import Quaternion, { type IQuaternion } from './Quaternion.js'; +import { type ITransform } from './Transform.js'; +import type { PartialNullable } from '../core/types/index.js'; -import Vector3 from './Vector3.js'; -import Quaternion from './Quaternion.js'; -import Transform from './Transform.js'; +export interface IPose { + /** + * The ROSLIB.Vector3 describing the position. + */ + position: IVector3; + /** + * The ROSLIB.Quaternion describing the orientation. + */ + orientation: IQuaternion; +} /** * A Pose in 3D space. Values are copied into this object. */ -export default class Pose { - /** - * @param {Object} [options] - * @param {Vector3} [options.position] - The ROSLIB.Vector3 describing the position. - * @param {Quaternion} [options.orientation] - The ROSLIB.Quaternion describing the orientation. - */ - constructor(options) { - options = options || {}; - // copy the values into this object if they exist - options = options || {}; - this.position = new Vector3(options.position); - this.orientation = new Quaternion(options.orientation); +export default class Pose implements IPose { + + position: Vector3; + orientation: Quaternion; + + constructor(options?: PartialNullable) { + this.position = new Vector3(options?.position); + this.orientation = new Quaternion(options?.orientation); } + /** * Apply a transform against this pose. * * @param {Transform} tf - The transform to be applied. */ - applyTransform(tf) { + applyTransform(tf: ITransform) { this.position.multiplyQuaternion(tf.rotation); this.position.add(tf.translation); - var tmp = tf.rotation.clone(); + const tmp = new Quaternion(tf.rotation); tmp.multiply(this.orientation); this.orientation = tmp; } + /** * Clone a copy of this pose. * * @returns {Pose} The cloned pose. */ - clone() { + clone(): Pose { return new Pose(this); } + /** * Multiply this pose with another pose without altering this pose. * * @returns {Pose} The result of the multiplication. */ - multiply(pose) { - var p = pose.clone(); + multiply(pose: Pose): Pose { + const p = pose.clone(); p.applyTransform({ rotation: this.orientation, translation: this.position }); return p; } + /** * Compute the inverse of this pose. * * @returns {Pose} The inverse of the pose. */ - getInverse() { - var inverse = this.clone(); + getInverse(): Pose { + const inverse = this.clone(); inverse.orientation.invert(); inverse.position.multiplyQuaternion(inverse.orientation); inverse.position.x *= -1; diff --git a/src/math/Quaternion.js b/src/math/Quaternion.ts similarity index 54% rename from src/math/Quaternion.js rename to src/math/Quaternion.ts index c43919dfb..d59a58122 100644 --- a/src/math/Quaternion.js +++ b/src/math/Quaternion.ts @@ -2,46 +2,66 @@ * @fileOverview * @author David Gossow - dgossow@willowgarage.com */ +import type { PartialNullable } from '../core/types/index.js'; + +export interface IQuaternion { + /** + * The x value. + */ + x: number; + /** + * The y value. + */ + y: number; + /** + * The z value. + */ + z: number; + /** + * The w value. + */ + w: number; +} /** * A Quaternion. */ -export default class Quaternion { - /** - * @param {Object} [options] - * @param {number|null} [options.x=0] - The x value. - * @param {number|null} [options.y=0] - The y value. - * @param {number|null} [options.z=0] - The z value. - * @param {number|null} [options.w=1] - The w value. - */ - constructor(options) { - options = options || {}; - this.x = options.x || 0; - this.y = options.y || 0; - this.z = options.z || 0; - this.w = typeof options.w === 'number' ? options.w : 1; +export default class Quaternion implements IQuaternion { + x: number; + y: number; + z: number; + w: number; + + constructor(options?: PartialNullable | null) { + this.x = options?.x ?? 0; + this.y = options?.y ?? 0; + this.z = options?.z ?? 0; + this.w = typeof options?.w === 'number' ? options.w : 1; } + /** * Perform a conjugation on this quaternion. */ - conjugate() { + conjugate(): void { this.x *= -1; this.y *= -1; this.z *= -1; } + /** * Return the norm of this quaternion. */ - norm() { + norm(): number { return Math.sqrt( this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w ); } + /** * Perform a normalization on this quaternion. */ - normalize() { - var l = Math.sqrt( + normalize(): void { + let l = Math.sqrt( this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w ); if (l === 0) { @@ -57,34 +77,37 @@ export default class Quaternion { this.w = this.w * l; } } + /** * Convert this quaternion into its inverse. */ - invert() { + invert(): void { this.conjugate(); this.normalize(); } + /** * Set the values of this quaternion to the product of itself and the given quaternion. * * @param {Quaternion} q - The quaternion to multiply with. */ - multiply(q) { - var newX = this.x * q.w + this.y * q.z - this.z * q.y + this.w * q.x; - var newY = -this.x * q.z + this.y * q.w + this.z * q.x + this.w * q.y; - var newZ = this.x * q.y - this.y * q.x + this.z * q.w + this.w * q.z; - var newW = -this.x * q.x - this.y * q.y - this.z * q.z + this.w * q.w; + multiply(q: IQuaternion): void { + const newX = this.x * q.w + this.y * q.z - this.z * q.y + this.w * q.x; + const newY = -this.x * q.z + this.y * q.w + this.z * q.x + this.w * q.y; + const newZ = this.x * q.y - this.y * q.x + this.z * q.w + this.w * q.z; + const newW = -this.x * q.x - this.y * q.y - this.z * q.z + this.w * q.w; this.x = newX; this.y = newY; this.z = newZ; this.w = newW; } + /** * Clone a copy of this quaternion. * * @returns {Quaternion} The cloned quaternion. */ - clone() { + clone(): Quaternion { return new Quaternion(this); } } diff --git a/src/math/Transform.js b/src/math/Transform.js deleted file mode 100644 index 4139f63aa..000000000 --- a/src/math/Transform.js +++ /dev/null @@ -1,31 +0,0 @@ -/** - * @fileOverview - * @author David Gossow - dgossow@willowgarage.com - */ - -import Vector3 from './Vector3.js'; -import Quaternion from './Quaternion.js'; - -/** - * A Transform in 3-space. Values are copied into this object. - */ -export default class Transform { - /** - * @param {Object} options - * @param {Vector3} options.translation - The ROSLIB.Vector3 describing the translation. - * @param {Quaternion} options.rotation - The ROSLIB.Quaternion describing the rotation. - */ - constructor(options) { - // Copy the values into this object if they exist - this.translation = new Vector3(options.translation); - this.rotation = new Quaternion(options.rotation); - } - /** - * Clone a copy of this transform. - * - * @returns {Transform} The cloned transform. - */ - clone() { - return new Transform(this); - } -} diff --git a/src/math/Transform.ts b/src/math/Transform.ts new file mode 100644 index 000000000..176423edf --- /dev/null +++ b/src/math/Transform.ts @@ -0,0 +1,42 @@ +/** + * @fileOverview + * @author David Gossow - dgossow@willowgarage.com + */ + +import Vector3, { type IVector3 } from './Vector3.js'; +import Quaternion, { type IQuaternion } from './Quaternion.js'; + +export interface ITransform { + /** + * The ROSLIB.Vector3 describing the translation. + */ + translation: IVector3; + /** + * The ROSLIB.Quaternion describing the rotation. + */ + rotation: IQuaternion; +} + +/** + * A Transform in 3-space. Values are copied into this object. + */ +export default class Transform implements ITransform { + + translation: Vector3; + rotation: Quaternion; + + constructor(options: ITransform) { + // Copy the values into this object if they exist + this.translation = new Vector3(options.translation); + this.rotation = new Quaternion(options.rotation); + } + + /** + * Clone a copy of this transform. + * + * @returns {Transform} The cloned transform. + */ + clone(): Transform { + return new Transform(this); + } +} diff --git a/src/math/Vector3.js b/src/math/Vector3.ts similarity index 53% rename from src/math/Vector3.js rename to src/math/Vector3.ts index 3d9f933e2..9017d390a 100644 --- a/src/math/Vector3.js +++ b/src/math/Vector3.ts @@ -2,65 +2,82 @@ * @fileOverview * @author David Gossow - dgossow@willowgarage.com */ +import { type IQuaternion } from './Quaternion.js'; +import type { PartialNullable } from '../core/types/index.js'; -import Quaternion from './Quaternion.js'; + +export interface IVector3 { + /** + * The x value. + */ + x: number; + /** + * The y value. + */ + y: number; + /** + * The z value. + */ + z: number; +} /** * A 3D vector. */ -export default class Vector3 { - /** - * @param {Object} [options] - * @param {number} [options.x=0] - The x value. - * @param {number} [options.y=0] - The y value. - * @param {number} [options.z=0] - The z value. - */ - constructor(options) { - options = options || {}; - this.x = options.x || 0; - this.y = options.y || 0; - this.z = options.z || 0; +export default class Vector3 implements IVector3 { + x: number; + y: number; + z: number; + + constructor(options?: PartialNullable | null) { + this.x = options?.x ?? 0; + this.y = options?.y ?? 0; + this.z = options?.z ?? 0; } + /** * Set the values of this vector to the sum of itself and the given vector. * * @param {Vector3} v - The vector to add with. */ - add(v) { + add(v: IVector3): void { this.x += v.x; this.y += v.y; this.z += v.z; } + /** * Set the values of this vector to the difference of itself and the given vector. * * @param {Vector3} v - The vector to subtract with. */ - subtract(v) { + subtract(v: IVector3): void { this.x -= v.x; this.y -= v.y; this.z -= v.z; } + /** * Multiply the given Quaternion with this vector. * * @param {Quaternion} q - The quaternion to multiply with. */ - multiplyQuaternion(q) { - var ix = q.w * this.x + q.y * this.z - q.z * this.y; - var iy = q.w * this.y + q.z * this.x - q.x * this.z; - var iz = q.w * this.z + q.x * this.y - q.y * this.x; - var iw = -q.x * this.x - q.y * this.y - q.z * this.z; + multiplyQuaternion(q: IQuaternion) { + const ix = q.w * this.x + q.y * this.z - q.z * this.y; + const iy = q.w * this.y + q.z * this.x - q.x * this.z; + const iz = q.w * this.z + q.x * this.y - q.y * this.x; + const iw = -q.x * this.x - q.y * this.y - q.z * this.z; this.x = ix * q.w + iw * -q.x + iy * -q.z - iz * -q.y; this.y = iy * q.w + iw * -q.y + iz * -q.x - ix * -q.z; this.z = iz * q.w + iw * -q.z + ix * -q.y - iy * -q.x; } + /** * Clone a copy of this vector. * * @returns {Vector3} The cloned vector. */ - clone() { + clone(): Vector3 { return new Vector3(this); } } diff --git a/src/math/index.js b/src/math/index.ts similarity index 100% rename from src/math/index.js rename to src/math/index.ts diff --git a/src/urdf/UrdfBox.js b/src/urdf/UrdfBox.js deleted file mode 100644 index 437c45399..000000000 --- a/src/urdf/UrdfBox.js +++ /dev/null @@ -1,35 +0,0 @@ -/** - * @fileOverview - * @author Benjamin Pitzer - ben.pitzer@gmail.com - * @author Russell Toris - rctoris@wpi.edu - */ - -import Vector3 from '../math/Vector3.js'; -import * as UrdfTypes from './UrdfTypes.js'; - -/** - * A Box element in a URDF. - */ -export default class UrdfBox { - /** @type {Vector3 | null} */ - dimension; - /** - * @param {Object} options - * @param {Element} options.xml - The XML element to parse. - */ - constructor(options) { - this.type = UrdfTypes.URDF_BOX; - - // Parse the xml string - var xyz = options.xml.getAttribute('size')?.split(' '); - if (xyz) { - this.dimension = new Vector3({ - x: parseFloat(xyz[0]), - y: parseFloat(xyz[1]), - z: parseFloat(xyz[2]) - }); - } else { - this.dimension = null; - } - } -} diff --git a/src/urdf/UrdfBox.ts b/src/urdf/UrdfBox.ts new file mode 100644 index 000000000..8be1b3f54 --- /dev/null +++ b/src/urdf/UrdfBox.ts @@ -0,0 +1,32 @@ +/** + * @fileOverview + * @author Benjamin Pitzer - ben.pitzer@gmail.com + * @author Russell Toris - rctoris@wpi.edu + */ + +import { Vector3 } from '../math/index.js'; +import { UrdfAttrs, UrdfDefaultOptions, UrdfType } from './UrdfTypes.js'; + +/** + * A Box element in a URDF. + */ +export default class UrdfBox { + type: UrdfType; + dimension: Vector3 | null = null; + + constructor({xml}: UrdfDefaultOptions) { + this.type = UrdfType.BOX; + + // Parse the xml string + const size: string[] | undefined = xml.getAttribute(UrdfAttrs.Size)?.split(' '); + if (!size || size.length !== 3) { + return; + } + + this.dimension = new Vector3({ + x: parseFloat(size[0]), + y: parseFloat(size[1]), + z: parseFloat(size[2]) + }); + } +} diff --git a/src/urdf/UrdfColor.js b/src/urdf/UrdfColor.js deleted file mode 100644 index be5ec2821..000000000 --- a/src/urdf/UrdfColor.js +++ /dev/null @@ -1,25 +0,0 @@ -/** - * @fileOverview - * @author Benjamin Pitzer - ben.pitzer@gmail.com - * @author Russell Toris - rctoris@wpi.edu - */ - -/** - * A Color element in a URDF. - */ -export default class UrdfColor { - /** - * @param {Object} options - * @param {Element} options.xml - The XML element to parse. - */ - constructor(options) { - // Parse the xml string - var rgba = options.xml.getAttribute('rgba')?.split(' '); - if (rgba) { - this.r = parseFloat(rgba[0]); - this.g = parseFloat(rgba[1]); - this.b = parseFloat(rgba[2]); - this.a = parseFloat(rgba[3]); - } - } -} diff --git a/src/urdf/UrdfColor.ts b/src/urdf/UrdfColor.ts new file mode 100644 index 000000000..99edd2396 --- /dev/null +++ b/src/urdf/UrdfColor.ts @@ -0,0 +1,43 @@ +/** + * @fileOverview + * @author Benjamin Pitzer - ben.pitzer@gmail.com + * @author Russell Toris - rctoris@wpi.edu + */ + +import { UrdfAttrs, UrdfDefaultOptions } from './UrdfTypes.js'; + +/** + * A Color element in a URDF. + */ +export default class UrdfColor { + + /** + * Color Red, [0, 1] + */ + r: number = 0.0; + /** + * Color Green, [0, 1] + */ + g: number = 0.0; + /** + * Color Blue, [0, 1] + */ + b: number = 0.0; + /** + * Alpha/Opacity, [0, 1] + */ + a: number = 1.0; + + constructor({xml}: UrdfDefaultOptions) { + // Parse the xml string + const rgba: string[] | undefined = xml.getAttribute(UrdfAttrs.Rgba)?.split(' '); + if (!rgba || rgba.length !== 4) { + return; + } + + this.r = parseFloat(rgba[0]); + this.g = parseFloat(rgba[1]); + this.b = parseFloat(rgba[2]); + this.a = parseFloat(rgba[3]); + } +} diff --git a/src/urdf/UrdfCylinder.js b/src/urdf/UrdfCylinder.js deleted file mode 100644 index 84b7a98f3..000000000 --- a/src/urdf/UrdfCylinder.js +++ /dev/null @@ -1,24 +0,0 @@ -/** - * @fileOverview - * @author Benjamin Pitzer - ben.pitzer@gmail.com - * @author Russell Toris - rctoris@wpi.edu - */ - -import * as UrdfTypes from './UrdfTypes.js'; - -/** - * A Cylinder element in a URDF. - */ -export default class UrdfCylinder { - /** - * @param {Object} options - * @param {Element} options.xml - The XML element to parse. - */ - constructor(options) { - this.type = UrdfTypes.URDF_CYLINDER; - // @ts-expect-error -- possibly null - this.length = parseFloat(options.xml.getAttribute('length')); - // @ts-expect-error -- possibly null - this.radius = parseFloat(options.xml.getAttribute('radius')); - } -} diff --git a/src/urdf/UrdfCylinder.ts b/src/urdf/UrdfCylinder.ts new file mode 100644 index 000000000..4e962becc --- /dev/null +++ b/src/urdf/UrdfCylinder.ts @@ -0,0 +1,24 @@ +/** + * @fileOverview + * @author Benjamin Pitzer - ben.pitzer@gmail.com + * @author Russell Toris - rctoris@wpi.edu + */ + +import { UrdfDefaultOptions, UrdfType, UrdfAttrs } from './UrdfTypes.js'; + +/** + * A Cylinder element in a URDF. + */ +export default class UrdfCylinder { + + type: UrdfType; + length: number; + radius: number; + + constructor({xml}: UrdfDefaultOptions) { + this.type = UrdfType.CYLINDER; + + this.length = parseFloat(xml.getAttribute(UrdfAttrs.Length) ?? 'NaN'); + this.radius = parseFloat(xml.getAttribute(UrdfAttrs.Radius) ?? 'NaN'); + } +} diff --git a/src/urdf/UrdfJoint.js b/src/urdf/UrdfJoint.js deleted file mode 100644 index be9ec286b..000000000 --- a/src/urdf/UrdfJoint.js +++ /dev/null @@ -1,95 +0,0 @@ -/** - * @fileOverview - * @author David V. Lu!! - davidvlu@gmail.com - */ - -import Pose from '../math/Pose.js'; -import Vector3 from '../math/Vector3.js'; -import Quaternion from '../math/Quaternion.js'; - -/** - * A Joint element in a URDF. - */ -export default class UrdfJoint { - /** - * @param {Object} options - * @param {Element} options.xml - The XML element to parse. - */ - constructor(options) { - this.name = options.xml.getAttribute('name'); - this.type = options.xml.getAttribute('type'); - - var parents = options.xml.getElementsByTagName('parent'); - if (parents.length > 0) { - this.parent = parents[0].getAttribute('link'); - } - - var children = options.xml.getElementsByTagName('child'); - if (children.length > 0) { - this.child = children[0].getAttribute('link'); - } - - var limits = options.xml.getElementsByTagName('limit'); - if (limits.length > 0) { - this.minval = parseFloat(limits[0].getAttribute('lower') || 'NaN'); - this.maxval = parseFloat(limits[0].getAttribute('upper') || 'NaN'); - } - - // Origin - var origins = options.xml.getElementsByTagName('origin'); - if (origins.length === 0) { - // use the identity as the default - this.origin = new Pose(); - } else { - // Check the XYZ - var xyzValue = origins[0].getAttribute('xyz'); - var position = new Vector3(); - if (xyzValue) { - var xyz = xyzValue.split(' '); - position = new Vector3({ - x: parseFloat(xyz[0]), - y: parseFloat(xyz[1]), - z: parseFloat(xyz[2]) - }); - } - - // Check the RPY - var rpyValue = origins[0].getAttribute('rpy'); - var orientation = new Quaternion(); - if (rpyValue) { - var rpy = rpyValue.split(' '); - // Convert from RPY - var roll = parseFloat(rpy[0]); - var pitch = parseFloat(rpy[1]); - var yaw = parseFloat(rpy[2]); - var phi = roll / 2.0; - var the = pitch / 2.0; - var psi = yaw / 2.0; - var x = - Math.sin(phi) * Math.cos(the) * Math.cos(psi) - - Math.cos(phi) * Math.sin(the) * Math.sin(psi); - var y = - Math.cos(phi) * Math.sin(the) * Math.cos(psi) + - Math.sin(phi) * Math.cos(the) * Math.sin(psi); - var z = - Math.cos(phi) * Math.cos(the) * Math.sin(psi) - - Math.sin(phi) * Math.sin(the) * Math.cos(psi); - var w = - Math.cos(phi) * Math.cos(the) * Math.cos(psi) + - Math.sin(phi) * Math.sin(the) * Math.sin(psi); - - orientation = new Quaternion({ - x: x, - y: y, - z: z, - w: w - }); - orientation.normalize(); - } - this.origin = new Pose({ - position: position, - orientation: orientation - }); - } - } -} diff --git a/src/urdf/UrdfJoint.ts b/src/urdf/UrdfJoint.ts new file mode 100644 index 000000000..75b97f824 --- /dev/null +++ b/src/urdf/UrdfJoint.ts @@ -0,0 +1,50 @@ +/** + * @fileOverview + * @author David V. Lu!! - davidvlu@gmail.com + */ + +import { UrdfAttrs, UrdfDefaultOptions } from './UrdfTypes.js'; +import { Pose } from '../math/index.js'; +import { parseUrdfOrigin } from './UrdfUtils.js'; + +/** + * A Joint element in a URDF. + */ +export default class UrdfJoint { + + name: string; + type: string | null; + parent: string | null = null; + child: string | null = null; + minval: number = NaN; + maxval: number = NaN; + origin: Pose = new Pose(); + + + constructor({xml}: UrdfDefaultOptions) { + this.name = xml.getAttribute(UrdfAttrs.Name) ?? 'unknown_name'; + this.type = xml.getAttribute(UrdfAttrs.Type); + + const parents = xml.getElementsByTagName(UrdfAttrs.Parent); + if (parents.length > 0) { + this.parent = parents[0].getAttribute(UrdfAttrs.Link); + } + + const children = xml.getElementsByTagName(UrdfAttrs.Child); + if (children.length > 0) { + this.child = children[0].getAttribute(UrdfAttrs.Link); + } + + const limits = xml.getElementsByTagName(UrdfAttrs.Limit); + if (limits.length > 0) { + this.minval = parseFloat(limits[0].getAttribute(UrdfAttrs.Lower) ?? 'NaN'); + this.maxval = parseFloat(limits[0].getAttribute(UrdfAttrs.Upper) ?? 'NaN'); + } + + // Origin + const origins = xml.getElementsByTagName(UrdfAttrs.Origin); + if (origins.length > 0) { + this.origin = parseUrdfOrigin(origins[0]); + } + } +} diff --git a/src/urdf/UrdfLink.js b/src/urdf/UrdfLink.ts similarity index 50% rename from src/urdf/UrdfLink.js rename to src/urdf/UrdfLink.ts index 8bc1e3a5a..eba01dae8 100644 --- a/src/urdf/UrdfLink.js +++ b/src/urdf/UrdfLink.ts @@ -5,21 +5,21 @@ */ import UrdfVisual from './UrdfVisual.js'; +import { UrdfAttrs, UrdfDefaultOptions } from './UrdfTypes.js'; /** * A Link element in a URDF. */ export default class UrdfLink { - /** - * @param {Object} options - * @param {Element} options.xml - The XML element to parse. - */ - constructor(options) { - this.name = options.xml.getAttribute('name'); - this.visuals = []; - var visuals = options.xml.getElementsByTagName('visual'); - for (var i = 0; i < visuals.length; i++) { + name: string; + visuals: UrdfVisual[] = []; + + constructor({xml}: UrdfDefaultOptions) { + this.name = xml.getAttribute(UrdfAttrs.Name) ?? 'unknown_name'; + const visuals = xml.getElementsByTagName(UrdfAttrs.Visuals); + + for (let i = 0; i < visuals.length; i++) { this.visuals.push( new UrdfVisual({ xml: visuals[i] diff --git a/src/urdf/UrdfMaterial.js b/src/urdf/UrdfMaterial.ts similarity index 52% rename from src/urdf/UrdfMaterial.js rename to src/urdf/UrdfMaterial.ts index c3fbd3142..22a671130 100644 --- a/src/urdf/UrdfMaterial.js +++ b/src/urdf/UrdfMaterial.ts @@ -5,31 +5,29 @@ */ import UrdfColor from './UrdfColor.js'; +import { UrdfAttrs, UrdfDefaultOptions } from './UrdfTypes.js'; /** * A Material element in a URDF. */ export default class UrdfMaterial { - /** @type {string | null} */ - textureFilename = null; - /** @type {UrdfColor | null} */ - color = null; - /** - * @param {Object} options - * @param {Element} options.xml - The XML element to parse. - */ - constructor(options) { - - this.name = options.xml.getAttribute('name'); + + name: string; + textureFilename: string | null = null; + color: UrdfColor | null = null; + + constructor({xml}: UrdfDefaultOptions) { + + this.name = xml.getAttribute(UrdfAttrs.Name) ?? 'unknown_name'; // Texture - var textures = options.xml.getElementsByTagName('texture'); + const textures = xml.getElementsByTagName(UrdfAttrs.Texture); if (textures.length > 0) { - this.textureFilename = textures[0].getAttribute('filename'); + this.textureFilename = textures[0].getAttribute(UrdfAttrs.Filename); } // Color - var colors = options.xml.getElementsByTagName('color'); + const colors = xml.getElementsByTagName(UrdfAttrs.Color); if (colors.length > 0) { // Parse the RBGA string this.color = new UrdfColor({ @@ -37,10 +35,12 @@ export default class UrdfMaterial { }); } } + isLink() { return this.color === null && this.textureFilename === null; } - assign(obj) { + + assign(obj: UrdfMaterial): this & UrdfMaterial { return Object.assign(this, obj); } } diff --git a/src/urdf/UrdfMesh.js b/src/urdf/UrdfMesh.js deleted file mode 100644 index d4429ec66..000000000 --- a/src/urdf/UrdfMesh.js +++ /dev/null @@ -1,36 +0,0 @@ -/** - * @fileOverview - * @author Benjamin Pitzer - ben.pitzer@gmail.com - * @author Russell Toris - rctoris@wpi.edu - */ - -import Vector3 from '../math/Vector3.js'; -import * as UrdfTypes from './UrdfTypes.js'; - -/** - * A Mesh element in a URDF. - */ -export default class UrdfMesh { - /** @type {Vector3 | null} */ - scale = null; - /** - * @param {Object} options - * @param {Element} options.xml - The XML element to parse. - */ - constructor(options) { - this.type = UrdfTypes.URDF_MESH; - this.filename = options.xml.getAttribute('filename'); - - // Check for a scale - var scale = options.xml.getAttribute('scale'); - if (scale) { - // Get the XYZ - var xyz = scale.split(' '); - this.scale = new Vector3({ - x: parseFloat(xyz[0]), - y: parseFloat(xyz[1]), - z: parseFloat(xyz[2]) - }); - } - } -} diff --git a/src/urdf/UrdfMesh.ts b/src/urdf/UrdfMesh.ts new file mode 100644 index 000000000..5bbedc507 --- /dev/null +++ b/src/urdf/UrdfMesh.ts @@ -0,0 +1,38 @@ +/** + * @fileOverview + * @author Benjamin Pitzer - ben.pitzer@gmail.com + * @author Russell Toris - rctoris@wpi.edu + */ + +import { Vector3 } from '../math/index.js'; +import { UrdfAttrs, UrdfDefaultOptions, UrdfType } from './UrdfTypes.js'; + +/** + * A Mesh element in a URDF. + */ +export default class UrdfMesh { + type: UrdfType; + scale: Vector3 | null = null; + filename: string | null; + + /** + * @param {Object} options + * @param {Element} options.xml - The XML element to parse. + */ + constructor({xml}: UrdfDefaultOptions) { + this.type = UrdfType.MESH; + this.filename = xml.getAttribute('filename'); + + // Check for a scale + const scale: string[] | undefined = xml.getAttribute(UrdfAttrs.Scale)?.split(' '); + if (!scale || scale.length !== 3) { + return; + } + + this.scale = new Vector3({ + x: parseFloat(scale[0]), + y: parseFloat(scale[1]), + z: parseFloat(scale[2]) + }); + } +} diff --git a/src/urdf/UrdfModel.js b/src/urdf/UrdfModel.js deleted file mode 100644 index 3c432d8c6..000000000 --- a/src/urdf/UrdfModel.js +++ /dev/null @@ -1,98 +0,0 @@ -/** - * @fileOverview - * @author Benjamin Pitzer - ben.pitzer@gmail.com - * @author Russell Toris - rctoris@wpi.edu - */ - -import UrdfMaterial from './UrdfMaterial.js'; -import UrdfLink from './UrdfLink.js'; -import UrdfJoint from './UrdfJoint.js'; -import { DOMParser, MIME_TYPE } from '@xmldom/xmldom'; - -// See https://developer.mozilla.org/docs/XPathResult#Constants -var XPATH_FIRST_ORDERED_NODE_TYPE = 9; - -/** - * A URDF Model can be used to parse a given URDF into the appropriate elements. - */ -export default class UrdfModel { - materials = {}; - links = {}; - joints = {}; - /** - * @param {Object} options - * @param {Element | null} [options.xml] - The XML element to parse. - * @param {string} [options.string] - The XML element to parse as a string. - */ - constructor(options) { - var xmlDoc = options.xml; - var string = options.string; - - // Check if we are using a string or an XML element - if (string) { - // Parse the string - var parser = new DOMParser(); - xmlDoc = parser.parseFromString(string, MIME_TYPE.XML_TEXT).documentElement; - } - if (!xmlDoc) { - throw new Error('No URDF document parsed!'); - } - - // Initialize the model with the given XML node. - // Get the robot tag - var robotXml = xmlDoc; - - // Get the robot name - this.name = robotXml.getAttribute('name'); - - // Parse all the visual elements we need - for (var nodes = robotXml.childNodes, i = 0; i < nodes.length; i++) { - /** @type {Element} */ - // @ts-expect-error -- unknown why this doesn't work properly. - var node = nodes[i]; - if (node.tagName === 'material') { - var material = new UrdfMaterial({ - xml: node - }); - // Make sure this is unique - if (this.materials[material.name] !== void 0) { - if (this.materials[material.name].isLink()) { - this.materials[material.name].assign(material); - } else { - console.warn('Material ' + material.name + 'is not unique.'); - } - } else { - this.materials[material.name] = material; - } - } else if (node.tagName === 'link') { - var link = new UrdfLink({ - xml: node - }); - // Make sure this is unique - if (this.links[link.name] !== void 0) { - console.warn('Link ' + link.name + ' is not unique.'); - } else { - // Check for a material - for (var j = 0; j < link.visuals.length; j++) { - var mat = link.visuals[j].material; - if (mat !== null && mat.name) { - if (this.materials[mat.name] !== void 0) { - link.visuals[j].material = this.materials[mat.name]; - } else { - this.materials[mat.name] = mat; - } - } - } - - // Add the link - this.links[link.name] = link; - } - } else if (node.tagName === 'joint') { - var joint = new UrdfJoint({ - xml: node - }); - this.joints[joint.name] = joint; - } - } - } -} diff --git a/src/urdf/UrdfModel.ts b/src/urdf/UrdfModel.ts new file mode 100644 index 000000000..a63bd2d73 --- /dev/null +++ b/src/urdf/UrdfModel.ts @@ -0,0 +1,117 @@ +/** + * @fileOverview + * @author Benjamin Pitzer - ben.pitzer@gmail.com + * @author Russell Toris - rctoris@wpi.edu + */ + +import { DOMParser, MIME_TYPE } from '@xmldom/xmldom'; +import UrdfMaterial from './UrdfMaterial.js'; +import UrdfLink from './UrdfLink.js'; +import UrdfJoint from './UrdfJoint.js'; +import { isElement } from './UrdfUtils.js'; + +// See https://developer.mozilla.org/docs/XPathResult#Constants +// const XPATH_FIRST_ORDERED_NODE_TYPE = 9; + +type KeyedObject = { [key: string]: T }; + +export interface UrdfModelOptions { + /** + * The XML element to parse. + */ + xml?: Element; + /** + * The XML element to parse as a string. + */ + string: string; +} + +/** + * A URDF Model can be used to parse a given URDF into the appropriate elements. + */ +export default class UrdfModel { + + name: string | null; + materials: KeyedObject = {}; + links: KeyedObject = {}; + joints: KeyedObject = {}; + + constructor({xml, string}: UrdfModelOptions) { + let xmlDoc = xml; + + // Check if we are using a string or an XML element + if (string) { + // Parse the string + xmlDoc = new DOMParser().parseFromString(string, MIME_TYPE.XML_TEXT).documentElement; + } + + if (!xmlDoc) { + throw new Error('No URDF document parsed!'); + } + + // Get the robot name + this.name = xmlDoc.getAttribute('name'); + + const childNodes = xmlDoc.childNodes; + // Parse all the visual elements we need + for (let i = 0; i < childNodes.length; i++) { + const node = childNodes[i]; + + // Safety check to make sure we're working with an element. + if (!isElement(node)) { + continue; + } + + switch (node.tagName) { + case 'material': { + const material = new UrdfMaterial({xml: node}); + // Make sure this is unique + if (!Object.hasOwn(this.materials, material.name)) { + this.materials[material.name] = material; + break; + } + + if (this.materials[material.name].isLink()) { + this.materials[material.name].assign(material); + } else { + console.warn(`Material ${ material.name } is not unique.`); + } + + break; + } + case 'link': { + const link = new UrdfLink({xml: node}); + // Make sure this is unique + if (Object.hasOwn(this.links, link.name)) { + console.warn(`Link ${ link.name } is not unique.`); + break; + } + + // Check for a material + for (let j = 0; j < link.visuals.length; j++) { + const mat = link.visuals[j].material; + if (mat === null || !mat.name) { + continue; + } + + if (Object.hasOwn(this.materials, mat.name)) { + link.visuals[j].material = this.materials[mat.name]; + } else { + this.materials[mat.name] = mat; + } + } + + // Add the link + this.links[link.name] = link; + + break; + } + case 'joint': { + const joint = new UrdfJoint({xml: node}); + this.joints[joint.name] = joint; + break; + } + } + } + } +} diff --git a/src/urdf/UrdfSphere.js b/src/urdf/UrdfSphere.js deleted file mode 100644 index 13c585fba..000000000 --- a/src/urdf/UrdfSphere.js +++ /dev/null @@ -1,21 +0,0 @@ -/** - * @fileOverview - * @author Benjamin Pitzer - ben.pitzer@gmail.com - * @author Russell Toris - rctoris@wpi.edu - */ - -import * as UrdfTypes from './UrdfTypes.js'; - -/** - * A Sphere element in a URDF. - */ -export default class UrdfSphere { - /** - * @param {Object} options - * @param {Element} options.xml - The XML element to parse. - */ - constructor(options) { - this.type = UrdfTypes.URDF_SPHERE; - this.radius = parseFloat(options.xml.getAttribute('radius') || 'NaN'); - } -} diff --git a/src/urdf/UrdfSphere.ts b/src/urdf/UrdfSphere.ts new file mode 100644 index 000000000..753384e4d --- /dev/null +++ b/src/urdf/UrdfSphere.ts @@ -0,0 +1,21 @@ +/** + * @fileOverview + * @author Benjamin Pitzer - ben.pitzer@gmail.com + * @author Russell Toris - rctoris@wpi.edu + */ + +import { UrdfDefaultOptions, UrdfType } from './UrdfTypes.js'; + +/** + * A Sphere element in a URDF. + */ +export default class UrdfSphere { + + type: UrdfType; + radius: number = NaN; + + constructor({xml}: UrdfDefaultOptions) { + this.type = UrdfType.SPHERE; + this.radius = parseFloat(xml.getAttribute('radius') ?? 'NaN'); + } +} diff --git a/src/urdf/UrdfTypes.js b/src/urdf/UrdfTypes.js deleted file mode 100644 index 972d8283a..000000000 --- a/src/urdf/UrdfTypes.js +++ /dev/null @@ -1,4 +0,0 @@ -export const URDF_SPHERE = 0; -export const URDF_BOX = 1; -export const URDF_CYLINDER = 2; -export const URDF_MESH = 3; diff --git a/src/urdf/UrdfTypes.ts b/src/urdf/UrdfTypes.ts new file mode 100644 index 000000000..2a66851bc --- /dev/null +++ b/src/urdf/UrdfTypes.ts @@ -0,0 +1,38 @@ +export enum UrdfType { + SPHERE = 0, + BOX = 1, + CYLINDER = 2, + MESH = 3 +} + +export enum UrdfAttrs { + Name = 'name', + Type = 'type', + Parent = 'parent', + Link = 'link', + Child = 'child', + Limit = 'limit', + Upper = 'upper', + Lower = 'lower', + Origin = 'origin', + Xyz = 'xyz', + Rpy = 'rpy', + Size = 'size', + Rgba = 'rgba', + Length = 'length', + Radius = 'radius', + Visuals = 'visual', + Texture = 'texture', + Filename = 'filename', + Color = 'color', + Geometry = 'geometry', + Material = 'material', + Scale = 'scale', +} + +export interface UrdfDefaultOptions { + /** + * The XML element to parse. + */ + xml: Element; +} \ No newline at end of file diff --git a/src/urdf/UrdfUtils.ts b/src/urdf/UrdfUtils.ts new file mode 100644 index 000000000..6dd72b73a --- /dev/null +++ b/src/urdf/UrdfUtils.ts @@ -0,0 +1,59 @@ +/********** Utility Methods for parsing Joint **********/ +import { Pose, Quaternion, Vector3 } from '../math/index.js'; +import { UrdfAttrs } from './UrdfTypes.js'; + +export function parseUrdfOrigin(originElement: Element): Pose { + // Check the XYZ + const xyz: string[] | undefined = originElement.getAttribute(UrdfAttrs.Xyz)?.split(' '); + let position: Vector3 = new Vector3(); + if (xyz && xyz.length === 3) { + position = new Vector3({ + x: parseFloat(xyz[0]), + y: parseFloat(xyz[1]), + z: parseFloat(xyz[2]) + }); + } + + // Check the RPY + const rpy = originElement.getAttribute(UrdfAttrs.Rpy)?.split(' '); + let orientation = new Quaternion(); + if (rpy && rpy.length === 3) { + // Convert from RPY + const roll = parseFloat(rpy[0]); + const pitch = parseFloat(rpy[1]); + const yaw = parseFloat(rpy[2]); + const phi = roll / 2.0; + const the = pitch / 2.0; + const psi = yaw / 2.0; + const x = + Math.sin(phi) * Math.cos(the) * Math.cos(psi) - + Math.cos(phi) * Math.sin(the) * Math.sin(psi); + const y = + Math.cos(phi) * Math.sin(the) * Math.cos(psi) + + Math.sin(phi) * Math.cos(the) * Math.sin(psi); + const z = + Math.cos(phi) * Math.cos(the) * Math.sin(psi) - + Math.sin(phi) * Math.sin(the) * Math.cos(psi); + const w = + Math.cos(phi) * Math.cos(the) * Math.cos(psi) + + Math.sin(phi) * Math.sin(the) * Math.sin(psi); + + orientation = new Quaternion({ + x: x, + y: y, + z: z, + w: w + }); + orientation.normalize(); + } + + return new Pose({ + position: position, + orientation: orientation + }); +} + +export function isElement(node: Node): node is Element { + // Node.ELEMENT_TYPE = 1 + return node.nodeType === 1; +} \ No newline at end of file diff --git a/src/urdf/UrdfVisual.js b/src/urdf/UrdfVisual.js deleted file mode 100644 index 2832a72fb..000000000 --- a/src/urdf/UrdfVisual.js +++ /dev/null @@ -1,140 +0,0 @@ -/** - * @fileOverview - * @author Benjamin Pitzer - ben.pitzer@gmail.com - * @author Russell Toris - rctoris@wpi.edu - */ - -import Pose from '../math/Pose.js'; -import Vector3 from '../math/Vector3.js'; -import Quaternion from '../math/Quaternion.js'; - -import UrdfCylinder from './UrdfCylinder.js'; -import UrdfBox from './UrdfBox.js'; -import UrdfMaterial from './UrdfMaterial.js'; -import UrdfMesh from './UrdfMesh.js'; -import UrdfSphere from './UrdfSphere.js'; - -/** - * A Visual element in a URDF. - */ -export default class UrdfVisual { - /** @type {Pose | null} */ - origin = null; - /** @type {UrdfMesh | UrdfSphere | UrdfBox | UrdfCylinder | null} */ - geometry = null; - /** @type {UrdfMaterial | null} */ - material = null; - /** - * @param {Object} options - * @param {Element} options.xml - The XML element to parse. - */ - constructor(options) { - var xml = options.xml; - this.name = options.xml.getAttribute('name'); - - // Origin - var origins = xml.getElementsByTagName('origin'); - if (origins.length === 0) { - // use the identity as the default - this.origin = new Pose(); - } else { - // Check the XYZ - var xyzValue = origins[0].getAttribute('xyz'); - var position = new Vector3(); - if (xyzValue) { - var xyz = xyzValue.split(' '); - position = new Vector3({ - x: parseFloat(xyz[0]), - y: parseFloat(xyz[1]), - z: parseFloat(xyz[2]) - }); - } - - // Check the RPY - var rpyValue = origins[0].getAttribute('rpy'); - var orientation = new Quaternion(); - if (rpyValue) { - var rpy = rpyValue.split(' '); - // Convert from RPY - var roll = parseFloat(rpy[0]); - var pitch = parseFloat(rpy[1]); - var yaw = parseFloat(rpy[2]); - var phi = roll / 2.0; - var the = pitch / 2.0; - var psi = yaw / 2.0; - var x = - Math.sin(phi) * Math.cos(the) * Math.cos(psi) - - Math.cos(phi) * Math.sin(the) * Math.sin(psi); - var y = - Math.cos(phi) * Math.sin(the) * Math.cos(psi) + - Math.sin(phi) * Math.cos(the) * Math.sin(psi); - var z = - Math.cos(phi) * Math.cos(the) * Math.sin(psi) - - Math.sin(phi) * Math.sin(the) * Math.cos(psi); - var w = - Math.cos(phi) * Math.cos(the) * Math.cos(psi) + - Math.sin(phi) * Math.sin(the) * Math.sin(psi); - - orientation = new Quaternion({ - x: x, - y: y, - z: z, - w: w - }); - orientation.normalize(); - } - this.origin = new Pose({ - position: position, - orientation: orientation - }); - } - - // Geometry - var geoms = xml.getElementsByTagName('geometry'); - if (geoms.length > 0) { - var geom = geoms[0]; - var shape = null; - // Check for the shape - for (var i = 0; i < geom.childNodes.length; i++) { - /** @type {Element} */ - // @ts-expect-error -- unknown why this doesn't work properly. - var node = geom.childNodes[i]; - if (node.nodeType === 1) { - shape = node; - break; - } - } - if (shape) { - // Check the type - var type = shape.nodeName; - if (type === 'sphere') { - this.geometry = new UrdfSphere({ - xml: shape - }); - } else if (type === 'box') { - this.geometry = new UrdfBox({ - xml: shape - }); - } else if (type === 'cylinder') { - this.geometry = new UrdfCylinder({ - xml: shape - }); - } else if (type === 'mesh') { - this.geometry = new UrdfMesh({ - xml: shape - }); - } else { - console.warn('Unknown geometry type ' + type); - } - } - } - - // Material - var materials = xml.getElementsByTagName('material'); - if (materials.length > 0) { - this.material = new UrdfMaterial({ - xml: materials[0] - }); - } - } -} diff --git a/src/urdf/UrdfVisual.ts b/src/urdf/UrdfVisual.ts new file mode 100644 index 000000000..499198862 --- /dev/null +++ b/src/urdf/UrdfVisual.ts @@ -0,0 +1,86 @@ +/** + * @fileOverview + * @author Benjamin Pitzer - ben.pitzer@gmail.com + * @author Russell Toris - rctoris@wpi.edu + */ + +import Pose from '../math/Pose.js'; +import UrdfCylinder from './UrdfCylinder.js'; +import UrdfBox from './UrdfBox.js'; +import UrdfMaterial from './UrdfMaterial.js'; +import UrdfMesh from './UrdfMesh.js'; +import UrdfSphere from './UrdfSphere.js'; +import { UrdfAttrs, UrdfDefaultOptions } from './UrdfTypes.js'; +import { isElement, parseUrdfOrigin } from './UrdfUtils.js'; + +export type UrdfGeometryLike = UrdfMesh | UrdfSphere | UrdfBox | UrdfCylinder; + +function parseUrdfGeometry(geometryElem: Element): UrdfGeometryLike | null { + + let childShape: Element | null = null; + for (let i = 0; i < geometryElem.childNodes.length; i++) { + + const node = geometryElem.childNodes[i]; + if (isElement(node)) { + // Safe type check after checking nodeType + childShape = node; + break; + } + } + + if (!childShape) { + return null; + } + + const options: UrdfDefaultOptions = { + xml: childShape + } + + switch (childShape.nodeName) { + case 'sphere': + return new UrdfSphere(options); + case 'box': + return new UrdfBox(options); + case 'cylinder': + return new UrdfCylinder(options); + case 'mesh': + return new UrdfMesh(options); + default: + console.warn(`Unknown geometry type ${ childShape.nodeName }`); + return null + } +} + +/** + * A Visual element in a URDF. + */ +export default class UrdfVisual { + name: string | null; + origin: Pose | null = new Pose(); + geometry: UrdfGeometryLike | null = null; + material: UrdfMaterial | null = null; + + constructor({xml}: UrdfDefaultOptions) { + this.name = xml.getAttribute('name'); + + // Origin + const origins = xml.getElementsByTagName(UrdfAttrs.Origin); + if (origins.length > 0) { + this.origin = parseUrdfOrigin(origins[0]); + } + + // Geometry + const geoms = xml.getElementsByTagName(UrdfAttrs.Geometry); + if (geoms.length > 0) { + this.geometry = parseUrdfGeometry(geoms[0]); + } + + // Material + const materials = xml.getElementsByTagName(UrdfAttrs.Material); + if (materials.length > 0) { + this.material = new UrdfMaterial({ + xml: materials[0] + }); + } + } +} diff --git a/src/urdf/index.js b/src/urdf/index.ts similarity index 85% rename from src/urdf/index.js rename to src/urdf/index.ts index 8fa6d56c1..1ad27f6de 100644 --- a/src/urdf/index.js +++ b/src/urdf/index.ts @@ -6,6 +6,6 @@ export { default as UrdfMaterial } from './UrdfMaterial.js'; export { default as UrdfMesh } from './UrdfMesh.js'; export { default as UrdfModel } from './UrdfModel.js'; export { default as UrdfSphere } from './UrdfSphere.js'; -export { default as UrdfVisual } from './UrdfVisual.js'; +export { default as UrdfVisual, type UrdfGeometryLike } from './UrdfVisual.js'; export * from './UrdfTypes.js'; diff --git a/src/util/cborTypedArrayTags.js b/src/util/cborTypedArrayTags.js deleted file mode 100644 index faec1f65a..000000000 --- a/src/util/cborTypedArrayTags.js +++ /dev/null @@ -1,115 +0,0 @@ -var UPPER32 = Math.pow(2, 32); - -var warnedPrecision = false; -function warnPrecision() { - if (!warnedPrecision) { - warnedPrecision = true; - console.warn( - 'CBOR 64-bit integer array values may lose precision. No further warnings.' - ); - } -} - -/** - * Unpack 64-bit unsigned integer from byte array. - * @param {Uint8Array} bytes - */ -function decodeUint64LE(bytes) { - warnPrecision(); - - var byteLen = bytes.byteLength; - var offset = bytes.byteOffset; - var arrLen = byteLen / 8; - - var buffer = bytes.buffer.slice(offset, offset + byteLen); - var uint32View = new Uint32Array(buffer); - - var arr = new Array(arrLen); - for (var i = 0; i < arrLen; i++) { - var si = i * 2; - var lo = uint32View[si]; - var hi = uint32View[si + 1]; - arr[i] = lo + UPPER32 * hi; - } - - return arr; -} - -/** - * Unpack 64-bit signed integer from byte array. - * @param {Uint8Array} bytes - */ -function decodeInt64LE(bytes) { - warnPrecision(); - - var byteLen = bytes.byteLength; - var offset = bytes.byteOffset; - var arrLen = byteLen / 8; - - var buffer = bytes.buffer.slice(offset, offset + byteLen); - var uint32View = new Uint32Array(buffer); - var int32View = new Int32Array(buffer); - - var arr = new Array(arrLen); - for (var i = 0; i < arrLen; i++) { - var si = i * 2; - var lo = uint32View[si]; - var hi = int32View[si + 1]; - arr[i] = lo + UPPER32 * hi; - } - - return arr; -} - -/** - * Unpack typed array from byte array. - * @param {Uint8Array} bytes - * @param {ArrayConstructor} ArrayType - Desired output array type - */ -function decodeNativeArray(bytes, ArrayType) { - var byteLen = bytes.byteLength; - var offset = bytes.byteOffset; - var buffer = bytes.buffer.slice(offset, offset + byteLen); - return new ArrayType(buffer); -} - -/** - * Supports a subset of draft CBOR typed array tags: - * - * - * Only supports little-endian tags for now. - */ -var nativeArrayTypes = { - 64: Uint8Array, - 69: Uint16Array, - 70: Uint32Array, - 72: Int8Array, - 77: Int16Array, - 78: Int32Array, - 85: Float32Array, - 86: Float64Array -}; - -/** - * We can also decode 64-bit integer arrays, since ROS has these types. - */ -var conversionArrayTypes = { - 71: decodeUint64LE, - 79: decodeInt64LE -}; - -/** - * Handle CBOR typed array tags during decoding. - * @param {Uint8Array} data - * @param {Number} tag - */ -export default function cborTypedArrayTagger(data, tag) { - if (tag in nativeArrayTypes) { - var arrayType = nativeArrayTypes[tag]; - return decodeNativeArray(data, arrayType); - } - if (tag in conversionArrayTypes) { - return conversionArrayTypes[tag](data); - } - return data; -} diff --git a/src/util/cborTypedArrayTags.ts b/src/util/cborTypedArrayTags.ts new file mode 100644 index 000000000..af81769e4 --- /dev/null +++ b/src/util/cborTypedArrayTags.ts @@ -0,0 +1,128 @@ +type TypedArrayConstructor = + | Uint8ArrayConstructor + | Uint16ArrayConstructor + | Uint32ArrayConstructor + | Int8ArrayConstructor + | Int16ArrayConstructor + | Int32ArrayConstructor + | Float32ArrayConstructor + | Float64ArrayConstructor; + +// Type for functions that convert arrays +type ArrayConverter = (bytes: Uint8Array) => number[]; + +const UPPER32 = Math.pow(2, 32); + +let warnedPrecision = false; +function warnPrecision() { + if (!warnedPrecision) { + warnedPrecision = true; + console.warn( + 'CBOR 64-bit integer array values may lose precision. No further warnings.' + ); + } +} + +/** + * Unpack 64-bit unsigned integer from byte array. + * @param {Uint8Array} bytes + */ +function decodeUint64LE(bytes: Uint8Array) { + warnPrecision(); + + const byteLen = bytes.byteLength; + const offset = bytes.byteOffset; + const arrLen = byteLen / 8; + + const buffer = bytes.buffer.slice(offset, offset + byteLen); + const uint32View = new Uint32Array(buffer); + + const arr = new Array(arrLen); + for (let i = 0; i < arrLen; i++) { + const si = i * 2; + const lo = uint32View[si]; + const hi = uint32View[si + 1]; + arr[i] = lo + UPPER32 * hi; + } + + return arr; +} + +/** + * Unpack 64-bit signed integer from byte array. + * @param {Uint8Array} bytes + */ +function decodeInt64LE(bytes: Uint8Array) { + warnPrecision(); + + const byteLen = bytes.byteLength; + const offset = bytes.byteOffset; + const arrLen = byteLen / 8; + + const buffer = bytes.buffer.slice(offset, offset + byteLen); + const uint32View = new Uint32Array(buffer); + const int32View = new Int32Array(buffer); + + const arr = new Array(arrLen); + for (let i = 0; i < arrLen; i++) { + const si = i * 2; + const lo = uint32View[si]; + const hi = int32View[si + 1]; + arr[i] = lo + UPPER32 * hi; + } + + return arr; +} + +/** + * Unpack typed array from byte array. + * @param {Uint8Array} bytes + * @param {ArrayConstructor} ArrayType - Desired output array type + */ +function decodeNativeArray(bytes: Uint8Array, ArrayType: T): T['prototype'] { + const byteLen = bytes.byteLength; + const offset = bytes.byteOffset; + const buffer = bytes.buffer.slice(offset, offset + byteLen); + return new ArrayType(buffer); +} + +/** + * Supports a subset of draft CBOR typed array tags: + * + * + * Only supports little-endian tags for now. + */ +const nativeArrayTypes = { + 64: Uint8Array, + 69: Uint16Array, + 70: Uint32Array, + 72: Int8Array, + 77: Int16Array, + 78: Int32Array, + 85: Float32Array, + 86: Float64Array +} as const; + +/** + * We can also decode 64-bit integer arrays, since ROS has these types. + */ +const conversionArrayTypes: Record = { + 71: decodeUint64LE, + 79: decodeInt64LE +} as const; + +/** + * Handle CBOR typed array tags during decoding. + * @param {Uint8Array} data + * @param tag + */ +export default function cborTypedArrayTagger(data: Uint8Array, tag: keyof typeof nativeArrayTypes) { + if (tag in nativeArrayTypes) { + const arrayType = nativeArrayTypes[tag]; + return decodeNativeArray(data, arrayType); + } + if (tag in conversionArrayTypes) { + return conversionArrayTypes[tag](data); + } + return data; +} diff --git a/test/cbor.test.js b/test/cbor.test.ts similarity index 67% rename from test/cbor.test.js rename to test/cbor.test.ts index 4c2d3824a..ce719881d 100644 --- a/test/cbor.test.js +++ b/test/cbor.test.ts @@ -3,9 +3,9 @@ import CBOR from 'cbor-js'; import cborTypedArrayTagger from '../src/util/cborTypedArrayTags.js'; /** Convert hex string to ArrayBuffer. */ -function hexToBuffer(hex) { - var tokens = hex.match(/[0-9a-fA-F]{2}/gi); - var arr = tokens.map(function(t) { +function hexToBuffer(hex: string) { + const tokens = hex.match(/[0-9a-fA-F]{2}/gi) ?? []; + const arr = tokens.map((t: string) => { return parseInt(t, 16); }); return new Uint8Array(arr).buffer; @@ -15,8 +15,8 @@ function hexToBuffer(hex) { describe('CBOR Typed Array Tagger', function() { it('should convert tagged Uint16Array', function() { - var data = hexToBuffer('d84546010002000300'); - var msg = CBOR.decode(data, cborTypedArrayTagger); + const data = hexToBuffer('d84546010002000300'); + const msg = CBOR.decode(data, cborTypedArrayTagger); expect(msg).to.be.a('Uint16Array'); expect(msg).to.have.lengthOf(3); @@ -26,8 +26,8 @@ describe('CBOR Typed Array Tagger', function() { }); it('should convert tagged Uint32Array', function() { - var data = hexToBuffer('d8464c010000000200000003000000'); - var msg = CBOR.decode(data, cborTypedArrayTagger); + const data = hexToBuffer('d8464c010000000200000003000000'); + const msg = CBOR.decode(data, cborTypedArrayTagger); expect(msg).to.be.a('Uint32Array'); expect(msg).to.have.lengthOf(3); @@ -37,8 +37,8 @@ describe('CBOR Typed Array Tagger', function() { }); it('should convert tagged Uint64Array', function() { - var data = hexToBuffer('d8475818010000000000000002000000000000000300000000000000'); - var msg = CBOR.decode(data, cborTypedArrayTagger); + const data = hexToBuffer('d8475818010000000000000002000000000000000300000000000000'); + const msg = CBOR.decode(data, cborTypedArrayTagger); expect(msg).to.be.a('Array'); expect(msg).to.have.lengthOf(3); @@ -48,8 +48,8 @@ describe('CBOR Typed Array Tagger', function() { }); it('should convert tagged Int8Array', function() { - var data = hexToBuffer('d8484301fe03'); - var msg = CBOR.decode(data, cborTypedArrayTagger); + const data = hexToBuffer('d8484301fe03'); + const msg = CBOR.decode(data, cborTypedArrayTagger); expect(msg).to.be.a('Int8Array'); expect(msg).to.have.lengthOf(3); @@ -59,8 +59,8 @@ describe('CBOR Typed Array Tagger', function() { }); it('should convert tagged Int16Array', function() { - var data = hexToBuffer('d84d460100feff0300'); - var msg = CBOR.decode(data, cborTypedArrayTagger); + const data = hexToBuffer('d84d460100feff0300'); + const msg = CBOR.decode(data, cborTypedArrayTagger); expect(msg).to.be.a('Int16Array'); expect(msg).to.have.lengthOf(3); @@ -70,8 +70,8 @@ describe('CBOR Typed Array Tagger', function() { }); it('should convert tagged Int32Array', function() { - var data = hexToBuffer('d84e4c01000000feffffff03000000'); - var msg = CBOR.decode(data, cborTypedArrayTagger); + const data = hexToBuffer('d84e4c01000000feffffff03000000'); + const msg = CBOR.decode(data, cborTypedArrayTagger); expect(msg).to.be.a('Int32Array'); expect(msg).to.have.lengthOf(3); @@ -81,8 +81,8 @@ describe('CBOR Typed Array Tagger', function() { }); it('should convert tagged Int64Array', function() { - var data = hexToBuffer('d84f58180100000000000000feffffffffffffff0300000000000000'); - var msg = CBOR.decode(data, cborTypedArrayTagger); + const data = hexToBuffer('d84f58180100000000000000feffffffffffffff0300000000000000'); + const msg = CBOR.decode(data, cborTypedArrayTagger); expect(msg).to.be.a('Array'); expect(msg).to.have.lengthOf(3); @@ -92,8 +92,8 @@ describe('CBOR Typed Array Tagger', function() { }); it('should convert tagged Float32Array', function() { - var data = hexToBuffer('d8554ccdcc8c3fcdcc0cc033335340'); - var msg = CBOR.decode(data, cborTypedArrayTagger); + const data = hexToBuffer('d8554ccdcc8c3fcdcc0cc033335340'); + const msg = CBOR.decode(data, cborTypedArrayTagger); expect(msg).to.be.a('Float32Array'); expect(msg).to.have.lengthOf(3); @@ -103,8 +103,8 @@ describe('CBOR Typed Array Tagger', function() { }); it('should convert tagged Float64Array', function() { - var data = hexToBuffer('d85658189a9999999999f13f9a999999999901c06666666666660a40'); - var msg = CBOR.decode(data, cborTypedArrayTagger); + const data = hexToBuffer('d85658189a9999999999f13f9a999999999901c06666666666660a40'); + const msg = CBOR.decode(data, cborTypedArrayTagger); expect(msg).to.be.a('Float64Array'); expect(msg).to.have.lengthOf(3); @@ -114,8 +114,8 @@ describe('CBOR Typed Array Tagger', function() { }); it('should be able to unpack two typed arrays', function() { - var data = hexToBuffer('82d8484308fe05d84d460100feff0300'); - var msg = CBOR.decode(data, cborTypedArrayTagger); + const data = hexToBuffer('82d8484308fe05d84d460100feff0300'); + const msg = CBOR.decode(data, cborTypedArrayTagger); expect(msg).to.be.a('Array'); expect(msg).to.have.lengthOf(2); diff --git a/test/math-examples.test.js b/test/math-examples.test.js deleted file mode 100644 index e91643af5..000000000 --- a/test/math-examples.test.js +++ /dev/null @@ -1,81 +0,0 @@ -import { describe, it, expect } from 'vitest'; -import * as ROSLIB from '../src/RosLib.js'; - -function clone(x) { - var y = {}; - for (var prop in x) { - if (x.hasOwnProperty(prop)) { - y[prop] = typeof x[prop] === 'object' ? clone(x[prop]) : x[prop]; - } - } - return y; -} - -describe('Math examples', function() { - var v1, q1, v2, q2; - var pos; - it('Vector3 example', function() { - // Let's start by adding some vectors. - v1 = new ROSLIB.Vector3({ - x: 1, - y: 2, - z: 3 - }); - v2 = v1.clone(); - expect(v1).not.equal(v2); - expect(v1).eql(v2); - - v1.add(v2); - expect(clone(v1)).eql({ - x: 2, - y: 4, - z: 6 - }); - }); - - it('Quaternion example', function() { - // Now let's play with some quaternions. - q1 = new ROSLIB.Quaternion({ - x: 0.1, - y: 0.2, - z: 0.3, - w: 0.4 - }); - q2 = q1.clone(); - expect(q1).not.equal(q2); - expect(q1).eql(q2); - - q1.multiply(q2); - q1.invert(); - expect(q1.x).to.be.within(-0.26667, -0.26666); - expect(q1.y).to.be.within(-0.53334, -0.53333); - expect(q1.z).to.be.within(-0.80000, -0.79999); - expect(q1.w).to.be.within(0.06666, 0.06667); - }); - - it('Pose example', function() { - // Let's copy the results into a pose. - pos = new ROSLIB.Pose({ - position: v1, - orientation: q1 - }); - expect(clone(pos)).to.eql(clone({position: v1, orientation: q1})); - }); - - it('Transform example', function() { - // Finally, let's play with some transforms. - var tf = new ROSLIB.Transform({ - translation: v2, - rotation: q2 - }); - pos.applyTransform(tf); - expect(pos.orientation.x).to.be.within(-0.1, -0.09999); - expect(pos.orientation.y).to.be.within(-0.20001, -0.20000); - expect(pos.orientation.z).to.be.within(-0.3, -0.3); - expect(pos.orientation.w).to.be.within(0.39999, 0.4); - - expect(pos.position.x).to.be.within(1.6, 1.60001); - expect(pos.position.y).to.be.within(3.2, 3.20001); - expect(pos.position.z).to.be.within(4.8, 4.80001); - }); -}); diff --git a/test/math-examples.test.ts b/test/math-examples.test.ts new file mode 100644 index 000000000..e984fa425 --- /dev/null +++ b/test/math-examples.test.ts @@ -0,0 +1,84 @@ +import { describe, it, expect } from 'vitest'; +import * as ROSLIB from '../src/RosLib.js'; + +function clone(x: object) { + const y = {}; + for (const prop in x) { + if (x.hasOwnProperty(prop)) { + y[prop] = typeof x[prop] === 'object' ? clone(x[prop]) : x[prop]; + } + } + return y; +} + +describe('Math examples', function () { + let v1: ROSLIB.Vector3; + let q1: ROSLIB.Quaternion; + let v2: ROSLIB.Vector3; + let q2: ROSLIB.Quaternion; + let pos: ROSLIB.Pose; + it('Vector3 example', function () { + // Let's start by adding some vectors. + v1 = new ROSLIB.Vector3({ + x: 1, + y: 2, + z: 3 + }); + v2 = v1.clone(); + expect(v1).not.equal(v2); + expect(v1).eql(v2); + + v1.add(v2); + expect(clone(v1)).eql({ + x: 2, + y: 4, + z: 6 + }); + }); + + it('Quaternion example', function () { + // Now let's play with some quaternions. + q1 = new ROSLIB.Quaternion({ + x: 0.1, + y: 0.2, + z: 0.3, + w: 0.4 + }); + q2 = q1.clone(); + expect(q1).not.equal(q2); + expect(q1).eql(q2); + + q1.multiply(q2); + q1.invert(); + expect(q1.x).to.be.within(-0.26667, -0.26666); + expect(q1.y).to.be.within(-0.53334, -0.53333); + expect(q1.z).to.be.within(-0.80000, -0.79999); + expect(q1.w).to.be.within(0.06666, 0.06667); + }); + + it('Pose example', function () { + // Let's copy the results into a pose. + pos = new ROSLIB.Pose({ + position: v1, + orientation: q1 + }); + expect(clone(pos)).to.eql(clone({position: v1, orientation: q1})); + }); + + it('Transform example', function () { + // Finally, let's play with some transforms. + const tf = new ROSLIB.Transform({ + translation: v2, + rotation: q2 + }); + pos.applyTransform(tf); + expect(pos.orientation.x).to.be.within(-0.1, -0.09999); + expect(pos.orientation.y).to.be.within(-0.20001, -0.20000); + expect(pos.orientation.z).to.be.within(-0.3, -0.3); + expect(pos.orientation.w).to.be.within(0.39999, 0.4); + + expect(pos.position.x).to.be.within(1.6, 1.60001); + expect(pos.position.y).to.be.within(3.2, 3.20001); + expect(pos.position.z).to.be.within(4.8, 4.80001); + }); +}); diff --git a/test/quaternion.test.js b/test/quaternion.test.ts similarity index 61% rename from test/quaternion.test.js rename to test/quaternion.test.ts index 515c6f9e6..ccc641c06 100644 --- a/test/quaternion.test.js +++ b/test/quaternion.test.ts @@ -2,49 +2,49 @@ import { describe, it, expect } from 'vitest'; import * as ROSLIB from '../src/RosLib.js'; -describe('Quaternion', function() { +describe('Quaternion', function () { - describe('creation', function() { + describe('creation', function () { // Test fails. Claims returning Object. // it('should return an object of the correct type', function() { - // var q = new ROSLIB.Quaternion(); + // const q = new ROSLIB.Quaternion(); // expect(q).to.be.a('ROSLIB.Quaternion'); // }); - it('should return an identity quaternion when no params are specified', function() { - var q = new ROSLIB.Quaternion(); + it('should return an identity quaternion when no params are specified', function () { + const q = new ROSLIB.Quaternion(); expect(q.x).to.equal(0); expect(q.y).to.equal(0); expect(q.z).to.equal(0); expect(q.w).to.equal(1); }); - it('should return an identity quaternion when null is specified', function() { - var q = new ROSLIB.Quaternion({ x: null, y: null, z: null, w: null }); + it('should return an identity quaternion when null is specified', function () { + const q = new ROSLIB.Quaternion({x: null, y: null, z: null, w: null}); expect(q.x).to.equal(0); expect(q.y).to.equal(0); expect(q.z).to.equal(0); expect(q.w).to.equal(1); }); - it('should return a quaternion matching the options hash', function() { - var q = new ROSLIB.Quaternion({ x: 1.1, y: 2.2, z: 3.3, w: 4.4 }); + it('should return a quaternion matching the options hash', function () { + const q = new ROSLIB.Quaternion({x: 1.1, y: 2.2, z: 3.3, w: 4.4}); expect(q.x).to.equal(1.1); expect(q.y).to.equal(2.2); expect(q.z).to.equal(3.3); expect(q.w).to.equal(4.4); }); - it('should return a quaternion matching the options', function() { - var q = new ROSLIB.Quaternion({ x: 1, y: 0, z: 0, w: 0 }); + it('should return a quaternion matching the options', function () { + let q = new ROSLIB.Quaternion({x: 1, y: 0, z: 0, w: 0}); expect(q.x).to.equal(1); expect(q.y).to.equal(0); expect(q.z).to.equal(0); expect(q.w).to.equal(0); - q = new ROSLIB.Quaternion({ x: 0, y: 1, z: 0, w: 0 }); + q = new ROSLIB.Quaternion({x: 0, y: 1, z: 0, w: 0}); expect(q.x).to.equal(0); expect(q.y).to.equal(1); expect(q.z).to.equal(0); expect(q.w).to.equal(0); - q = new ROSLIB.Quaternion({ x: 0, y: 0, z: 1, w: 0 }); + q = new ROSLIB.Quaternion({x: 0, y: 0, z: 1, w: 0}); expect(q.x).to.equal(0); expect(q.y).to.equal(0); expect(q.z).to.equal(1); @@ -52,13 +52,13 @@ describe('Quaternion', function() { }); }); - describe('conjugation', function() { - it('should conjugate itself', function() { - var q = new ROSLIB.Quaternion({ x: 1.1, y: 2.2, z: 3.3, w: 4.4 }); + describe('conjugation', function () { + it('should conjugate itself', function () { + const q = new ROSLIB.Quaternion({x: 1.1, y: 2.2, z: 3.3, w: 4.4}); q.conjugate(); - expect(q.x).to.equal(1.1*-1); - expect(q.y).to.equal(2.2*-1); - expect(q.z).to.equal(3.3*-1); + expect(q.x).to.equal(1.1 * -1); + expect(q.y).to.equal(2.2 * -1); + expect(q.z).to.equal(3.3 * -1); }); }); diff --git a/test/transform.test.js b/test/transform.test.ts similarity index 59% rename from test/transform.test.js rename to test/transform.test.ts index f07f51696..229d9697b 100644 --- a/test/transform.test.js +++ b/test/transform.test.ts @@ -1,18 +1,18 @@ import { describe, it, expect } from 'vitest'; import * as ROSLIB from '../src/RosLib.js'; -describe('Transform', function() { +describe('Transform', function () { - describe('creation', function() { + describe('creation', function () { // Fails test. Claims type is Object. // it('should return an object of the correct type', function() { - // var t = new ROSLIB.Transform(); + // const t = new ROSLIB.Transform(); // expect(t).to.be.a('ROSLIB.Transform'); // }); - it('should contain a valid vector and quaternion', function() { - var t = new ROSLIB.Transform({ - translation: new ROSLIB.Vector3({ x: 1, y: 2, z: 3 }), - rotation: new ROSLIB.Quaternion({ x: 0.9, y: 0.8, z: 0.7, w: 1 }) + it('should contain a valid vector and quaternion', function () { + const t = new ROSLIB.Transform({ + translation: new ROSLIB.Vector3({x: 1, y: 2, z: 3}), + rotation: new ROSLIB.Quaternion({x: 0.9, y: 0.8, z: 0.7, w: 1}) }); // expect(t.translation).to.be.a('ROSLIB.Vector3'); expect(t.translation.x).to.equal(1); diff --git a/test/urdf.test.js b/test/urdf.test.js deleted file mode 100644 index 9578b0e99..000000000 --- a/test/urdf.test.js +++ /dev/null @@ -1,122 +0,0 @@ -import { describe, it, expect } from 'vitest'; -import * as ROSLIB from '../src/RosLib.js'; - -import { DOMParser } from '@xmldom/xmldom'; -// See https://developer.mozilla.org/docs/XPathResult#Constants -var XPATH_FIRST_ORDERED_NODE_TYPE = 9; - -var sample_urdf = function (){ - return '' + - ' '+ // test well-behaved versions of the basic shapes - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ // and an extra one with a material - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ // link with referenced material and multiple visuals - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ' '+ - ''; -} - -describe('URDF', function() { - - describe('parsing', function() { - it('should load simple xml', function() { - // http://wiki.ros.org/urdf/Tutorials/Create%20your%20own%20urdf%20file - var urdfModel = new ROSLIB.UrdfModel({ - string: sample_urdf() - }); - - expect(urdfModel.name).to.equal('test_robot'); - }); - - it('should correctly construct visual elements', function() { - var urdfModel = new ROSLIB.UrdfModel({ - string: sample_urdf() - }); - - // Check all the visual elements - expect(urdfModel.links['link1'].visuals.length).to.equal(1); - expect(urdfModel.links['link1'].visuals[0].geometry.radius).to.equal(1.0); - expect(urdfModel.links['link2'].visuals[0].geometry.dimension.x).to.equal(0.5); - expect(urdfModel.links['link2'].visuals[0].geometry.dimension.y).to.equal(0.5); - expect(urdfModel.links['link2'].visuals[0].geometry.dimension.z).to.equal(0.5); - expect(urdfModel.links['link3'].visuals[0].geometry.length).to.equal(2.0); - expect(urdfModel.links['link3'].visuals[0].geometry.radius).to.equal(0.2); - - expect(urdfModel.links['link4'].visuals.length).to.equal(1); - expect(urdfModel.links['link4'].visuals[0].material.name).to.equal('red'); - expect(urdfModel.links['link4'].visuals[0].material.color.r).to.equal(1.0); - expect(urdfModel.links['link4'].visuals[0].material.color.g).to.equal(0); - expect(urdfModel.links['link4'].visuals[0].material.color.b).to.equal(0); - expect(urdfModel.links['link4'].visuals[0].material.color.a).to.equal(1.0); - - expect(urdfModel.links['link5'].visuals.length).to.equal(2); - expect(urdfModel.links['link5'].visuals[0].material.name).to.equal('blue'); - expect(urdfModel.links['link5'].visuals[0].material.color.r).to.equal(0.0); - expect(urdfModel.links['link5'].visuals[0].material.color.g).to.equal(0.0); - expect(urdfModel.links['link5'].visuals[0].material.color.b).to.equal(1.0); - expect(urdfModel.links['link5'].visuals[0].material.color.a).to.equal(1.0); - }); - - it('is ignorant to the xml node', function(){ - var parser = new DOMParser(); - var xml = parser.parseFromString(sample_urdf(), 'text/xml'); - var robotXml = xml.documentElement; - expect(robotXml.getAttribute('name')).to.equal('test_robot'); - }); - }); - -}); diff --git a/test/urdf.test.ts b/test/urdf.test.ts new file mode 100644 index 000000000..f94a2b932 --- /dev/null +++ b/test/urdf.test.ts @@ -0,0 +1,182 @@ +import { describe, it, expect } from 'vitest'; +import * as ROSLIB from '../src/RosLib.js'; + +import { DOMParser, MIME_TYPE } from '@xmldom/xmldom'; +// See https://developer.mozilla.org/docs/XPathResult#Constants +const XPATH_FIRST_ORDERED_NODE_TYPE = 9; + +const sample_urdf = function () { + return '' + + ' ' + // test well-behaved versions of the basic shapes + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + // and an extra one with a material + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + // link with referenced material and multiple visuals + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ' ' + + ''; +}; + +function expectGeometryType(geometry: ROSLIB.UrdfGeometryLike | null | undefined, type: ROSLIB.UrdfType) { + expect(geometry).toBeTruthy(); + switch (type) { + case ROSLIB.UrdfType.SPHERE: + expect(geometry!.type).to.equal(ROSLIB.UrdfType.SPHERE); + expect(geometry).toBeInstanceOf(ROSLIB.UrdfSphere); + break; + case ROSLIB.UrdfType.BOX: + expect(geometry!.type).to.equal(ROSLIB.UrdfType.BOX); + expect(geometry).toBeInstanceOf(ROSLIB.UrdfBox); + break; + case ROSLIB.UrdfType.CYLINDER: + expect(geometry!.type).to.equal(ROSLIB.UrdfType.CYLINDER); + expect(geometry).toBeInstanceOf(ROSLIB.UrdfCylinder); + break; + case ROSLIB.UrdfType.MESH: + expect(geometry!.type).to.equal(ROSLIB.UrdfType.MESH); + expect(geometry).toBeInstanceOf(ROSLIB.UrdfMesh); + break; + } +} + +function expectBoxGeometry(geometry: ROSLIB.UrdfBox, dimensions: ROSLIB.Vector3) { + expect(geometry.dimension?.x).to.equal(dimensions.x); + expect(geometry.dimension?.y).to.equal(dimensions.y); + expect(geometry.dimension?.z).to.equal(dimensions.z); +} + +function expectCylinderGeometry(geometry: ROSLIB.UrdfCylinder, length: number, radius: number) { + expect(geometry.length).to.equal(length); + expect(geometry.radius).to.equal(radius); +} + +function expectMaterialWithColor(material: ROSLIB.UrdfMaterial | null | undefined, name: string, r: number, g: number, b: number, a: number) { + expect(material).toBeTruthy(); + expect(material?.name).to.equal(name); + expect(material?.color).toBeTruthy(); + expect(material?.color?.r).to.equal(r); + expect(material?.color?.g).to.equal(g); + expect(material?.color?.b).to.equal(b); + expect(material?.color?.a).to.equal(a); +} + +describe('URDF', function () { + + describe('parsing', function () { + it('should load simple xml', function () { + // http://wiki.ros.org/urdf/Tutorials/Create%20your%20own%20urdf%20file + const urdfModel = new ROSLIB.UrdfModel({ + string: sample_urdf() + }); + + expect(urdfModel.name).to.equal('test_robot'); + }); + + it('should correctly construct visual elements', function () { + const urdfModel = new ROSLIB.UrdfModel({ + string: sample_urdf() + }); + + // Check types and values of visuals + expect(urdfModel.links['link1'].visuals.length).to.equal(1); + expectGeometryType(urdfModel.links['link1']?.visuals[0]?.geometry, ROSLIB.UrdfType.SPHERE); + + expect(urdfModel.links['link2'].visuals.length).to.equal(1); + expectGeometryType(urdfModel.links['link2']?.visuals[0]?.geometry, ROSLIB.UrdfType.BOX); + + expect(urdfModel.links['link3'].visuals.length).to.equal(1); + expectGeometryType(urdfModel.links['link3']?.visuals[0]?.geometry, ROSLIB.UrdfType.CYLINDER); + + expect(urdfModel.links['link4'].visuals.length).to.equal(1); + expectGeometryType(urdfModel.links['link4']?.visuals[0]?.geometry, ROSLIB.UrdfType.BOX); + + expect(urdfModel.links['link5'].visuals.length).to.equal(2); + expectGeometryType(urdfModel.links['link5']?.visuals[0]?.geometry, ROSLIB.UrdfType.BOX); + expectGeometryType(urdfModel.links['link5']?.visuals[1]?.geometry, ROSLIB.UrdfType.BOX); + + // Check all the dimensions + expect((urdfModel.links['link1']?.visuals[0]?.geometry as ROSLIB.UrdfSphere).radius).to.equal(1.0); + expectBoxGeometry( + (urdfModel.links['link2']?.visuals[0]?.geometry as ROSLIB.UrdfBox), + new ROSLIB.Vector3({x: 0.5, y: 0.5, z: 0.5}) + ); + expectCylinderGeometry((urdfModel.links['link3']?.visuals[0]?.geometry as ROSLIB.UrdfCylinder), 2.0, 0.2); + expectBoxGeometry( + (urdfModel.links['link4']?.visuals[0]?.geometry as ROSLIB.UrdfBox), + new ROSLIB.Vector3({x: 1.0, y: 1.0, z: 1.0}) + ); + expectBoxGeometry( + (urdfModel.links['link5']?.visuals[0]?.geometry as ROSLIB.UrdfBox), + new ROSLIB.Vector3({x: 1.0, y: 1.0, z: 1.0}) + ); + expectBoxGeometry( + (urdfModel.links['link5']?.visuals[1]?.geometry as ROSLIB.UrdfBox), + new ROSLIB.Vector3({x: 2.0, y: 2.0, z: 2.0}) + ); + + expectMaterialWithColor(urdfModel.links['link4'].visuals[0].material, 'red', 1.0, 0, 0, 1.0); + expectMaterialWithColor(urdfModel.links['link5'].visuals[0].material, 'blue', 0.0, 0.0, 1.0, 1.0); + }); + + it('is ignorant to the xml node', function () { + const parser = new DOMParser(); + const xml = parser.parseFromString(sample_urdf(), MIME_TYPE.XML_TEXT); + const robotXml = xml.documentElement; + expect(robotXml.getAttribute('name')).to.equal('test_robot'); + }); + }); + +}); diff --git a/tsconfig.json b/tsconfig.json index c466c4a0e..4403665c7 100644 --- a/tsconfig.json +++ b/tsconfig.json @@ -1,6 +1,7 @@ { "compilerOptions": { "target": "es5" /* Set the JavaScript language version for emitted JavaScript and include compatible library declarations. */, + "lib": ["es2022"] /* Specify library files to be included in the compilation. */, "module": "esnext" /* Specify what module code is generated. */, "rootDir": "./src" /* Specify the root folder within your source files. */, "allowJs": true /* Allow JavaScript files to be a part of your program. Use the 'checkJS' option to get errors from these files. */,