Skip to content

Commit 7e2b1fd

Browse files
ft-labft-lab
committed
ros package support
1 parent 9e8a93a commit 7e2b1fd

File tree

11 files changed

+327
-6
lines changed

11 files changed

+327
-6
lines changed

‎tests/data/assets/grid.png‎

666 Bytes
Loading

‎tests/data/error_ref_ros_package.urdf‎

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,24 @@
1+
<?xml version="1.0"?>
2+
<robot name="simple_ref_ros_package">
3+
<link name="Baselink">
4+
<visual>
5+
<geometry>
6+
<box size="0.5 0.5 0.5"/>
7+
</geometry>
8+
</visual>
9+
</link>
10+
<link name="link_mesh_stl">
11+
<visual>
12+
<geometry>
13+
<!-- When specifying a package name without specifying a relative path. -->
14+
<mesh filename="package://test_package"/>
15+
</geometry>
16+
</visual>
17+
</link>
18+
19+
<joint name="root_joint" type="fixed">
20+
<origin rpy="0 0 0" xyz="1 0 0"/>
21+
<parent link="Baselink"/>
22+
<child link="link_mesh_stl"/>
23+
</joint>
24+
</robot>

‎tests/data/ros_packages.urdf‎

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
<?xml version="1.0"?>
2+
<robot name="ros_packages">
3+
<material name="texture_material">
4+
<texture filename="package://test_texture_package/assets/grid.png"/>
5+
</material>
6+
7+
<link name="BaseLink">
8+
<visual>
9+
<geometry>
10+
<box size="0.5 0.5 0.5"/>
11+
</geometry>
12+
<material name="texture_material"/>
13+
</visual>
14+
</link>
15+
<link name="link_mesh_stl">
16+
<visual>
17+
<geometry>
18+
<mesh filename="package://test_package/assets/box.stl"/>
19+
</geometry>
20+
</visual>
21+
</link>
22+
23+
<joint name="root_joint" type="fixed">
24+
<origin rpy="0 0 0" xyz="1 0 0"/>
25+
<parent link="BaseLink"/>
26+
<child link="link_mesh_stl"/>
27+
</joint>
28+
</robot>

‎tests/testCli.py‎

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -148,3 +148,30 @@ def test_conversion_exception_verbose(self):
148148
usdex.test.ScopedDiagnosticChecker(self, [], level=usdex.core.DiagnosticsLevel.eWarning),
149149
):
150150
run()
151+
152+
def test_conversion_exception_ros_package_name_format(self):
153+
robot = "tests/data/simple_ref_ros_package.urdf"
154+
output_dir = self.tmpDir()
155+
156+
# Run the converter with the specified ROS package names.
157+
# Here, we will specify an incorrect CLI argument.
158+
package_1 = "test_package" # Error: If no path is specified
159+
with (
160+
self.assertRaisesRegex(RuntimeError, r"Invalid format: .*. Expected format: package_name=path"),
161+
patch("sys.argv", ["urdf_usd_converter", robot, str(output_dir), "--package", package_1]),
162+
):
163+
self.assertEqual(run(), 1, "Expected non-zero exit code for invalid input")
164+
165+
def test_conversion_exception_multiple_ros_packages_name_format(self):
166+
robot = "tests/data/simple_ref_ros_package.urdf"
167+
output_dir = self.tmpDir()
168+
169+
# Run the converter with the specified ROS package names.
170+
# Here, we will specify an incorrect CLI argument.
171+
package_1 = "test_package=/home/foo" # Correct specification
172+
package_2 = "test_texture_package=" # Error: If no path is specified
173+
with (
174+
self.assertRaisesRegex(RuntimeError, r"Invalid format: .*. Expected format: package_name=path"),
175+
patch("sys.argv", ["urdf_usd_converter", robot, str(output_dir), "--package", package_1, "--package", package_2]),
176+
):
177+
self.assertEqual(run(), 1, "Expected non-zero exit code for invalid input")

