464 lines
14 KiB
JavaScript
464 lines
14 KiB
JavaScript
import { test } from "node:test";
|
|
import assert from "node:assert/strict";
|
|
import { Matrix4 } from "three";
|
|
import {
|
|
componentPlacementAtZero,
|
|
extractMtlTextureReferences,
|
|
extractObjMaterialReferences,
|
|
packageMeshUri,
|
|
resolvePackageResource,
|
|
addDefaultJointLimits,
|
|
semanticAssemblyNames,
|
|
semanticComponentScope,
|
|
semanticJointNames,
|
|
semanticLinkNames,
|
|
uniqueComponentScopes,
|
|
validateAssemblyTree,
|
|
validateJointLimitRequirements,
|
|
validateUrdfElementTree,
|
|
} from "../src/workspace/ros2-export.ts";
|
|
|
|
const part = (instanceId, extra = {}) => ({
|
|
instanceId,
|
|
component: { componentName: instanceId, componentCode: instanceId },
|
|
urdfMainPath: "robot.urdf",
|
|
resources: [],
|
|
resourceIssues: [],
|
|
...extra,
|
|
});
|
|
const edge = (parentInstanceId, childInstanceId) => ({
|
|
parentInstanceId,
|
|
parentFrameIndex: 0,
|
|
parentFrame: "mount",
|
|
parentLink: "mount",
|
|
childInstanceId,
|
|
childFrameIndex: 0,
|
|
childFrame: "mount",
|
|
childRootLink: "root",
|
|
});
|
|
const assembly = (rootInstanceId, connections) => ({
|
|
status: "ready",
|
|
rootInstanceId,
|
|
connections,
|
|
issues: [],
|
|
});
|
|
const xmlNode = (tagName, attributes = {}, children = []) => ({
|
|
tagName,
|
|
namespaceURI: null,
|
|
prefix: null,
|
|
attributes: Object.keys(attributes).map((name) => ({ name })),
|
|
childNodes: children.map((child) => ({ ...child, nodeType: 1 })),
|
|
});
|
|
|
|
const limitNode = (attributes = {}) => ({
|
|
tagName: "limit",
|
|
nodeType: 1,
|
|
attributes: Object.keys(attributes).map((name) => ({ name })),
|
|
childNodes: [],
|
|
getAttribute: (name) => attributes[name] ?? null,
|
|
hasAttribute: (name) =>
|
|
Object.prototype.hasOwnProperty.call(attributes, name),
|
|
setAttribute: (name, value) => {
|
|
attributes[name] = value;
|
|
},
|
|
});
|
|
|
|
const jointNode = (type, limit) => {
|
|
const children = limit ? [limit] : [];
|
|
const attributes = { type };
|
|
return {
|
|
tagName: "joint",
|
|
nodeType: 1,
|
|
attributes: Object.keys(attributes).map((name) => ({ name })),
|
|
childNodes: children,
|
|
getAttribute: (name) => attributes[name] ?? null,
|
|
ownerDocument: {
|
|
createElement: () => limitNode(),
|
|
},
|
|
appendChild: (child) => {
|
|
children.push(child);
|
|
return child;
|
|
},
|
|
};
|
|
};
|
|
|
|
test("component categories produce stable human-readable URDF scopes", () => {
|
|
assert.equal(semanticComponentScope("TORSO_0"), "center_torso");
|
|
assert.equal(semanticComponentScope("ARM_L"), "left_arm");
|
|
assert.equal(semanticComponentScope("ARM_R"), "right_arm");
|
|
assert.equal(semanticComponentScope("SENSOR_R"), "right_sensor");
|
|
});
|
|
|
|
test("repeated component types receive a suffix only on the repeated scope", () => {
|
|
const scopes = uniqueComponentScopes([
|
|
{ instanceId: "left-arm-1", component: { category: "ARM_L" } },
|
|
{ instanceId: "torso", component: { category: "TORSO_0" } },
|
|
{ instanceId: "left-arm-2", component: { category: "ARM_L" } },
|
|
]);
|
|
assert.equal(scopes.get("left-arm-1"), "left_arm");
|
|
assert.equal(scopes.get("torso"), "center_torso");
|
|
assert.equal(scopes.get("left-arm-2"), "left_arm_2");
|
|
});
|
|
|
|
test("component Link and Joint names describe their role instead of source part IDs", () => {
|
|
const armLinks = semanticLinkNames("left_arm", "LINK_ARM_L", [
|
|
"LINK_ARM_L",
|
|
"L1",
|
|
"L2",
|
|
"LINK_HAND_L",
|
|
]);
|
|
assert.deepEqual(
|
|
[...armLinks],
|
|
[
|
|
["LINK_ARM_L", "left_arm_root_link"],
|
|
["L1", "left_arm_segment_1_link"],
|
|
["L2", "left_arm_segment_2_link"],
|
|
["LINK_HAND_L", "left_arm_wrist_link"],
|
|
],
|
|
);
|
|
assert.deepEqual(
|
|
[
|
|
...semanticJointNames("left_arm", armLinks, [
|
|
{ name: "J1", child: "L1" },
|
|
{ name: "J2", child: "LINK_HAND_L" },
|
|
]),
|
|
],
|
|
[
|
|
["J1", "left_arm_segment_1_joint"],
|
|
["J2", "left_arm_wrist_joint"],
|
|
],
|
|
);
|
|
|
|
const torsoLinks = semanticLinkNames("center_torso", "base_link", [
|
|
"base_link",
|
|
"LINK_ARM_L",
|
|
"LINK_ARM_R",
|
|
"LINK_HEAD_0",
|
|
]);
|
|
assert.equal(torsoLinks.get("base_link"), "center_torso_root_link");
|
|
assert.equal(torsoLinks.get("LINK_ARM_L"), "center_torso_left_shoulder_link");
|
|
assert.equal(torsoLinks.get("LINK_HEAD_0"), "center_torso_neck_link");
|
|
});
|
|
|
|
test("assembly names express only parent and child component scopes", () => {
|
|
assert.deepEqual(semanticAssemblyNames("center_torso", "left_arm", false), {
|
|
fixedJoint: "center_torso_to_left_arm_fixed_joint",
|
|
});
|
|
const movable = semanticAssemblyNames("left_arm", "left_hand", true);
|
|
assert.deepEqual(movable, {
|
|
mountLink: "left_arm_to_left_hand_mount_link",
|
|
rotorLink: "left_arm_to_left_hand_rotor_link",
|
|
mountFixedJoint: "left_arm_to_left_hand_mount_fixed_joint",
|
|
joint: "left_arm_to_left_hand_joint",
|
|
childFixedJoint: "left_arm_to_left_hand_child_fixed_joint",
|
|
});
|
|
for (const name of Object.values(movable))
|
|
assert.doesNotMatch(name, /(?:^|_)part_|assembly_/);
|
|
});
|
|
|
|
test("URDF core elements accept their standard attributes and parent-child structure", () => {
|
|
const tree = xmlNode("robot", { name: "test_robot" }, [
|
|
xmlNode("material", { name: "paint" }, [
|
|
xmlNode("color", { rgba: "0.5 0.5 0.5 1" }),
|
|
]),
|
|
xmlNode("material", { name: "textured" }, [
|
|
xmlNode("texture", { filename: "texture.png" }),
|
|
]),
|
|
xmlNode("link", { name: "base" }, [
|
|
xmlNode("inertial", {}, [
|
|
xmlNode("origin", { xyz: "0 0 0", rpy: "0 0 0" }),
|
|
xmlNode("mass", { value: "1" }),
|
|
xmlNode("inertia", {
|
|
ixx: "1",
|
|
ixy: "0",
|
|
ixz: "0",
|
|
iyy: "1",
|
|
iyz: "0",
|
|
izz: "1",
|
|
}),
|
|
]),
|
|
xmlNode("visual", { name: "mesh_visual" }, [
|
|
xmlNode("origin", { xyz: "0 0 0", rpy: "0 0 0" }),
|
|
xmlNode("geometry", {}, [
|
|
xmlNode("mesh", { filename: "mesh.stl", scale: "1 1 1" }),
|
|
]),
|
|
xmlNode("material", { name: "inline" }, [
|
|
xmlNode("color", { rgba: "1 1 1 1" }),
|
|
]),
|
|
]),
|
|
xmlNode("visual", {}, [
|
|
xmlNode("geometry", {}, [xmlNode("box", { size: "1 1 1" })]),
|
|
]),
|
|
xmlNode("visual", {}, [
|
|
xmlNode("geometry", {}, [
|
|
xmlNode("cylinder", { radius: "1", length: "1" }),
|
|
]),
|
|
]),
|
|
xmlNode("visual", {}, [
|
|
xmlNode("geometry", {}, [xmlNode("sphere", { radius: "1" })]),
|
|
]),
|
|
xmlNode("collision", { name: "collision" }, [
|
|
xmlNode("origin", { xyz: "0 0 0", rpy: "0 0 0" }),
|
|
xmlNode("geometry", {}, [
|
|
xmlNode("mesh", { filename: "collision.stl" }),
|
|
]),
|
|
]),
|
|
]),
|
|
xmlNode("joint", { name: "joint", type: "revolute" }, [
|
|
xmlNode("origin", { xyz: "0 0 0", rpy: "0 0 0" }),
|
|
xmlNode("parent", { link: "base" }),
|
|
xmlNode("child", { link: "tip" }),
|
|
xmlNode("axis", { xyz: "0 0 1" }),
|
|
xmlNode("calibration", { rising: "0", falling: "0" }),
|
|
xmlNode("dynamics", { damping: "0", friction: "0" }),
|
|
xmlNode("limit", {
|
|
lower: "-1",
|
|
upper: "1",
|
|
effort: "1",
|
|
velocity: "1",
|
|
}),
|
|
xmlNode("mimic", { joint: "other", multiplier: "1", offset: "0" }),
|
|
xmlNode("safety_controller", {
|
|
soft_lower_limit: "-1",
|
|
soft_upper_limit: "1",
|
|
k_position: "1",
|
|
k_velocity: "1",
|
|
}),
|
|
]),
|
|
]);
|
|
|
|
assert.doesNotThrow(() => validateUrdfElementTree(tree, "test_robot"));
|
|
assert.throws(
|
|
() =>
|
|
validateUrdfElementTree(
|
|
xmlNode("robot", { name: "test_robot" }, [xmlNode("plugin")]),
|
|
"test_robot",
|
|
),
|
|
/不支持的 URDF 扩展 <plugin>/,
|
|
);
|
|
assert.throws(
|
|
() =>
|
|
validateUrdfElementTree(
|
|
xmlNode("link", { name: "base" }, [
|
|
xmlNode("inertial", { unexpected: "value" }),
|
|
]),
|
|
"test_robot",
|
|
),
|
|
/<inertial> 含有非标准属性 unexpected/,
|
|
);
|
|
});
|
|
|
|
test("movable joints may omit effort and velocity limits during export", () => {
|
|
const withoutLimits = undefined;
|
|
const positionOnly = {
|
|
hasAttribute: (name) => name === "lower" || name === "upper",
|
|
};
|
|
|
|
assert.doesNotThrow(() =>
|
|
validateJointLimitRequirements("continuous", "arm", withoutLimits, "robot"),
|
|
);
|
|
assert.doesNotThrow(() =>
|
|
validateJointLimitRequirements("revolute", "arm", positionOnly, "robot"),
|
|
);
|
|
assert.doesNotThrow(() =>
|
|
validateJointLimitRequirements("prismatic", "slide", positionOnly, "robot"),
|
|
);
|
|
assert.throws(
|
|
() =>
|
|
validateJointLimitRequirements(
|
|
"revolute",
|
|
"arm",
|
|
{ hasAttribute: (name) => name === "lower" },
|
|
"robot",
|
|
),
|
|
/limit 缺少 upper/,
|
|
);
|
|
assert.throws(
|
|
() =>
|
|
validateJointLimitRequirements("prismatic", "slide", undefined, "robot"),
|
|
/必须定义 limit/,
|
|
);
|
|
});
|
|
|
|
test("export adds placeholder effort and velocity without overwriting source values", () => {
|
|
const existing = limitNode({ effort: "4.5" });
|
|
const continuous = jointNode("continuous", existing);
|
|
const revolute = jointNode("revolute");
|
|
const fixed = jointNode("fixed");
|
|
const robot = {
|
|
getElementsByTagName: (tagName) =>
|
|
tagName === "joint" ? [continuous, revolute, fixed] : [],
|
|
ownerDocument: {
|
|
createElement: () => limitNode(),
|
|
},
|
|
};
|
|
|
|
assert.equal(addDefaultJointLimits(robot), 2);
|
|
assert.equal(existing.getAttribute("effort"), "4.5");
|
|
assert.equal(existing.getAttribute("velocity"), "1");
|
|
assert.equal(revolute.childNodes.length, 1);
|
|
assert.equal(revolute.childNodes[0].getAttribute("effort"), "1");
|
|
assert.equal(revolute.childNodes[0].getAttribute("velocity"), "1");
|
|
assert.equal(fixed.childNodes.length, 0);
|
|
});
|
|
|
|
test("assembly export orders a connected tree from its configured root", () => {
|
|
const components = [part("arm"), part("torso"), part("hand")];
|
|
const ordered = validateAssemblyTree(
|
|
components,
|
|
assembly("torso", [edge("arm", "hand"), edge("torso", "arm")]),
|
|
);
|
|
assert.deepEqual(
|
|
ordered.map(({ instanceId }) => instanceId),
|
|
["torso", "arm", "hand"],
|
|
);
|
|
});
|
|
|
|
test("incomplete, ambiguous-parent, cyclic and disconnected assemblies are rejected", () => {
|
|
const three = [part("root"), part("left"), part("right")];
|
|
assert.throws(
|
|
() => validateAssemblyTree(three, assembly("root", [edge("root", "left")])),
|
|
/连接数|未连接/,
|
|
);
|
|
assert.throws(
|
|
() =>
|
|
validateAssemblyTree(
|
|
three,
|
|
assembly("root", [edge("root", "left"), edge("root", "left")]),
|
|
),
|
|
/连接数|多个父连接/,
|
|
);
|
|
assert.throws(
|
|
() =>
|
|
validateAssemblyTree(
|
|
three,
|
|
assembly("root", [edge("left", "right"), edge("right", "left")]),
|
|
),
|
|
/断开/,
|
|
);
|
|
assert.throws(
|
|
() =>
|
|
validateAssemblyTree(
|
|
three,
|
|
assembly("root", [edge("root", "left"), edge("right", "right")]),
|
|
),
|
|
/自身|连接数/,
|
|
);
|
|
});
|
|
|
|
test("duplicate instances, blocked results and resource issues are rejected", () => {
|
|
assert.throws(
|
|
() =>
|
|
validateAssemblyTree(
|
|
[part("root"), part("root")],
|
|
assembly("root", [edge("root", "other")]),
|
|
),
|
|
/重复/,
|
|
);
|
|
assert.throws(
|
|
() =>
|
|
validateAssemblyTree([part("root")], {
|
|
...assembly("root", []),
|
|
status: "blocked",
|
|
}),
|
|
/未就绪/,
|
|
);
|
|
assert.throws(
|
|
() =>
|
|
validateAssemblyTree(
|
|
[part("root", { resourceIssues: [{ message: "missing" }] })],
|
|
assembly("root", []),
|
|
),
|
|
/资源清单/,
|
|
);
|
|
});
|
|
|
|
test("resource resolution handles URDF-relative paths, parent paths and package URIs", () => {
|
|
const resources = [
|
|
{ relativePath: "meshes/base.stl", url: "/base" },
|
|
{ relativePath: "materials/body.mtl", url: "/body" },
|
|
{ relativePath: "textures/red.png", url: "/red" },
|
|
];
|
|
assert.equal(
|
|
resolvePackageResource(resources, "urdf/robot.urdf", "../meshes/base.stl"),
|
|
resources[0],
|
|
);
|
|
assert.equal(
|
|
resolvePackageResource(
|
|
resources,
|
|
"meshes/base.obj",
|
|
"../materials/body.mtl",
|
|
),
|
|
resources[1],
|
|
);
|
|
assert.equal(
|
|
resolvePackageResource(
|
|
resources,
|
|
"urdf/robot.urdf",
|
|
"package://source/meshes/base.stl",
|
|
),
|
|
resources[0],
|
|
);
|
|
assert.throws(
|
|
() =>
|
|
resolvePackageResource(resources, "meshes/base.obj", "../../outside.mtl"),
|
|
/不能离开/,
|
|
);
|
|
});
|
|
|
|
test("ambiguous package suffixes and unlisted absolute URLs are rejected", () => {
|
|
const resources = [
|
|
{ relativePath: "part_a/meshes/link.stl", url: "/a" },
|
|
{ relativePath: "part_b/meshes/link.stl", url: "/b" },
|
|
];
|
|
assert.throws(
|
|
() =>
|
|
resolvePackageResource(
|
|
resources,
|
|
"robot.urdf",
|
|
"package://source/meshes/link.stl",
|
|
),
|
|
/唯一定位/,
|
|
);
|
|
assert.throws(
|
|
() =>
|
|
resolvePackageResource(resources, "robot.urdf", "file:///tmp/link.stl"),
|
|
/映射/,
|
|
);
|
|
});
|
|
|
|
test("OBJ and MTL dependency paths are parsed with quoting and map options", () => {
|
|
assert.deepEqual(
|
|
extractObjMaterialReferences(
|
|
'mtllib "materials/body paint.mtl" other.mtl\n',
|
|
),
|
|
["materials/body paint.mtl", "other.mtl"],
|
|
);
|
|
assert.deepEqual(
|
|
extractMtlTextureReferences(
|
|
'newmtl body\nmap_Kd -s 1 1 1 1 textures/albedo.png\nbump -bm 0.4 "normal map.png"\n',
|
|
),
|
|
["textures/albedo.png", "normal map.png"],
|
|
);
|
|
});
|
|
|
|
test("zero-pose placement honors both non-root interface offsets", () => {
|
|
const parentInterface = new Matrix4().makeTranslation(0.4, -0.2, 0.8);
|
|
const childInterface = new Matrix4().makeTranslation(-0.15, 0.3, 0.05);
|
|
const placement = componentPlacementAtZero(parentInterface, childInterface);
|
|
const interfaceAtZero = placement.clone().multiply(childInterface);
|
|
assert.ok(
|
|
interfaceAtZero.elements.every(
|
|
(value, index) =>
|
|
Math.abs(value - parentInterface.elements[index]) < 1e-10,
|
|
),
|
|
);
|
|
});
|
|
|
|
test("package mesh URIs encode path segments without flattening paths", () => {
|
|
assert.equal(
|
|
packageMeshUri("meshes/part 1/Link#2.STL"),
|
|
"package://cadworld_robot_description/meshes/part%201/Link%232.STL",
|
|
);
|
|
});
|