Skip to content

Commit cbb20d2

Browse files
ft-labft-lab
committed
ROS package: Automatic package resolution
1 parent cc32791 commit cbb20d2

File tree

5 files changed

+212
-41
lines changed

5 files changed

+212
-41
lines changed

‎tests/data/warning_ref_ros_package_not_exist.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_not_exist">
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 and a non-existent relative path. -->
14+
<mesh filename="package://test_package/not_exist/not_exist_box.stl"/>
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: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -138,6 +138,28 @@ def test_conversion_exception_non_verbose(self):
138138
):
139139
self.assertEqual(run(), 1, "Expected non-zero exit code when conversion raises exception")
140140

141+
def test_conversion_warning_verifying_elements(self):
142+
robot = "tests/data/verifying_elements.urdf"
143+
robot_name = pathlib.Path(robot).stem
144+
output_dir = self.tmpDir()
145+
with (
146+
patch("sys.argv", ["urdf_usd_converter", robot, str(output_dir)]),
147+
usdex.test.ScopedDiagnosticChecker(
148+
self,
149+
[
150+
(Tf.TF_DIAGNOSTIC_WARNING_TYPE, ".*Transmission is not supported.*"),
151+
(Tf.TF_DIAGNOSTIC_WARNING_TYPE, ".*Gazebo is not supported.*"),
152+
(Tf.TF_DIAGNOSTIC_WARNING_TYPE, ".*Calibration is not supported.*"),
153+
(Tf.TF_DIAGNOSTIC_WARNING_TYPE, ".*Dynamics is not supported.*"),
154+
(Tf.TF_DIAGNOSTIC_WARNING_TYPE, ".*Mimic is not supported.*"),
155+
(Tf.TF_DIAGNOSTIC_WARNING_TYPE, ".*Safety controller is not supported.*"),
156+
],
157+
level=usdex.core.DiagnosticsLevel.eWarning,
158+
),
159+
):
160+
self.assertEqual(run(), 0, "Expected non-zero exit code for invalid input")
161+
self.assertTrue((pathlib.Path(self.tmpDir()) / f"{robot_name}.usda").exists())
162+
141163
def test_conversion_exception_verbose(self):
142164
# Test exception handling when verbose=True (should re-raise)
143165
robot = "tests/data/simple_box.urdf"
@@ -196,6 +218,22 @@ def test_conversion_warning_multiple_ros_packages_invalid(self):
196218
self.assertEqual(run(), 0, "Expected non-zero exit code for invalid input")
197219
self.assertTrue((pathlib.Path(self.tmpDir()) / f"{robot_name}.usda").exists())
198220

221+
def test_conversion_warning_ros_package_not_exist(self):
222+
robot = "tests/data/warning_ref_ros_package_not_exist.urdf"
223+
robot_name = pathlib.Path(robot).stem
224+
output_dir = self.tmpDir()
225+
226+
with (
227+
patch("sys.argv", ["urdf_usd_converter", robot, str(output_dir)]),
228+
usdex.test.ScopedDiagnosticChecker(
229+
self,
230+
[(Tf.TF_DIAGNOSTIC_WARNING_TYPE, ".*No such file or directory:.*")],
231+
level=usdex.core.DiagnosticsLevel.eWarning,
232+
),
233+
):
234+
self.assertEqual(run(), 0, "Expected non-zero exit code for invalid input")
235+
self.assertTrue((pathlib.Path(self.tmpDir()) / f"{robot_name}.usda").exists())
236+
199237
def test_conversion_warning_https(self):
200238
robot = "tests/data/mesh_https.urdf"
201239
robot_name = pathlib.Path(robot).stem

‎tests/testROSPackagesCli.py‎

Lines changed: 64 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -12,38 +12,20 @@
1212

1313
class TestROSPackagesCli(ConverterTestCase):
1414
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".
15+
"""
16+
If the `package` argument is not specified in `converter.convert`.
17+
In this case, if the mesh or texture URI specifies "package://PackageName/foo/test.png",
18+
and the relative path "foo/test.png" exists, PackageName="" is assigned and automatically resolved.
19+
"""
1820
input_path = "tests/data/ros_packages.urdf"
1921
output_dir = self.tmpDir()
2022

2123
with patch("sys.argv", ["urdf_usd_converter", input_path, output_dir]):
2224
self.assertEqual(run(), 0, f"Failed to convert {input_path}")
2325