‎tests/testConverter.py‎

Lines changed: 69 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,9 +1,10 @@
11
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
22
# SPDX-License-Identifier: Apache-2.0
33
import pathlib
4+
import shutil
45

56
import usdex.test
6-
from pxr import Tf
7+
from pxr import Tf, Usd, UsdGeom
78

89
import urdf_usd_converter
910
from tests.util.ConverterTestCase import ConverterTestCase
@@ -78,3 +79,70 @@ def test_load_warning_obj_no_shape(self):
7879
level=usdex.core.DiagnosticsLevel.eWarning,
7980
):
8081
converter.convert(input_path, output_dir)
82+
83+
def test_ros_package_name_without_relative_path(self):
84+
# When an invalid path is specified in the ROS package URI.
85+
86+
input_path = "tests/data/error_ref_ros_package.urdf"
87+
output_dir = self.tmpDir()
88+
89+
converter = urdf_usd_converter.Converter()
90+
with self.assertRaisesRegex(ValueError, r".*Invalid ROS package URI. No relative path specified: package://test_package.*"):
91+
converter.convert(input_path, output_dir)
92+
93+
def test_ros_packages(self):
94+
# Specify ROS package arguments as Converter constructor.
95+
96+
input_path = "tests/data/ros_packages.urdf"
97+
output_dir = self.tmpDir()
98+
99+
test_package_dir = output_dir + "/temp"
100+
test_texture_package_dir = output_dir + "/temp/textures"
101+
pathlib.Path(test_package_dir + "/assets").mkdir(parents=True, exist_ok=True)
102+
pathlib.Path(test_texture_package_dir + "/assets").mkdir(parents=True, exist_ok=True)
103+
104+
# Copy "tests/data/assets/box.stl" to test_package_dir
105+
shutil.copy("tests/data/assets/box.stl", test_package_dir + "/assets")
106+
107+
# Copy "tests/data/assets/grid.png" to test_texture_package_dir
108+
shutil.copy("tests/data/assets/grid.png", test_texture_package_dir + "/assets")
109+
110+
temp_stl_file_path = test_package_dir + "/assets/box.stl"
111+
temp_texture_file_path = test_texture_package_dir + "/assets/grid.png"
112+
self.assertTrue(pathlib.Path(temp_stl_file_path).exists())
113+
self.assertTrue(pathlib.Path(temp_texture_file_path).exists())
114+
115+
packages = [
116+
{"name": "test_package", "path": test_package_dir},
117+
{"name": "test_texture_package", "path": test_texture_package_dir},
118+
]
119+
converter = urdf_usd_converter.Converter(ros_packages=packages)
120+
asset_path = converter.convert(input_path, output_dir)
121+
self.assertIsNotNone(asset_path)
122+
self.assertTrue(pathlib.Path(asset_path.path).exists())
123+
124+
# If the URI specified in converter.convert is invalid, a warning will be displayed and processing will continue.
125+
# Therefore, this process opens the USD file to verify that the mesh has been loaded correctly.
126+
output_path = pathlib.Path(output_dir) / "ros_packages.usda"
127+
self.assertTrue(output_path.exists())
128+
129+
self.stage: Usd.Stage = Usd.Stage.Open(str(output_path))
130+
self.assertIsValidUsd(self.stage)
131+
132+
# Check geometry.
133+
default_prim = self.stage.GetDefaultPrim()
134+
geometry_scope_prim = self.stage.GetPrimAtPath(default_prim.GetPath().AppendChild("Geometry"))
135+
self.assertTrue(geometry_scope_prim.IsValid())
136+
137+
link_mesh_stl_path = geometry_scope_prim.GetPath().AppendChild("BaseLink").AppendChild("link_mesh_stl")
138+
link_stl_prim = self.stage.GetPrimAtPath(link_mesh_stl_path)
139+
self.assertTrue(link_stl_prim.IsValid())
140+
self.assertTrue(link_stl_prim.IsA(UsdGeom.Xform))
141+
142+
stl_mesh_prim = self.stage.GetPrimAtPath(link_mesh_stl_path.AppendChild("box"))
143+
self.assertTrue(stl_mesh_prim.IsValid())
144+
self.assertTrue(stl_mesh_prim.IsA(UsdGeom.Mesh))
145+
self.assertTrue(stl_mesh_prim.HasAuthoredReferences())
146+
147+
# Check material texture.
148+
# TODO: Here we need to make sure that the reference to the usd file is correct after the texture is loaded.

