Skip to content

Commit 6d28bd3

Browse files
ft-labft-lab
committed
ros package support
1 parent 105f209 commit 6d28bd3

File tree

11 files changed

+365
-9
lines changed

11 files changed

+365
-9
lines changed

‎tests/data/assets/grid.png‎

666 Bytes
Loading

‎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/data/warning_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="warning_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/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: <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: <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: 75 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,76 @@ 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_load_warning_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/warning_ref_ros_package.urdf"
87+
output_dir = self.tmpDir()
88+
89+
converter = urdf_usd_converter.Converter()
90+
with usdex.test.ScopedDiagnosticChecker(
91+
self,
92+
[
93+
(Tf.TF_DIAGNOSTIC_WARNING_TYPE, ".*Invalid ROS package URI. No relative path specified: package://test_package.*"),
94+
],
95+
level=usdex.core.DiagnosticsLevel.eWarning,
96+
):
97+
converter.convert(input_path, output_dir)
98+
99+
def test_ros_packages(self):
100+
# Specify ROS package arguments as Converter constructor.
101+
102+
input_path = "tests/data/ros_packages.urdf"
103+
output_dir = self.tmpDir()
104+
105+
test_package_dir = output_dir + "/temp"
106+
test_texture_package_dir = output_dir + "/temp/textures"
107+
pathlib.Path(test_package_dir + "/assets").mkdir(parents=True, exist_ok=True)
108+
pathlib.Path(test_texture_package_dir + "/assets").mkdir(parents=True, exist_ok=True)
109+
110+
# Copy "tests/data/assets/box.stl" to test_package_dir
111+
shutil.copy("tests/data/assets/box.stl", test_package_dir + "/assets")
112+
113+
# Copy "tests/data/assets/grid.png" to test_texture_package_dir
114+
shutil.copy("tests/data/assets/grid.png", test_texture_package_dir + "/assets")
115+
116+
temp_stl_file_path = test_package_dir + "/assets/box.stl"
117+
temp_texture_file_path = test_texture_package_dir + "/assets/grid.png"
118+
self.assertTrue(pathlib.Path(temp_stl_file_path).exists())
119+
self.assertTrue(pathlib.Path(temp_texture_file_path).exists())
120+
121+
packages = [
122+
{"name": "test_package", "path": test_package_dir},
123+
{"name": "test_texture_package", "path": test_texture_package_dir},
124+
]
125+
converter = urdf_usd_converter.Converter(ros_packages=packages)
126+
asset_path = converter.convert(input_path, output_dir)
127+
self.assertIsNotNone(asset_path)
128+
self.assertTrue(pathlib.Path(asset_path.path).exists())
129+
130+
# If the URI specified in converter.convert is invalid, a warning will be displayed and processing will continue.
131+
# Therefore, this process opens the USD file to verify that the mesh has been loaded correctly.
132+
output_path = pathlib.Path(output_dir) / "ros_packages.usda"
133+
self.assertTrue(output_path.exists())
134+
135+
self.stage: Usd.Stage = Usd.Stage.Open(str(output_path))
136+
self.assertIsValidUsd(self.stage)
137+
138+
# Check geometry.
139+
default_prim = self.stage.GetDefaultPrim()
140+
geometry_scope_prim = self.stage.GetPrimAtPath(default_prim.GetPath().AppendChild("Geometry"))
141+
self.assertTrue(geometry_scope_prim.IsValid())
142+
143+
link_mesh_stl_path = geometry_scope_prim.GetPath().AppendChild("BaseLink").AppendChild("link_mesh_stl")
144+
link_stl_prim = self.stage.GetPrimAtPath(link_mesh_stl_path)
145+
self.assertTrue(link_stl_prim.IsValid())
146+
self.assertTrue(link_stl_prim.IsA(UsdGeom.Xform))
147+
148+
stl_mesh_prim = self.stage.GetPrimAtPath(link_mesh_stl_path.AppendChild("box"))
149+
self.assertTrue(stl_mesh_prim.IsValid())
150+
self.assertTrue(stl_mesh_prim.IsA(UsdGeom.Mesh))
151+
self.assertTrue(stl_mesh_prim.HasAuthoredReferences())
152+
153+
# Check material texture.
154+
# 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: 34 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -47,7 +47,12 @@ 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,
52+
scene=not args.no_physics_scene,
53+
comment=args.comment,
54+
ros_packages=args.package,
55+
)
5156
if result := converter.convert(args.input_file, args.output_dir):
5257
Tf.Status(f"Created USD Asset: {result.path}")
5358
return 0
@@ -62,6 +67,26 @@ def run() -> int:
6267
return 1
6368

6469

70+
def __parse_package(value):
71+
"""
72+
Parse the package argument.
73+
For package options, specify '<name>=<path>', such as '--package my_robot=/home/foo/my_robot'.
74+
This method converts them into a dictionary and returns it.
75+
76+
Args:
77+
value: The package argument to parse.
78+
79+
Returns:
80+
A dictionary with the package name and path.
81+
"""
82+
if "=" not in value:
83+
raise RuntimeError(f"Invalid format: {value}. Expected format: <name>=<path>")
84+
name, path = value.split("=", 1)
85+
if not name or not path:
86+
raise RuntimeError(f"Invalid format: {value}. Expected format: <name>=<path>")
87+
return {"name": name, "path": path}
88+
89+
6590
def __create_parser() -> argparse.ArgumentParser:
6691
parser = argparse.ArgumentParser(
6792
description="Convert URDF files to USD format",
@@ -111,5 +136,13 @@ def __create_parser() -> argparse.ArgumentParser:
111136
default="",
112137
help="Comment to add to the USD file",
113138
)
139+
parser.add_argument(
140+
"--package",
141+
"-p",
142+
type=__parse_package,
143+
action="append",
144+
default=[],
145+
help="ROS package name and local file path (e.g. --package my_robot=/home/foo/my_robot)",
146+
)
114147

115148
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]]

0 commit comments

Comments
 (0)