26+
# Check the USD file after converting ros_packages.urdf.
2427
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.
28+
self.check_usd_converted_from_urdf(output_path)
4729

4830
def test_specify_ros_package_names(self):
4931
"""
@@ -82,10 +64,65 @@ def test_specify_ros_package_names(self):
8264
):
8365
self.assertEqual(run(), 0, f"Failed to convert {input_path}")
8466

67+
# Check the USD file after converting ros_packages.urdf.
8568
output_path = pathlib.Path(output_dir) / "ros_packages.usda"
86-
self.assertTrue(output_path.exists())
69+
self.check_usd_converted_from_urdf(output_path)
70+
71+
def test_do_not_specify_ros_package_with_relative_path(self):
72+
"""
73+
If the `package` argument is not specified in `converter.convert`.
74+
ROS package name resolution is performed automatically.
75+
76+
Search for each mesh and texture from the relative path one directory up from the current directory,
77+
starting from `ros_packages.urdf` within the following directory structure.
78+
79+
[temp]
80+
[urdf]
81+
ros_packages.urdf
82+
[assets]
83+
box.stl
84+
grid.png
85+
"""
86+
temp_path = pathlib.Path(self.tmpDir())
87+
urdf_dir = temp_path / "urdf"
88+
mesh_dir = temp_path / "assets"
89+
texture_dir = temp_path / "assets"
90+
output_dir = temp_path / "output"
91+
input_path = urdf_dir / "ros_packages.urdf"
92+
93+
urdf_dir.mkdir(parents=True, exist_ok=True)
94+
mesh_dir.mkdir(parents=True, exist_ok=True)
95+
texture_dir.mkdir(parents=True, exist_ok=True)
96+
output_dir.mkdir(parents=True, exist_ok=True)
97+
98+
shutil.copy("tests/data/ros_packages.urdf", urdf_dir)
99+
shutil.copy("tests/data/assets/box.stl", mesh_dir)
100+
shutil.copy("tests/data/assets/grid.png", texture_dir)
101+
102+
with patch(
103+
"sys.argv",
104+
[
105+
"urdf_usd_converter",
106+
str(input_path),
107+
str(output_dir),
108+
],
109+
):
110+
self.assertEqual(run(), 0, f"Failed to convert {input_path}")
111+
112+
# Check the USD file after converting ros_packages.urdf.
113+
output_path = output_dir / "ros_packages.usda"
114+
self.check_usd_converted_from_urdf(output_path)
115+
116+
def check_usd_converted_from_urdf(self, usd_path: pathlib.Path):
117+
"""
118+
Perform checks on the USD file after converting ros_packages.urdf.
119+
120+
Args:
121+
usd_path: The path to the USD file.
122+
"""
123+
self.assertTrue(usd_path.exists())
87124

88-
self.stage: Usd.Stage = Usd.Stage.Open(str(output_path))
125+
self.stage: Usd.Stage = Usd.Stage.Open(str(usd_path))
89126
self.assertIsValidUsd(self.stage)
90127

91128
# Check geometry.

‎urdf_usd_converter/_impl/convert.py‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,7 @@
1414
from .material import convert_materials
1515
from .mesh import convert_meshes
1616
from .mesh_cache import MeshCache
17+
from .ros_package import search_ros_packages
1718
from .scene import convert_scene
1819
from .urdf_parser.elements import ElementRobot
1920
from .urdf_parser.parser import URDFParser
@@ -69,6 +70,13 @@ def convert(self, input_file: str, output_dir: str) -> Sdf.AssetPath:
6970
if package.get("name", None) and package.get("path", None):
7071
ros_packages[package.get("name")] = package.get("path")
7172

73+
# Search for ROS packages that reference meshes and material textures within URDF files.
74+
# If the package name is not in the ros_packages dictionary, add it.
75+
ros_packages_in_urdf = search_ros_packages(parser)
76+
for package_name in ros_packages_in_urdf:
77+
if package_name not in ros_packages:
78+
ros_packages[package_name] = ros_packages_in_urdf[package_name]
79+
7280
# Create the conversion data object
7381
data = ConversionData(
7482
urdf_parser=parser,

‎urdf_usd_converter/_impl/ros_package.py‎

Lines changed: 78 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,14 @@
11
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
22
# SPDX-License-Identifier: Apache-2.0
3+
import os
34
import pathlib
45

56
from pxr import Tf
67

78
from .data import ConversionData
9+
from .urdf_parser.parser import URDFParser
810

9-
__all__ = ["resolve_ros_package_paths"]
11+
__all__ = ["resolve_ros_package_paths", "search_ros_packages"]
1012

1113

1214
def resolve_ros_package_paths(uri: str, data: ConversionData) -> pathlib.Path:
@@ -21,22 +23,11 @@ def resolve_ros_package_paths(uri: str, data: ConversionData) -> pathlib.Path:
2123
The resolved path.
2224
"""
2325
if uri.startswith("package://"):
24-
# Remove "package://" from the URI.
25-
_, _, path_with_package_name = uri.partition("package://")
26-
27-
# [package name]/[relative path]
28-
split_dir = pathlib.Path(path_with_package_name)
29-
30-
split_dirs = split_dir.parts
31-
if len(split_dirs) < 2:
26+
package_name, relative_path = _split_package_name_and_path(uri)
27+
if not package_name or not relative_path:
3228
Tf.Warn(f"Invalid ROS package URI. No relative path specified: {uri}")
33-
34-
# The result after removing "package://[package name]" is an empty string.
3529
return pathlib.Path()
3630