‎tests/testROSPackagesCli.py‎

Lines changed: 107 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,107 @@
1+
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
2+
# SPDX-License-Identifier: Apache-2.0
3+
import pathlib
4+
import shutil
5+
from unittest.mock import patch
6+
7+
from pxr import Usd, UsdGeom
8+
9+
from tests.util.ConverterTestCase import ConverterTestCase
10+
from urdf_usd_converter._impl.cli import run
11+
12+
13+
class TestROSPackagesCli(ConverterTestCase):
14+
def test_do_not_specify_ros_package_name(self):
15+
# If the `package` argument is not specified in `converter.convert`.
16+
# In this case, if the mesh or texture URI specifies "package://PackageName/foo/test.png",
17+
# this will be removed and treated as a relative path "foo/test.png".
18+
input_path = "tests/data/ros_packages.urdf"
19+
output_dir = self.tmpDir()
20+
21+
with patch("sys.argv", ["urdf_usd_converter", input_path, output_dir]):
22+
self.assertEqual(run(), 0, f"Failed to convert {input_path}")
23+
24+
output_path = pathlib.Path(output_dir) / "ros_packages.usda"
25+
self.assertTrue(output_path.exists())
26+
27+
self.stage: Usd.Stage = Usd.Stage.Open(str(output_path))
28+
self.assertIsValidUsd(self.stage)
29+
30+
# Check geometry.
31+
default_prim = self.stage.GetDefaultPrim()
32+
geometry_scope_prim = self.stage.GetPrimAtPath(default_prim.GetPath().AppendChild("Geometry"))
33+
self.assertTrue(geometry_scope_prim.IsValid())
34+
35+
link_mesh_stl_path = geometry_scope_prim.GetPath().AppendChild("BaseLink").AppendChild("link_mesh_stl")
36+
link_stl_prim = self.stage.GetPrimAtPath(link_mesh_stl_path)
37+
self.assertTrue(link_stl_prim.IsValid())
38+
self.assertTrue(link_stl_prim.IsA(UsdGeom.Xform))
39+
40+
stl_mesh_prim = self.stage.GetPrimAtPath(link_mesh_stl_path.AppendChild("box"))
41+
self.assertTrue(stl_mesh_prim.IsValid())
42+
self.assertTrue(stl_mesh_prim.IsA(UsdGeom.Mesh))
43+
self.assertTrue(stl_mesh_prim.HasAuthoredReferences())
44+
45+
# Check material texture.
46+
# TODO: Here we need to make sure that the reference to the usd file is correct after the texture is loaded.
47+
48+
def test_specify_ros_package_names(self):
49+
"""
50+
Specify ROS package arguments as CLI
51+
"""
52+
input_path = "tests/data/ros_packages.urdf"
53+
output_dir = self.tmpDir()
54+
55+
test_package_dir = output_dir + "/temp"
56+
test_texture_package_dir = output_dir + "/temp/textures"
57+
pathlib.Path(test_package_dir + "/assets").mkdir(parents=True, exist_ok=True)
58+
pathlib.Path(test_texture_package_dir + "/assets").mkdir(parents=True, exist_ok=True)
59+
60+
# Copy "tests/data/assets/box.stl" to test_package_dir
61+
shutil.copy("tests/data/assets/box.stl", test_package_dir + "/assets")
62+
63+
# Copy "tests/data/assets/grid.png" to test_texture_package_dir
64+
shutil.copy("tests/data/assets/grid.png", test_texture_package_dir + "/assets")
65+
66+
temp_stl_file_path = test_package_dir + "/assets/box.stl"
67+
temp_texture_file_path = test_texture_package_dir + "/assets/grid.png"
68+
self.assertTrue(pathlib.Path(temp_stl_file_path).exists())
69+
self.assertTrue(pathlib.Path(temp_texture_file_path).exists())
70+
71+
with patch(
72+
"sys.argv",
73+
[
74+
"urdf_usd_converter",
75+
input_path,
76+
output_dir,
77+
"--package",
78+
"test_package=" + test_package_dir,
79+
"--package",
80+
"test_texture_package=" + test_texture_package_dir,
81+
],
82+
):
83+
self.assertEqual(run(), 0, f"Failed to convert {input_path}")
84+
85+
output_path = pathlib.Path(output_dir) / "ros_packages.usda"
86+
self.assertTrue(output_path.exists())
87+
88+
self.stage: Usd.Stage = Usd.Stage.Open(str(output_path))
89+
self.assertIsValidUsd(self.stage)
90+
91+
# Check geometry.
92+
default_prim = self.stage.GetDefaultPrim()
93+
geometry_scope_prim = self.stage.GetPrimAtPath(default_prim.GetPath().AppendChild("Geometry"))
94+
self.assertTrue(geometry_scope_prim.IsValid())
95+
96+
link_mesh_stl_path = geometry_scope_prim.GetPath().AppendChild("BaseLink").AppendChild("link_mesh_stl")
97+
link_stl_prim = self.stage.GetPrimAtPath(link_mesh_stl_path)
98+
self.assertTrue(link_stl_prim.IsValid())
99+
self.assertTrue(link_stl_prim.IsA(UsdGeom.Xform))
100+
101+
stl_mesh_prim = self.stage.GetPrimAtPath(link_mesh_stl_path.AppendChild("box"))
102+
self.assertTrue(stl_mesh_prim.IsValid())
103+
self.assertTrue(stl_mesh_prim.IsA(UsdGeom.Mesh))
104+
self.assertTrue(stl_mesh_prim.HasAuthoredReferences())
105+
106+
# Check material texture.
107+
# TODO: Here we need to make sure that the reference to the usd file is correct after the texture is loaded.

‎urdf_usd_converter/_impl/cli.py‎

Lines changed: 20 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -47,7 +47,9 @@ def run() -> int:
4747
Tf.Status(f"USDEX Version: {usdex.core.version()}")
4848

4949
try:
50-
converter = Converter(layer_structure=not args.no_layer_structure, scene=not args.no_physics_scene, comment=args.comment)
50+
converter = Converter(
51+
layer_structure=not args.no_layer_structure, scene=not args.no_physics_scene, comment=args.comment, ros_packages=args.package
52+
)
5153
if result := converter.convert(args.input_file, args.output_dir):
5254
Tf.Status(f"Created USD Asset: {result.path}")
5355
return 0
@@ -62,6 +64,15 @@ def run() -> int:
6264
return 1
6365

6466

67+
def __parse_package(value):
68+
if "=" not in value:
69+
raise RuntimeError(f"Invalid format: {value}. Expected format: package_name=path")
70+
name, path = value.split("=", 1)
71+
if not name or not path:
72+
raise RuntimeError(f"Invalid format: {value}. Expected format: package_name=path")
73+
return {"name": name, "path": path}
74+
75+
6576
def __create_parser() -> argparse.ArgumentParser:
6677
parser = argparse.ArgumentParser(
6778
description="Convert URDF files to USD format",
@@ -111,5 +122,13 @@ def __create_parser() -> argparse.ArgumentParser:
111122
default="",
112123
help="Comment to add to the USD file",
113124
)
125+
parser.add_argument(
126+
"--package",
127+
"-p",
128+
type=__parse_package,
129+
action="append",
130+
default=[],
131+
help="ROS package name and file path (e.g. --package my_robot=/home/foo/my_robot)",
132+
)
114133