37-
package_name = split_dirs[0]
38-
relative_path = pathlib.Path(*split_dirs[1:])
39-
4031
package_path = data.ros_packages.get(package_name, None)
4132
if package_path:
4233
base_path = pathlib.Path(package_path)
@@ -45,3 +36,76 @@ def resolve_ros_package_paths(uri: str, data: ConversionData) -> pathlib.Path:
4536
return relative_path
4637

4738
return pathlib.Path(uri)
39+
40+
41+
def search_ros_packages(urdf_parser: URDFParser) -> dict[str]:
42+
"""
43+
Automatically searches for packages based on the paths to meshes and material textures found in the urdf file.
44+
45+
Args:
46+
urdf_parser: The URDF parser.
47+
48+
Returns:
49+
A dictionary with the package name and path.
50+
"""
51+
meshes = urdf_parser.get_meshes()
52+
materials = urdf_parser.get_materials()
53+
54+
# Store references beginning with "package://".
55+
package_filenames = []
56+
for mesh in meshes:
57+
filename = mesh[0]
58+
if filename and filename.startswith("package://"):
59+
package_filenames.append(filename)
60+
61+
for material in materials:
62+
filename = material[2]
63+
if filename and filename.startswith("package://"):
64+
package_filenames.append(filename)
65+
66+
ros_packages = {}
67+
68+
for filename in package_filenames:
69+
package_name, relative_path = _split_package_name_and_path(filename)
70+
if package_name and relative_path and package_name not in ros_packages:
71+
# URDF file directory
72+
urdf_dir = urdf_parser.input_file.parent
73+
74+
# Traverse up from urdf_dir to parent directories.
75+
# if the file path "urdf_dir / relative_path" exists, store it in ros_packages.
76+
while urdf_dir != pathlib.Path():
77+
file_path = urdf_dir / relative_path
78+
if file_path.exists():
79+
# Relative path from the path containing the urdf file.
80+
ros_packages[package_name] = pathlib.Path(os.path.relpath(urdf_dir, urdf_parser.input_file.parent))
81+
break
82+
urdf_dir = urdf_dir.parent
83+
84+
return ros_packages
85+
86+
87+
def _split_package_name_and_path(uri: str) -> tuple[str, pathlib.Path]:
88+
"""
89+
Split the package name and path from the URI.
90+
91+
Args:
92+
uri: The URI to split.
93+
94+
Returns:
95+
The package name and path.
96+
None if the URI is invalid.
97+
"""
98+
# Remove "package://" from the URI.
99+
_, _, path_with_package_name = uri.partition("package://")
100+
101+
# [package name]/[relative path]
102+
split_dir = pathlib.Path(path_with_package_name)
103+
104+
split_dirs = split_dir.parts
105+
if len(split_dirs) < 2:
106+
# The result after removing "package://[package name]" is an empty string.
107+
return None, None
108+
109+
package_name = split_dirs[0]
110+
relative_path = pathlib.Path(*split_dirs[1:])
111+
return package_name, relative_path

0 commit comments

Comments
 (0)