115134
return parser

‎urdf_usd_converter/_impl/convert.py‎

Lines changed: 11 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
# SPDX-License-Identifier: Apache-2.0
33
import pathlib
44
import tempfile
5-
from dataclasses import dataclass
5+
from dataclasses import dataclass, field
66

77
import usdex.core
88
from pxr import Sdf, Tf, Usd, UsdGeom, UsdPhysics
@@ -28,9 +28,10 @@ class Params:
2828
layer_structure: bool = True
2929
scene: bool = True
3030
comment: str = ""
31+
ros_packages: list[dict[str, str]] = field(default_factory=list)
3132

32-
def __init__(self, layer_structure: bool = True, scene: bool = True, comment: str = ""):
33-
self.params = self.Params(layer_structure=layer_structure, scene=scene, comment=comment)
33+
def __init__(self, layer_structure: bool = True, scene: bool = True, comment: str = "", ros_packages: list[dict[str, str]] = []):
34+
self.params = self.Params(layer_structure=layer_structure, scene=scene, comment=comment, ros_packages=ros_packages)
3435

3536
def convert(self, input_file: str, output_dir: str) -> Sdf.AssetPath:
3637
"""
@@ -62,6 +63,12 @@ def convert(self, input_file: str, output_dir: str) -> Sdf.AssetPath:
6263
parser = URDFParser(input_path)
6364
parser.parse()
6465

66+
# Get the package name and path of the ROS package from the CLI arguments
67+
ros_packages = {}
68+
for package in self.params.ros_packages:
69+
if package.get("name", None) and package.get("path", None):
70+
ros_packages[package.get("name")] = package.get("path")
71+
6572
# Create the conversion data object
6673
data = ConversionData(
6774
urdf_parser=parser,
@@ -73,6 +80,7 @@ def convert(self, input_file: str, output_dir: str) -> Sdf.AssetPath:
7380
comment=self.params.comment,
7481
link_hierarchy=LinkHierarchy(parser.get_root_element()),
7582
mesh_cache=MeshCache(),
83+
ros_packages=ros_packages,
7684
)
7785

7886
# setup the main output layer (which will become an asset interface later)

‎urdf_usd_converter/_impl/data.py‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -34,3 +34,4 @@ class ConversionData:
3434
comment: str
3535
link_hierarchy: LinkHierarchy
3636
mesh_cache: MeshCache
37+
ros_packages: list[dict[str, str]]

‎urdf_usd_converter/_impl/mesh.py‎

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,7 @@
1010

1111
from .data import ConversionData, Tokens
1212
from .numpy import convert_vec3f_array
13+
from .ros_package import resolve_ros_package_paths
1314

1415
__all__ = ["convert_meshes"]
1516

@@ -37,7 +38,12 @@ def convert_meshes(data: ConversionData):
3738
for filename in mesh_names:
3839
safe_name = mesh_names[filename]["safe_name"]
3940

40-
filename = pathlib.Path(filename) if pathlib.Path(filename).is_absolute() else urdf_dir / pathlib.Path(filename)
41+
# Resolve the ROS package paths.
42+
resolved_path = resolve_ros_package_paths(filename, data) if filename.startswith("package://") else filename
43+
if resolved_path != filename:
44+
Tf.Status(f"Resolved ROS package path: {filename} -> {resolved_path}")
45+
46+
filename = pathlib.Path(resolved_path) if pathlib.Path(resolved_path).is_absolute() else urdf_dir / pathlib.Path(resolved_path)
4147
mesh_prim: Usd.Prim = usdex.core.defineXform(geo_scope, safe_name).GetPrim()
4248

4349
# If there are multiple mesh names (using file names), the meshes may have the same name but different scale values.

0 commit comments

Comments
 (0)