diff --git a/.cruft.json b/.cruft.json index 58facfc..3e06d3e 100644 --- a/.cruft.json +++ b/.cruft.json @@ -1,6 +1,6 @@ { "template": "https://github.com/UrbanMachine/create-ros-app.git", - "commit": "7697b9d1edf4d2b27d2bc9d3095fa893a8d92497", + "commit": "6a85fd934e7c6297d5fd717c545fad443cc4dfcf", "checkout": null, "context": { "cookiecutter": { diff --git a/.github/workflows/test.yaml b/.github/workflows/test.yaml index 387a4f3..25b43ca 100644 --- a/.github/workflows/test.yaml +++ b/.github/workflows/test.yaml @@ -89,7 +89,7 @@ jobs: defaults: run: shell: bash - timeout-minutes: 40 + timeout-minutes: 15 container: image: ${{needs.build-image.outputs.tagged_image}} credentials: diff --git a/docker/Dockerfile b/docker/Dockerfile index 6e46e0a..6273fd3 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -41,6 +41,7 @@ RUN --mount=type=cache,target="${APT_CACHE}" \ # ROS2 ros-${ROS2_DISTRO}-ros-base \ ros-${ROS2_DISTRO}-rosbridge-suite \ + ros-${ROS2_DISTRO}-rmw-cyclonedds-cpp \ # Build tools build-essential \ git \ @@ -101,13 +102,8 @@ COPY pkgs/node_helpers/pyproject.toml pkgs/node_helpers/pyproject.toml ########## Add Git ROS2 Packages ########## NOTE TO TEMPLATE USERS: If you need to depend on a package that is not in the ROS2 distro, you can add it here WORKDIR /ros-git-deps/ -RUN --mount=type=cache,target="${PIP_CACHE}" \ - --mount=type=cache,target="${APT_CACHE}" \ - install-ros-package-from-git \ - https://github.com/UrbanMachine/node_helpers.git main pkgs && \ ##################### Add your packages here! ########### install-ros-package-from-git {URL} {BRANCH} {PKGS PATH IN REPO} - echo "Done installing ROS2 packages from git" ###################################################################### # Install Poetry dependencies for each package in this repo diff --git a/docs/launching.rst b/docs/launching.rst index d6a7228..646119c 100644 --- a/docs/launching.rst +++ b/docs/launching.rst @@ -1,7 +1,7 @@ Launching ========= -The `node_helpers.launching` module provides utility functions and classes to streamline the management of ROS launch files. +The `node_helpers.launching` module provides utility functions and classes to streamline the management of ROS launch files and URDF configurations. This module is particularly useful for handling dynamic node swapping and ensuring file existence for launch operations. Core Features ------------- @@ -45,6 +45,9 @@ Core Features config_file = launching.required_file("/path/to/config.yaml") +3. **URDF Manipulation**: + For information on urdf launching, look into at the ``urdfs`` docs. + Error Handling and Validation ----------------------------- diff --git a/docs/urdfs.rst b/docs/urdfs.rst new file mode 100644 index 0000000..653314b --- /dev/null +++ b/docs/urdfs.rst @@ -0,0 +1,130 @@ +URDF Utilities +============== + +There are several URDF tools in `node_helpers` for launching, validating, and testing URDF-based systems, providing a standardized approach to handling URDFs in robotics applications. + +Overview +-------- + +The module includes the following components: +1. **node_helpers.launching.URDFModuleNodeFactory**: Streamlines creation of `joint_state_publisher` and `robot_state_publisher` for URDFs in launch files. +2. **node_helpers.urdfs.URDFConstant**: Provides consistent access to URDF frames and joints, with validation tools. This is key for accessing 'tf' frames and 'joints' in a standardized way, without having random string constants sprinkled around your codebase. +3. **node_helpers.testing.URDFModuleFixture**: Facilitates launching URDF modules for integration tests. + +URDFConstant +------------ + +The `URDFConstants` class provides a structured way to reference and validate URDF elements, such as joints and frames. It ensures URDF correctness and avoids duplicate names or missing elements. + +**Features**: + +- Load multiple URDF's but refer to them as a single module in code. +- Prepend namespaces to avoid conflicts. +- Validate that joints and frames exist in the URDF. +- Dynamically adjust URDFs with namespaces. + +**Example**: + +Below, we create the concept of a "BigBird" robot, which consists of two URDFs. +We then, at the bottom, create a `BigBirdURDF` object that encapsulates the URDFs and provides access to the joints and frames. + +The BigBirdJoint and BigBirdFrames classes define the joints and frames in the URDFs, +and refer to real URDF elements by their names, prepended with `bird_gantry` or `bird_base` +to point back to what URDF file they came from. The `urdf_paths` parameter in the `URDFConstants` constructor +specifies what URDF the prepended names refer to. + +.. code-block:: python + + from typing import NamedTuple + + from urdf_data.urdf_constants import URDFConstants + + + class BigBirdJoints(NamedTuple): + X: str = "bird_gantry.xaxis" + Y: str = "bird_gantry.yaxis" + Z: str = "bird_gantry.zaxis" + PAN: str = "bird_gantry.waxis" + + + class BigBirdFrames(NamedTuple): + BASE_LINK: str = "bird_base.gantry_base_link" + + X_AXIS_ORIGIN: str = "bird_gantry.xaxis_parent_datum" + X_AXIS_CURRENT: str = "bird_gantry.gantry_xlink" + + Y_AXIS_ORIGIN: str = "bird_gantry.yaxis_parent_datum" + Y_AXIS_CURRENT: str = "bird_gantry.gantry_ylink" + + Z_AXIS_ORIGIN: str = "bird_gantry.zaxis_parent_datum" + Z_AXIS_CURRENT: str = "bird_gantry.gantry_zlink" + + PAN_ORIGIN: str = "bird_gantry.waxis_parent_datum" + PAN_CURRENT: str = "bird_gantry.gantry_wlink" + + + TOOL_TIP: str = "bird.grasp_point" + + + BigBirdURDF = URDFConstants[BigBirdJoints, BigBirdFrames]( + registration_name="bird_robot", + urdf_paths=[ + ("bird_base", "path/to/bird_base/robot.urdf"), + ("bird_gantry", "path/to/bird_gantry/robot.urdf"), + joints=BigBirdJoints(), + frames=BigBirdFrames(), + ) + +Note that an example URDF constant can be found in ``pkgs/node_helpers_test/integration/urdfs/example_urdf_constants.py`` + +URDFModule +---------- + +The `URDFModuleNodeFactory` simplifies launching URDF nodes by generating `robot_state_publisher` and `joint_state_publisher` nodes for each URDF file. It applies namespaces to avoid collisions and ensures URDFs are properly loaded and validated. + +In the below example, a ``joint_state_publisher`` will be created under the ``/big_bird_left/`` namespace, +and multiple ``robot_state_publishers`` will be created for each URDF file in the `BigBirdURDF` constant. +For example, one will live under ``/big_bird_left/urdf_0/`` and the other under ``/big_bird_left/urdf_1/``. + +They will all publish to the same ``joint_state_publisher`` under the ``/big_bird_left/`` namespace. + +**Example**: + +.. code-block:: python + + from node_helpers.urdfs.urdf_module_launching import URDFModuleNodeFactory + + parameters = URDFModuleNodeFactory.Parameters( + namespace: "big_bird_left", + urdf_constant_name: "BigBirdURDF", + apply_namespace_to_urdf: True, + ) + factory = URDFModuleNodeFactory(parameters) + nodes = factory.create_nodes() # these nodes can be added to a launch description + + +URDFModuleFixture +------------------ + +The ``URDFModuleFixture`` class is a pytest fixture utility for setting up URDF-based tests. It +will launch the URDF module (and all it's `robot_state_publisher`s and `joint_state_publisher`, +and ensure that all TF frames are published correctly before yielding the fixture. + +**Example**: + +.. code-block:: python + + from node_helpers.urdfs.urdf_module_fixture import URDFModuleFixture + + @pytest.fixture() + def big_bird_urdf_module() -> Generator[URDFModuleFixture, None, None]: + yield from URDFModuleFixture.set_up( + URDFModuleNodeFactory.Parameters( + namespace="big_bird_top", urdf_constant_name=BigBirdURDF.registration_name + ) + ) + + +A full example of how to integration test URDFs can be found under ``pkgs/node_helpers/node_helpers_test/integration/urdfs/test_forklift.py`` + +Note that ``node_helpers`` provides a helpful test URDF in ``pkgs/node_helpers/sample_urdfs/forklift/robot.urdf`` \ No newline at end of file diff --git a/pkgs/node_helpers/node_helpers/launching/__init__.py b/pkgs/node_helpers/node_helpers/launching/__init__.py index 0687eb2..5b6d07a 100644 --- a/pkgs/node_helpers/node_helpers/launching/__init__.py +++ b/pkgs/node_helpers/node_helpers/launching/__init__.py @@ -5,3 +5,4 @@ SwappableNode, apply_node_swaps, ) +from .urdf_module_launching import URDFModuleNodeFactory diff --git a/pkgs/node_helpers/node_helpers/launching/urdf_module_launching.py b/pkgs/node_helpers/node_helpers/launching/urdf_module_launching.py new file mode 100644 index 0000000..624b66e --- /dev/null +++ b/pkgs/node_helpers/node_helpers/launching/urdf_module_launching.py @@ -0,0 +1,124 @@ +from functools import reduce +from operator import iconcat +from typing import Any + +from launch_ros.actions import Node +from pydantic import BaseModel + +from node_helpers.urdfs.urdf_constants import URDFConstants + + +class URDFModuleNodeFactory: + """A helper object for creating nodes for a urdf module. + + This class takes a URDFConstant and a namespace, and then for each child URDF will: + 1) Spin up a robot state publisher under the namespace '/{namespace}/urdf_{idx}' + 2) Spin up a joint state publisher under the above namespace. + + If there are 3 URDFs in the URDFConstant, there will be 6 nodes total launched. + Potential future optimizations include only spinning up `joint_state_publishers` if + there happen to be any joints in the URDF being spun up. + """ + + class Parameters(BaseModel): + namespace: str + """The namespace under which under which joint state publishers and robot state + publishers will live, a la /{namespace}/urdf_# """ + + urdf_constant_name: str + """The chosen URDFConstant.registration_name to spin up. In configuration, you + can reference these as strings, using the name attribute to load a specific + instance of a URDFConstant.""" + + apply_namespace_to_urdf: bool = True + """If True, the node namespace will be prepended to the URDF frames. This is + the behaviour used by hardware modules. Set this to False if your URDF is not + part of a hardware module.""" + + def __init__(self, parameters: Parameters): + self._params = parameters + + # Create the URDFConstant, with a namespace optionally prepended + base_urdf_constants = URDFConstants[Any, Any].get_registered_instance( + self._params.urdf_constant_name + ) + self.urdf_constants = ( + base_urdf_constants.with_namespace(self._params.namespace) + if self._params.apply_namespace_to_urdf + else base_urdf_constants + ) + + def create_nodes(self) -> list[Node]: + """Create the nodes required to load and visualize each specified urdf path""" + urdf_strs = self.urdf_constants.load_urdfs() + + urdf_nodes = [ + [ + self.create_robot_state_publisher( + namespace=self._params.namespace, + urdf_index=urdf_index, + urdf_str=urdf_str, + ), + self.create_joint_state_publisher( + namespace=self._params.namespace, + urdf_index=urdf_index, + ), + ] + for urdf_index, urdf_str in enumerate(urdf_strs) + ] + return reduce(iconcat, urdf_nodes, []) + + @staticmethod + def create_joint_state_publisher(namespace: str, urdf_index: int) -> Node: + return Node( + package="joint_state_publisher", + namespace=URDFModuleNodeFactory.urdf_namespace(namespace, urdf_index), + executable="joint_state_publisher", + parameters=[ + { + "source_list": [ + f"/{namespace}/desired_joint_states", + ] + } + ], + ) + + @staticmethod + def create_robot_state_publisher( + namespace: str, + urdf_index: int, + urdf_str: str, + ) -> Node: + """Create a robot state publisher using the hardware module standards + :param namespace: The namespace under which to create the urdf namespace + :param urdf_index: The index of this urdf within the parent namespace + :param urdf_str: The urdf as a string to pass to the robot_state_publisher + :return: The robot state publisher node. + """ + + return Node( + package="robot_state_publisher", + namespace=URDFModuleNodeFactory.urdf_namespace(namespace, urdf_index), + executable="robot_state_publisher", + parameters=[ + {"robot_description": urdf_str}, + ], + ) + + @property + def urdf_namespaces(self) -> list[str]: + """Returns the namespaces under which URDFs are stored, for rviz remapping.""" + return [ + self.urdf_namespace(self._params.namespace, urdf_id) + for urdf_id in range(len(self.urdf_constants)) + ] + + @staticmethod + def urdf_namespace(namespace: str, urdf_index: int) -> str: + """A helper for creating the namespace for a given urdf in a module + + :param namespace: The parent namespace that will own one or more URDFs + :param urdf_index: The index of this particular urdf + :return: The formatted namespace string + """ + return f"{namespace}/urdf_{urdf_index}" diff --git a/pkgs/node_helpers/node_helpers/testing/__init__.py b/pkgs/node_helpers/node_helpers/testing/__init__.py index e805974..ae9aa9c 100644 --- a/pkgs/node_helpers/node_helpers/testing/__init__.py +++ b/pkgs/node_helpers/node_helpers/testing/__init__.py @@ -20,6 +20,15 @@ from .resources import MessageResource, NumpyResource, resource_path from .threads import ContextThread, DynamicContextThread, get_unclosed_threads from .transforms import set_up_static_transforms +from .urdf_frame_validation import ( + validate_coincident_transforms, + validate_expected_rotation, +) +from .urdf_module_fixture import ( + TFClient, + URDFFixtureSetupFailed, + URDFModuleFixture, +) faulthandler.enable() @@ -42,4 +51,9 @@ "set_up_node", "rclpy_context", "run_and_cancel_task", + "URDFModuleFixture", + "TFClient", + "URDFFixtureSetupFailed", + "validate_coincident_transforms", + "validate_expected_rotation", ] diff --git a/pkgs/node_helpers/node_helpers/testing/urdf_frame_validation.py b/pkgs/node_helpers/node_helpers/testing/urdf_frame_validation.py new file mode 100644 index 0000000..ca97727 --- /dev/null +++ b/pkgs/node_helpers/node_helpers/testing/urdf_frame_validation.py @@ -0,0 +1,95 @@ +import numpy as np +from rclpy.duration import Duration +from rclpy.time import Time + +from node_helpers.ros2_numpy import numpify +from node_helpers.testing.urdf_module_fixture import TFClient, URDFModuleFixture + +_TEST_TF_TIMEOUT = Duration(seconds=10) + + +def validate_coincident_transforms( + urdf_module: URDFModuleFixture, + tf_client: TFClient, + joint_origin: str, + joint_current: str, + same_position: bool, + same_orientation: bool, +) -> None: + """Test that joints that are expected to be coincident while at 0, are so. + This means both position and orientation are equivalent. + + :param urdf_module: The URDFModuleFixture instance to use for the test. + :param tf_client: The TFClient instance to use for the test. + :param joint_origin: The joint we transform from. + :param joint_current: The joint we transform to. + :param same_position: Whether the position of the frames should be the same. + :param same_orientation: Whether the orientation of the frames should be the same + """ + assert joint_origin != joint_current + assert same_position or same_orientation + transform = tf_client.buffer.lookup_transform( + joint_origin, joint_current, Time(), timeout=_TEST_TF_TIMEOUT + ) + position = np.round(numpify(transform.transform.translation), decimals=5) + rotation = np.round(numpify(transform.transform.rotation), decimals=5) + + if same_position: + error = ( + f"The frame '{joint_origin}' is expected to be coincident with " + f"'{joint_current}' when homed! Current offset: {position}, in URDF " + f"'{urdf_module.parameters.urdf_constant_name}'" + ) + assert np.isclose(position, (0, 0, 0)).all(), error + + if same_orientation: + error = ( + f"The frame '{joint_origin}' is expected to be coincident with " + f"'{joint_current}' when homed! Current rotation quaternion: {rotation}, " + f"in URDF '{urdf_module.parameters.urdf_constant_name}'" + ) + assert np.isclose(rotation, (0, 0, 0, 1)).all(), error + + +def validate_expected_rotation( + urdf_module: URDFModuleFixture, + tf_client: TFClient, + from_frame: str, + to_frame: str, + start_point: tuple[float, float, float], + expected_end_point: tuple[float, float, float], +) -> None: + """Test that two frames have a specific rotation between them. To do this, the test + places a point at 'start_point' and then applies the given rotation between the + frames, and check if it ends up at the 'expected_end_point'. + + :param urdf_module: The URDFModuleFixture instance to use for the test. + :param tf_client: The TFClient instance to use for the test. + :param from_frame: The frame we transform from. + :param to_frame: The frame we transform to. + :param start_point: The point being transformed. + :param expected_end_point: The point we expect after transformation. + :raises ImportError: If the 'scipy' package is not installed. + """ + try: + from scipy.spatial.transform import Rotation + except ImportError as e: + raise ImportError( + "The 'scipy' package is required for this test. Please install it using " + "the following command: 'pip install scipy'." + "In the future we might replace this dependency with ." + ) from e + + transform = tf_client.buffer.lookup_transform( + to_frame, from_frame, Time(), timeout=_TEST_TF_TIMEOUT + ) + rotation = Rotation.from_quat(numpify(transform.transform.rotation)) + + point_after_transformation = np.round(rotation.apply(np.array(start_point)), 4) + + error = ( + f"After applying the rotation between '{from_frame}' and '{to_frame}', the " + f"point {start_point} was expected to end up at {expected_end_point}, but " + f"instead ended up at {point_after_transformation}." + ) + assert np.isclose(point_after_transformation, expected_end_point).all(), error diff --git a/pkgs/node_helpers/node_helpers/testing/urdf_module_fixture.py b/pkgs/node_helpers/node_helpers/testing/urdf_module_fixture.py new file mode 100644 index 0000000..7dcab52 --- /dev/null +++ b/pkgs/node_helpers/node_helpers/testing/urdf_module_fixture.py @@ -0,0 +1,175 @@ +from collections.abc import Generator +from contextlib import contextmanager +from dataclasses import dataclass +from itertools import chain +from typing import Any, TypeVar + +import tf2_py +from geometry_msgs.msg import TransformStamped +from rclpy.duration import Duration +from rclpy.node import Node +from rclpy.time import Time +from std_msgs.msg import Header +from tf2_ros import Buffer, TransformListener + +from node_helpers.launching.urdf_module_launching import URDFModuleNodeFactory +from node_helpers.nodes import HelpfulNode +from node_helpers.testing.nodes import ( + set_up_external_nodes_from_launchnode, + set_up_node, +) +from node_helpers.tf import ConstantStaticTransformBroadcaster + + +class URDFFixtureSetupFailed(Exception): + """ + Once upon a time, there was an era of about 6 months where tests that relied on + URDFs would occasionally fail with the error "tf2.LookupException". This exception + initially would happen somewhere inside the test, and later we started checking if + the frames were transformable before the test started. This moved the flakiness to + the setup of the test, where the frames would sometimes not be transformable. + + Why. WHY. WHY are frames sometimes, rarely, not transformable? We don't know. All + we know is that sometimes in tests the Joint state publisher and Robot state + publisher somehow don't quite work the way they should, and don't publish frames, + and thus the TF buffer also doesn't have the frames. + + So, to fix this issue, we added a retry mechanism to URDFModuleFixture.set_up, so + it will retry up to 3 times to set up the URDFModuleFixture. If it still fails, it + will raise this exception, which will cause the test to fail. + + Hopefully, this problem will go away in future ROS releases....... + """ + + +class TFClient(Node): + def __init__(self, **kwargs: Any) -> None: + super().__init__("tf_node", **kwargs) + self.buffer = Buffer(node=self) + self.tf_listener = TransformListener(self.buffer, self) + + +@dataclass +class URDFModuleFixture: + """A helper for launching a urdf module for use as a pytest fixture""" + + parameters: URDFModuleNodeFactory.Parameters + """Useful for interrogating what parameters the fixture is using in a test""" + + @classmethod + def set_up( + cls, + urdf_factory_params: URDFModuleNodeFactory.Parameters, + static_transforms: list[tuple[str, str]] | None = None, + retries: int = 3, + ) -> Generator["URDFModuleFixture", None, None]: + """Sets up the necessary nodes for test interaction with a URDF. + + :param urdf_factory_params: The parameters for the URDFModuleNodeFactory + :param static_transforms: Static transforms to apply, as a list of + (parent, child) frames. All transforms will be identity transforms. + This feature is used to "glue" parts of this URDF to other existing + TF's in the system. + :param retries: The number of retries left. Default is 3. + :yields: The module with all nodes started + :raises URDFFixtureSetupFailed: If the setup fails after all retries + """ + + # Create various robot_state_publishers and joint_state_publishers + urdf_module_factory = URDFModuleNodeFactory(parameters=urdf_factory_params) + urdf_launch_nodes = urdf_module_factory.create_nodes() + + # Publish static transforms as configured by the URDF module + static_transforms = static_transforms or [] + transform_node_generator = set_up_node( + _TransformBroadcasterNode, + namespace=urdf_factory_params.namespace, + node_name="transform_broadcaster", + multi_threaded=True, + ) + + # Spin up the joint & robot state publishers, and validate that the frames are + # available in the TF tree. If they are not, retry up to 3 times to make up for + # the flakiness of the robot_state_publisher and joint_state_publisher. + urdf_nodes_generator = set_up_external_nodes_from_launchnode(urdf_launch_nodes) + expected_frames = list( + urdf_module_factory.urdf_constants.frames._asdict().values() + ) + list(chain.from_iterable(static_transforms)) + + try: + with ( + _generator_context_manager(transform_node_generator) as transform_node, + _generator_context_manager(urdf_nodes_generator), + ): + transform_node.add_static_transforms(static_transforms) + + # Wait until every frame in the URDFConstants and the static_transforms + # has been published in the TF tree + transform_node.wait_for_frames(expected_frames) + + yield URDFModuleFixture(urdf_factory_params) + except URDFFixtureSetupFailed: + if retries == 0: + raise + yield from cls.set_up( + urdf_factory_params=urdf_factory_params, + static_transforms=static_transforms, + retries=retries - 1, + ) + + +class _TransformBroadcasterNode(HelpfulNode): + """A basic node to fulfill some of the TF publishing and listening needs of the + URDFModuleFixture. + + It handles: + - Publishing basic static transforms + - Listening to the TF buffer + """ + + def __init__(self, **kwargs: Any): + super().__init__("transform_broadcaster", **kwargs) + self.tf_buffer = Buffer(node=self) + self._listener = TransformListener(self.tf_buffer, self) + + def wait_for_frames(self, frames: list[str]) -> None: + """Waits until all of the frames can be accessed from every other frame + and connected in the TF Tree + :param frames: the list of frames to wait for + :raises URDFFixtureSetupFailed: If the frames are not transformable + """ + for a_frame in frames: + for b_frame in frames: + try: + self.tf_buffer.lookup_transform( + a_frame, b_frame, Time(), Duration(seconds=10) + ) + except (tf2_py.TransformException, tf2_py.ConnectivityException) as e: + raise URDFFixtureSetupFailed( + f"Could not find a transform between {a_frame} and {b_frame}. " + "Maybe you need to set the static_transforms parameter in the " + "URDFModuleFixture.set_up call, or maybe this is a flaky " + "test failure." + ) from e + + def add_static_transforms(self, static_transforms: list[tuple[str, str]]) -> None: + for parent, child in static_transforms: + ConstantStaticTransformBroadcaster( + self, + TransformStamped(header=Header(frame_id=parent), child_frame_id=child), + publish_seconds=0.1, + ) + + +T = TypeVar("T") + + +@contextmanager +def _generator_context_manager( + generator: Generator[T, None, None], +) -> Generator[T, None, None]: + """Wraps the generator in a context manager, so the generator is always emptied""" + try: + yield next(generator) + finally: + tuple(generator) diff --git a/pkgs/node_helpers/node_helpers/urdfs/README.md b/pkgs/node_helpers/node_helpers/urdfs/README.md new file mode 100644 index 0000000..754bd74 --- /dev/null +++ b/pkgs/node_helpers/node_helpers/urdfs/README.md @@ -0,0 +1,13 @@ +# node_helpers.urdfs + +The `node_helpers.urdfs` module provides tools to streamline working with URDFs in ROS2, enabling efficient URDF handling for launch files, testing, and runtime validation. + +### Features +- **URDFConstant**: Facilitates URDF validation and consistent referencing of joints and frames in code. + +There are other URDF utilities in `node_helpers` in other modules, such as +- `node_helpers.launching.URDFModuleNodeFactory`: Automates the creation of launch file nodes for URDFs, including robot and joint state publishers. +- `node_helpers.testing.URDFModuleFixture`: Simplifies URDF testing by providing a fixture for loading URDFs and creating robot state publishers. +- `Helper Functions`: Additional utilities for namespace application, path fixing, and frame validation. + +For detailed usage, examples, and API reference, see the [comprehensive documentation](../../../../docs/urdfs.rst). diff --git a/pkgs/node_helpers/node_helpers/urdfs/__init__.py b/pkgs/node_helpers/node_helpers/urdfs/__init__.py new file mode 100644 index 0000000..0fca6df --- /dev/null +++ b/pkgs/node_helpers/node_helpers/urdfs/__init__.py @@ -0,0 +1 @@ +from .urdf_constants import URDFConstants, URDFValidationError diff --git a/pkgs/node_helpers/node_helpers/urdfs/loading.py b/pkgs/node_helpers/node_helpers/urdfs/loading.py new file mode 100644 index 0000000..e75ac09 --- /dev/null +++ b/pkgs/node_helpers/node_helpers/urdfs/loading.py @@ -0,0 +1,137 @@ +from pathlib import Path +from typing import cast +from xml.etree import ElementTree + +from ament_index_python import get_package_share_directory + +from node_helpers.launching.files import required_file + +NAMESPACE_FMT = "{namespace}.{name}" +_JOINT_TAG = "joint" +_LINK_TAG = "link" +_PARENT_TAG = "parent" +_CHILD_TAG = "child" +_NAME_KEY = "name" +_LINK_KEY = "link" + + +def load_urdf(package: str, relative_urdf_path: Path | str) -> str: + """Load a URDF as a string""" + + relative_urdf_path = Path(relative_urdf_path) + package_root = get_package_share_directory(package) + urdf_file = required_file(package_root, relative_urdf_path) + return urdf_file.read_text() + + +def fix_urdf_paths(package: str, relative_urdf_path: Path | str) -> str: + """Load a urdf and fix paths within the file to be relative to the package base. + + This assumes that all STLs and other resources required by the urdf are relative to + the urdf file. For example, if the urdf file is in package/cool-dir/robot.urdf, + then all the the STL's should also be found somewhere within cool-dir or in childs + of that directory. + + Some tools like onshape-urdf-exporter don't include the package name in the stl + paths within the robot.urdf file. This function fixes that by prepending the + package name to the stl paths. If it's detected to already be there, it's left + alone. + + :param package: The name of the package + :param relative_urdf_path: The path to the urdf relative to the package directory + :returns: The urdf text, with corrected paths. + """ + relative_urdf_path = Path(relative_urdf_path) + urdf_str = load_urdf(package, relative_urdf_path) + + if f"package://{package}" in urdf_str: + # The urdf already has the package name in the paths, so we don't need to fix it + return urdf_str + + urdf_str = urdf_str.replace( + "package://", f"package://{package}/{relative_urdf_path.parent}/" + ) + return urdf_str + + +def _assert_attributes_exist( + urdf_strs: list[str], find_names: list[str], tag: str +) -> None: + """Raise assertion errors if the names of a particular tag aren't specified in the + URDF. + + :param urdf_strs: The list of URDF files, loaded as strings + :param find_names: The list of joint names to validate + :param tag: The XML tag to look for names under. For example, 'joint' or 'link' + :raises ValueError: If the joint names don't exist + """ + if not len(find_names): + raise ValueError(f"There must be at least one {tag} name in order to validate!") + + urdf_tag_names = [_extract_tags(u, tag) for u in urdf_strs] + + seen_names = set() + for name_set in urdf_tag_names: + for name in name_set: + if name in seen_names: + raise ValueError( + f"The {tag} '{name}' has been seen in another URDF! When loading " + f"multiple URDFs with colliding names, make sure to apply " + f"namespaces to each." + ) + + if name in find_names: + seen_names.add(name) + + if seen_names != set(find_names): + raise ValueError( + f"The following {tag} names were expected but were missing: " + f"{set(find_names) - seen_names}" + ) + + +def _extract_tags(urdf_str: str, tag: str) -> list[str]: + """Extract all unique names of a specific tag type from a URDF + + :param urdf_str: The URDF to parse + :param tag: The tag to extract names of + :return: The list of names found for that tag + """ + urdf = ElementTree.fromstring(urdf_str) + names = {cast(str, node.get(_NAME_KEY)) for node in urdf.iter(tag)} + return list(names) + + +def assert_joint_names_exist(urdf_strs: list[str], names: list[str]) -> None: + return _assert_attributes_exist(urdf_strs, names, _JOINT_TAG) + + +def assert_link_names_exist(urdf_strs: list[str], names: list[str]) -> None: + return _assert_attributes_exist(urdf_strs, names, _LINK_TAG) + + +def prepend_namespace(urdf_str: str, namespace: str) -> str: + """Apply a namespace to ever link and joint name, to make them unique + :param urdf_str: A urdf laoded as text + :param namespace: The 'namespace' to prepend link and joint names with + :returns: The urdf text, with link and joint names prepended with the namespace + """ + + urdf = ElementTree.fromstring(urdf_str) + + # Rename all links and joints + for node in urdf.iter(): + if node.tag in [_JOINT_TAG, _LINK_TAG]: + node.set( + "name", + NAMESPACE_FMT.format(namespace=namespace, name=node.get(_NAME_KEY)), + ) + elif ( + node.tag in [_PARENT_TAG, _CHILD_TAG] and "link" in node.keys() # noqa: SIM118 + ): + node.set( + "link", + NAMESPACE_FMT.format(namespace=namespace, name=node.get(_LINK_KEY)), + ) + + return ElementTree.tostring(urdf, encoding="unicode") diff --git a/pkgs/node_helpers/node_helpers/urdfs/urdf_constants.py b/pkgs/node_helpers/node_helpers/urdfs/urdf_constants.py new file mode 100644 index 0000000..a350e5f --- /dev/null +++ b/pkgs/node_helpers/node_helpers/urdfs/urdf_constants.py @@ -0,0 +1,166 @@ +import logging +from collections.abc import Sequence +from pathlib import Path +from typing import Generic, NamedTuple, TypeVar + +from node_helpers.parameters import Choosable, DuplicateRegistrationError + +from . import loading as urdf_helpers + +JOINTS = TypeVar("JOINTS", bound=NamedTuple) +FRAMES = TypeVar("FRAMES", bound=NamedTuple) +EITHER = TypeVar("EITHER", bound=NamedTuple) + + +class URDFValidationError(Exception): + """Occurs during validation of a URDF""" + + +class URDFConstants(Choosable, Generic[JOINTS, FRAMES]): + def __init__( + self, + from_package: str, + registration_name: str, + urdf_paths: Sequence[tuple[str | None, str | Path]], + joints: JOINTS, + frames: FRAMES, + prepend_namespace: str | None = None, + ): + """ + :param from_package: The package to load the URDFs from + :param registration_name: The name to use when registering this URDF with the + global registry. Using this name you can access this URDF from anywhere, + using urdf.get_registered_instance(name). + Instances created using 'with_namespace' are not automatically registered. + :param urdf_paths: A list of tuples of form [("prepend-name", "urdf_path"), ...] + The first element in the tuple can be None or a string, and represents a + name to tack on to the start of every joint/link in the urdf. This will + allow for tacking together the same URDF multiple times into a larger + assembly. All urdf paths are relative to the urdf package root. + :param joints: The Joints model + :param frames: The Frames model + :param prepend_namespace: A namespace, if any, to prepend to joints and frames + + :raises DuplicateRegistrationError: If you use the same name on multiple URDFs + """ + + self.registration_name = registration_name + self.namespace = prepend_namespace + self.from_package = from_package + self.frames = self._prepend_namespace(frames, self.namespace) + self.joints = self._prepend_namespace(joints, self.namespace) + + self._urdf_paths = [(namespace, Path(path)) for namespace, path in urdf_paths] + + self.validate() + + # Register this instance with the global registry, so that it can be accessed + # via URDFConstant.get_registered_instance(name) + try: + self.register_instance(self.registration_name) + except DuplicateRegistrationError: + if prepend_namespace is None: + # This means that a URDF with this name was already registered, but it + # wasn't simply because someone instantiated a new URDF using + # urdf.with_namespace(...)! + raise + logging.info( + f"URDFConstant with name {self.registration_name} was already " + f"registered. This is fine, because the URDF had a namespace applied." + ) + + def __len__(self) -> int: + return len(self._urdf_paths) + + @staticmethod + def _prepend_namespace(model: EITHER, namespace: str | None) -> EITHER: + """Create a version of this model with a namespace applied to each attribute""" + + if namespace is None: + return model + namespace = namespace.replace("/", "") + + new_attributes: dict[str, str] = {} + for model_attr, attr_constant in model._asdict().items(): + namespaced_constant = urdf_helpers.NAMESPACE_FMT.format( + namespace=namespace, name=attr_constant + ) + new_attributes[model_attr] = namespaced_constant + + # Return a newly generated namedtuple + return type(model)(**new_attributes) # type: ignore + + def validate(self) -> None: + urdf_strs = self.load_urdfs() + + joint_names = list(self.joints) + frame_names = list(self.frames) + + # Validate that if two URDFs with the same path got loaded, that they all have a + # prefix associated with them. + for path_and_prefix in self._urdf_paths: + duplicates = [ + other for other in self._urdf_paths if other == path_and_prefix + ] + if len(duplicates) > 1: + raise URDFValidationError( + "When loading multiple of the same URDF file in one Constants, you " + "must apply a prefix to at least one of them." + ) + + # Validate joint and frame names exist in the URDF + try: + if len(joint_names): + urdf_helpers.assert_joint_names_exist(urdf_strs, joint_names) + + if len(frame_names): + urdf_helpers.assert_link_names_exist(urdf_strs, frame_names) + + except ValueError as e: + raise URDFValidationError(str(e)) from e + + # Validate there are no duplicate frame or joint names + if not ( + len(frame_names) == len(set(frame_names)) + and len(joint_names) == len(set(joint_names)) + ): + raise URDFValidationError( + f"There can be no duplicate frame or joint names! " + f"{self.frames=} {self.joints=}" + ) + + def load_urdfs(self) -> list[str]: + """Load each urdf with a prepended namespace, if configured. + URDFs will also have their respective configured prefixes + (configured in the constructor) applied. + + :return: A list of URDFs loaded as strings + """ + + urdfs = [] + for custom_namespace, urdf_path in self._urdf_paths: + urdf_str = urdf_helpers.fix_urdf_paths(self.from_package, urdf_path) + + if custom_namespace is not None: + urdf_str = urdf_helpers.prepend_namespace(urdf_str, custom_namespace) + if self.namespace is not None: + urdf_str = urdf_helpers.prepend_namespace(urdf_str, self.namespace) + urdfs.append(urdf_str) + return urdfs + + def with_namespace(self, namespace: str) -> "URDFConstants[JOINTS, FRAMES]": + """Returns URDFConstants with the namespace prepended to frames and joints""" + if self.namespace is not None: + raise ValueError( + "You can't add a namespace to a URDFConstants that already " + "had a namespace applied" + ) + + return URDFConstants( + from_package=self.from_package, + registration_name=self.registration_name, + urdf_paths=self._urdf_paths, + joints=self.joints, + frames=self.frames, + prepend_namespace=namespace.replace("/", ""), + ) diff --git a/pkgs/node_helpers/node_helpers_test/integration/urdfs/__init__.py b/pkgs/node_helpers/node_helpers_test/integration/urdfs/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/pkgs/node_helpers/node_helpers_test/integration/urdfs/example_urdf_constants.py b/pkgs/node_helpers/node_helpers_test/integration/urdfs/example_urdf_constants.py new file mode 100644 index 0000000..753fc0d --- /dev/null +++ b/pkgs/node_helpers/node_helpers_test/integration/urdfs/example_urdf_constants.py @@ -0,0 +1,25 @@ +from typing import NamedTuple + +from node_helpers.urdfs import URDFConstants + + +class ForkliftJoints(NamedTuple): + FORKS: str = "forks" + FORKS_PARENT_DATUM: str = "forks_parent_datum" + + +class ForkliftFrames(NamedTuple): + BASE_LINK: str = "forklift_body" + + # Joint tracking + FORKS_ORIGIN: str = "forks_origin" + FORKS: str = "forks" + + +ForkliftURDF = URDFConstants[ForkliftJoints, ForkliftFrames]( + from_package="node_helpers", + registration_name="forklift", + urdf_paths=[(None, "sample_urdfs/forklift/robot.urdf")], + joints=ForkliftJoints(), + frames=ForkliftFrames(), +) diff --git a/pkgs/node_helpers/node_helpers_test/integration/urdfs/test_forklift.py b/pkgs/node_helpers/node_helpers_test/integration/urdfs/test_forklift.py new file mode 100644 index 0000000..a098a9f --- /dev/null +++ b/pkgs/node_helpers/node_helpers_test/integration/urdfs/test_forklift.py @@ -0,0 +1,59 @@ +from collections.abc import Generator +from typing import cast + +import pytest +from node_helpers.launching import URDFModuleNodeFactory +from node_helpers.testing import ( + TFClient, + URDFModuleFixture, + set_up_node, + validate_coincident_transforms, +) + +from .example_urdf_constants import ForkliftURDF + +_NAMESPACED_CONSTANTS = ForkliftURDF.with_namespace("test_fixture") +_FRAMES = _NAMESPACED_CONSTANTS.frames +_JOINTS = _NAMESPACED_CONSTANTS.joints + + +@pytest.fixture() +def tf_client() -> Generator[TFClient, None, None]: + yield from set_up_node(TFClient, "cool_namespace", "tf_node") + + +@pytest.fixture() +def urdf_module() -> Generator[URDFModuleFixture, None, None]: + yield from URDFModuleFixture.set_up( + URDFModuleNodeFactory.Parameters( + namespace=cast(str, _NAMESPACED_CONSTANTS.namespace), + urdf_constant_name=ForkliftURDF.registration_name, + ), + static_transforms=[], + ) + + +@pytest.mark.parametrize( + ("joint_origin", "joint_current", "same_position", "same_orientation"), + ((_FRAMES.FORKS, _FRAMES.FORKS_ORIGIN, True, True),), +) +def test_coincident_transforms( + urdf_module: URDFModuleFixture, + tf_client: TFClient, + joint_origin: str, + joint_current: str, + same_position: bool, + same_orientation: bool, +) -> None: + """This test validates that the URDF is correctly formulated, such that the joint + `joint_current` is coincident with the joint `joint_origin` when the robot is in its + 0 position. + """ + validate_coincident_transforms( + urdf_module, + tf_client, + joint_origin, + joint_current, + same_position, + same_orientation, + ) diff --git a/pkgs/node_helpers/node_helpers_test/unit/urdfs/test_loading.py b/pkgs/node_helpers/node_helpers_test/unit/urdfs/test_loading.py new file mode 100644 index 0000000..a8d8e86 --- /dev/null +++ b/pkgs/node_helpers/node_helpers_test/unit/urdfs/test_loading.py @@ -0,0 +1,120 @@ +import pytest +from node_helpers.urdfs import loading as urdf_helpers +from node_helpers.urdfs.loading import NAMESPACE_FMT +from node_helpers_test.resources import GENERIC_URDF + +EXPECTED_JOINT_NAMES = ["shuttle1-joint", "clamp1-joint"] +EXPECTED_LINK_NAMES = ["base_link", "shuttle1", "clamp1"] + + +def test_fix_urdf_paths_makes_path_replacements() -> None: + package_name = "node_helpers" + expected_pattern = f"{package_name}/{GENERIC_URDF.parent}/" + + modified_urdf = urdf_helpers.fix_urdf_paths( + package=package_name, relative_urdf_path=GENERIC_URDF + ) + original_urdf = GENERIC_URDF.read_text() + assert modified_urdf != original_urdf + + n_expected_changes = original_urdf.count("package://") + assert n_expected_changes == modified_urdf.count(expected_pattern) + assert original_urdf.count(expected_pattern) == 0 + + +def test_assert_joint_names_exist() -> None: + urdf_text = GENERIC_URDF.read_text() + + # Raises error when no names provided + with pytest.raises(ValueError): + urdf_helpers.assert_joint_names_exist([urdf_text], []) + + # Raises an error when invalid names provided + with pytest.raises(ValueError): + urdf_helpers.assert_joint_names_exist( + [urdf_text], [*EXPECTED_JOINT_NAMES, "nonexistent-joint"] + ) + + # No assertions raised when all joint names are valid + urdf_helpers.assert_joint_names_exist([urdf_text], EXPECTED_JOINT_NAMES) + + +def test_assert_attributes_exist_fails_on_duplicate_attributes_across_urdfs() -> None: + """The basic contract with multi-urdf loading is that each urdf has unique frame + or joint names; at least, the ones that are being asserted. This test validates + that check is being done as expected.""" + + urdf_text = GENERIC_URDF.read_text() + urdf_text_namespaced = urdf_helpers.prepend_namespace( + GENERIC_URDF.read_text(), "cool" + ) + + _expected_links_namespaced = [ + NAMESPACE_FMT.format(namespace="cool", name=n) for n in EXPECTED_LINK_NAMES + ] + + # Raises error when two urdfs have the same joint name + with pytest.raises(ValueError): + urdf_helpers.assert_link_names_exist( + [urdf_text, urdf_text], EXPECTED_LINK_NAMES + ) + + # This should fail because the joint names don't actually exist + with pytest.raises(ValueError): + urdf_helpers.assert_link_names_exist( + [urdf_text_namespaced, urdf_text_namespaced], EXPECTED_LINK_NAMES + ) + + # Test happy path with just one urdf file + urdf_helpers.assert_link_names_exist( + [urdf_text_namespaced], _expected_links_namespaced + ) + urdf_helpers.assert_link_names_exist([urdf_text], EXPECTED_LINK_NAMES) + + # Test happy paths where the two urdfs are namespaced, and the joints all exist + urdf_helpers.assert_link_names_exist( + [urdf_text, urdf_text_namespaced], + [*_expected_links_namespaced, *EXPECTED_LINK_NAMES], + ) + urdf_helpers.assert_link_names_exist( + [urdf_text, urdf_text_namespaced], EXPECTED_LINK_NAMES + ) + urdf_helpers.assert_link_names_exist( + [urdf_text, urdf_text_namespaced], _expected_links_namespaced + ) + + +def test_assert_link_names_exist() -> None: + urdf_text = GENERIC_URDF.read_text() + + # Raises error when no names provided + with pytest.raises(ValueError): + urdf_helpers.assert_link_names_exist([urdf_text], []) + + # Raises an error when invalid names provided + with pytest.raises(ValueError): + urdf_helpers.assert_link_names_exist( + [urdf_text], [*EXPECTED_LINK_NAMES, "nonexistent-link"] + ) + + # No assertions raised when all link names are valid + urdf_helpers.assert_link_names_exist([urdf_text], EXPECTED_LINK_NAMES) + + +def test_prepend_namespace() -> None: + urdf_text: str = GENERIC_URDF.read_text() + namespace = "cool_namespace" + modified = urdf_helpers.prepend_namespace(urdf_text, namespace=namespace) + + for changes in (EXPECTED_JOINT_NAMES, EXPECTED_LINK_NAMES): + expected_changes = list(map(urdf_text.count, changes)) + actual_changes = list( + map( + modified.count, + [ + urdf_helpers.NAMESPACE_FMT.format(namespace=namespace, name=j) + for j in changes + ], + ) + ) + assert len(expected_changes) == len(actual_changes) diff --git a/pkgs/node_helpers/node_helpers_test/unit/urdfs/test_urdf_constants.py b/pkgs/node_helpers/node_helpers_test/unit/urdfs/test_urdf_constants.py new file mode 100644 index 0000000..3ddc62e --- /dev/null +++ b/pkgs/node_helpers/node_helpers_test/unit/urdfs/test_urdf_constants.py @@ -0,0 +1,271 @@ +from typing import NamedTuple + +import pytest +from node_helpers.parameters import ( + DuplicateRegistrationError, + UnregisteredChoosableError, +) +from node_helpers.urdfs import URDFConstants, URDFValidationError +from node_helpers_test.resources import GENERIC_URDF + + +class GenericURDFFrames(NamedTuple): + BASE_LINK: str = "base_link" + SOME_FRAME: str = "shuttle1" + + +class GenericURDFJoints(NamedTuple): + JOINT_1: str = "shuttle1-joint" + JOINT_2: str = "clamp1-joint" + + +def test_with_namespace() -> None: + """ + Validate that *.with_namespace() returns new copies with the namespace prepended + """ + constants: URDFConstants[GenericURDFJoints, GenericURDFFrames] = URDFConstants( + from_package="node_helpers", + registration_name="generic_urdf", + urdf_paths=[(None, GENERIC_URDF)], + joints=GenericURDFJoints(), + frames=GenericURDFFrames(), + ) + namespaced = constants.with_namespace("/bingus") + assert constants.registration_name == "generic_urdf" + assert namespaced.registration_name == "generic_urdf" + assert constants.get_registered_instance("generic_urdf") is constants + assert constants is not namespaced + assert constants.namespace is None + assert namespaced.namespace == "bingus" + assert constants._urdf_paths == namespaced._urdf_paths + assert constants.joints is not namespaced.joints + assert constants.frames is not namespaced.frames + assert isinstance(namespaced.frames, GenericURDFFrames) + assert isinstance(namespaced.joints, GenericURDFJoints) + assert f"bingus.{GenericURDFJoints().JOINT_2}" == namespaced.joints.JOINT_2 + assert f"bingus.{GenericURDFFrames().SOME_FRAME}" == namespaced.frames.SOME_FRAME + + +def test_multi_urdf_prefixes() -> None: + """Test when each urdf has a prefix""" + + class MixedPrefixFrames(NamedTuple): + BASE_LINK: str = "bingus.base_link" + SOME_FRAME: str = "cool-prefix.shuttle1" + + class MixedPrefixJoints(NamedTuple): + JOINT_1: str = "bingus.shuttle1-joint" + JOINT_2: str = "cool-prefix.clamp1-joint" + + # Happy path! No failures here, presumably + URDFConstants( + from_package="node_helpers", + registration_name="", + urdf_paths=[("bingus", GENERIC_URDF), ("cool-prefix", GENERIC_URDF)], + joints=MixedPrefixJoints(), + frames=MixedPrefixFrames(), + ) + + # This should fail because the urdfs will have duplicate joint/frame names + with pytest.raises(URDFValidationError): + URDFConstants( + from_package="node_helpers", + registration_name="", + urdf_paths=[(None, GENERIC_URDF), (None, GENERIC_URDF)], + joints=MixedPrefixJoints(), + frames=MixedPrefixFrames(), + ) + + # This should fail because the 'cool-prefix' joints weren't found in any urdf + with pytest.raises(URDFValidationError): + URDFConstants( + from_package="node_helpers", + registration_name="", + urdf_paths=[(None, GENERIC_URDF), ("uncool-prefix", GENERIC_URDF)], + joints=MixedPrefixJoints(), + frames=MixedPrefixFrames(), + ) + + +def test_duplicate_urdf_files_require_prefixes() -> None: + """Test that when two identical URDF files are loaded, the system requires a + prefix on at least one of them.""" + + with pytest.raises(URDFValidationError): + URDFConstants( + from_package="node_helpers", + registration_name="", + urdf_paths=[(None, GENERIC_URDF), (None, GENERIC_URDF)], + joints=GenericURDFJoints(), + frames=GenericURDFFrames(), + ) + with pytest.raises(URDFValidationError): + URDFConstants( + from_package="node_helpers", + registration_name="", + urdf_paths=[("same-prefix", GENERIC_URDF), ("same-prefix", GENERIC_URDF)], + joints=GenericURDFJoints(), + frames=GenericURDFFrames(), + ) + + class PrefixedURDFFrames(NamedTuple): + BASE_LINK_1: str = "prefixed_on_1.base_link" + SOME_FRAME_1: str = "prefixed_on_1.shuttle1" + BASE_LINK_2: str = "prefixed_on_2.base_link" + SOME_FRAME_2: str = "prefixed_on_2.shuttle1" + + class PrefixedURDFJoints(NamedTuple): + JOINT_1_1: str = "prefixed_on_1.shuttle1-joint" + JOINT_1_2: str = "prefixed_on_1.clamp1-joint" + JOINT_2_1: str = "prefixed_on_2.shuttle1-joint" + JOINT_2_2: str = "prefixed_on_2.clamp1-joint" + + # Both are prefixed, yay + URDFConstants( + from_package="node_helpers", + registration_name="cool-test-constant-91238", + urdf_paths=[("prefixed_on_1", GENERIC_URDF), ("prefixed_on_2", GENERIC_URDF)], + joints=PrefixedURDFJoints(), + frames=PrefixedURDFFrames(), + ) + + +def test_validate_fails_with_false_path() -> None: + with pytest.raises(FileNotFoundError): + URDFConstants( + from_package="node_helpers", + registration_name="", + urdf_paths=[("namespace", "invalid/path")], + joints=GenericURDFJoints(), + frames=GenericURDFFrames(), + ) + + +def test_nonexistent_frames_and_joints() -> None: + """Test that joints and frames are cross-referenced with their parent urdf + to ensure they exist + """ + + class BadFrames(NamedTuple): + BAD_FRAME: str = "this frame does not exist!" + + class BadJoints(NamedTuple): + BAD_JOINT: str = "This doesn't exist!" + + # Test with bad joints + with pytest.raises(URDFValidationError): + URDFConstants( + from_package="node_helpers", + registration_name="", + urdf_paths=[(None, GENERIC_URDF)], + joints=BadJoints(), + frames=GenericURDFFrames(), + ) + + # Test with bad frames + with pytest.raises(URDFValidationError): + URDFConstants( + from_package="node_helpers", + registration_name="", + urdf_paths=[(None, GENERIC_URDF)], + joints=GenericURDFJoints(), + frames=BadFrames(), + ) + + +def test_duplicate_frame_names_and_joints() -> None: + """Validate that duplicate joint names are found and cause failures""" + + class DuplicateFrames(NamedTuple): + FRAME_1: str = GenericURDFFrames().SOME_FRAME + FRAME_2: str = GenericURDFFrames().SOME_FRAME + + class DuplicateJoints(NamedTuple): + JOINT_1: str = GenericURDFJoints().JOINT_1 + JOINT_2: str = GenericURDFJoints().JOINT_1 + + # Test with duplicate joints + with pytest.raises(URDFValidationError): + URDFConstants( + from_package="node_helpers", + registration_name="", + urdf_paths=[(None, GENERIC_URDF)], + joints=DuplicateJoints(), + frames=GenericURDFFrames(), + ) + + # Test with duplicate frames + with pytest.raises(URDFValidationError): + URDFConstants( + from_package="node_helpers", + registration_name="", + urdf_paths=[(None, GENERIC_URDF)], + joints=GenericURDFJoints(), + frames=DuplicateFrames(), + ) + + +def test_applying_namespace_twice_fails() -> None: + not_namespaced: URDFConstants[GenericURDFJoints, GenericURDFFrames] = URDFConstants( + from_package="node_helpers", + registration_name="cool-test-constant-142", + urdf_paths=[(None, GENERIC_URDF)], + joints=GenericURDFJoints(), + frames=GenericURDFFrames(), + ) + namespaced = not_namespaced.with_namespace("cool_stuff") + with pytest.raises(ValueError): + namespaced.with_namespace("whoa_namespaceception") + + +def test_registration() -> None: + """Test that URDF constants are registered when expected""" + registered_name = "registered-urdf-constant" + + # This should fail validation, and not be registered + class DuplicateJoints(NamedTuple): + JOINT_1: str = GenericURDFJoints().JOINT_1 + JOINT_2: str = GenericURDFJoints().JOINT_1 + + with pytest.raises(URDFValidationError): + URDFConstants( + from_package="node_helpers", + registration_name=registered_name, + urdf_paths=[(None, GENERIC_URDF)], + joints=DuplicateJoints(), + frames=GenericURDFFrames(), + ) + + # Ensure it was not registered + with pytest.raises(UnregisteredChoosableError): + URDFConstants.get_registered_instance(registered_name) + + # Because validation failed, this should not be registered + with pytest.raises(UnregisteredChoosableError): + URDFConstants.get_registered_instance(registered_name) + + # This should work, and be registered + registered = URDFConstants( + from_package="node_helpers", + registration_name=registered_name, + urdf_paths=[(None, GENERIC_URDF)], + joints=GenericURDFJoints(), + frames=GenericURDFFrames(), + ) + + # Validate it's registered + assert URDFConstants.get_registered_instance(registered_name) is registered + + # Making instances by using 'with_namespace' should pass, but not replace the OG + new_instance = registered.with_namespace("blah") + assert URDFConstants.get_registered_instance(registered_name) is not new_instance + + # This should fail because the name is already registered + with pytest.raises(DuplicateRegistrationError): + URDFConstants( + from_package="node_helpers", + registration_name=registered_name, + urdf_paths=[(None, GENERIC_URDF)], + joints=GenericURDFJoints(), + frames=GenericURDFFrames(), + ) diff --git a/pkgs/node_helpers/package.xml b/pkgs/node_helpers/package.xml index 3207f4e..8f2140c 100644 --- a/pkgs/node_helpers/package.xml +++ b/pkgs/node_helpers/package.xml @@ -23,6 +23,10 @@ sensor_msgs tf_transformations + + joint_state_publisher + robot_state_publisher + ament_python diff --git a/pkgs/node_helpers/pyproject.toml b/pkgs/node_helpers/pyproject.toml index 78e1c63..ccd7c53 100644 --- a/pkgs/node_helpers/pyproject.toml +++ b/pkgs/node_helpers/pyproject.toml @@ -22,7 +22,7 @@ placeholder = "node_helpers.nodes.placeholder:main" [tool.colcon-poetry-ros.data-files] "share/ament_index/resource_index/packages" = ["resource/node_helpers"] -"share/node_helpers" = ["package.xml"] +"share/node_helpers" = ["package.xml", "sample_urdfs/"] [build-system] requires = ["poetry-core>=1.0.0"] diff --git a/pkgs/node_helpers/sample_urdfs/forklift/cube.stl b/pkgs/node_helpers/sample_urdfs/forklift/cube.stl new file mode 100644 index 0000000..1221f01 --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/cube.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:12061376bb0e0b4bebc0ce7cd743e8c6620097cc2a6ea86621a9ec9e50bd6c10 +size 684 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part1.stl b/pkgs/node_helpers/sample_urdfs/forklift/part1.stl new file mode 100644 index 0000000..684076c --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part1.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9e672e09ba0013e40ac3de54b730fd49ec7f52e2deafdcc967117a2a59c21099 +size 855884 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part10.stl b/pkgs/node_helpers/sample_urdfs/forklift/part10.stl new file mode 100644 index 0000000..baa1210 --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part10.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e48386f323e10fd9f200ef683403bf0b7c7d263ef85074759e3771a6b750d5ca +size 319084 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part12.stl b/pkgs/node_helpers/sample_urdfs/forklift/part12.stl new file mode 100644 index 0000000..db9eb99 --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part12.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:411ebadea2b8d5352dc4ae1a5b0167cd6ea176c6ddb43409bafdaae4b3623251 +size 2584 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part13a.stl b/pkgs/node_helpers/sample_urdfs/forklift/part13a.stl new file mode 100644 index 0000000..bae7a2e --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part13a.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7f40191220422d8063733315e0b62f0b1c83a67142671029aac80ee24ebb44fa +size 39984 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part14b.stl b/pkgs/node_helpers/sample_urdfs/forklift/part14b.stl new file mode 100644 index 0000000..aadcdbb --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part14b.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e1cf3ab91b777315ea1424724e4a174fe67b2108acb54f7a2981161f6a310a50 +size 26984 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part15a.stl b/pkgs/node_helpers/sample_urdfs/forklift/part15a.stl new file mode 100644 index 0000000..25928f0 --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part15a.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b3c1b877c7c89259ee12fb1fb59740c5f6ac5ac8309e6adef956dccf42030e5e +size 43984 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part16b.stl b/pkgs/node_helpers/sample_urdfs/forklift/part16b.stl new file mode 100644 index 0000000..840b48c --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part16b.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:610aef4d4aa383906689884b9596986c41784a2f4c744f91d2769900380bb269 +size 12884 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part17.stl b/pkgs/node_helpers/sample_urdfs/forklift/part17.stl new file mode 100644 index 0000000..a38b438 --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part17.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f4d79d44e9ccb9637618c87b4916bcfc41b4173dc152c879df77b33be62d35cd +size 678184 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part18.stl b/pkgs/node_helpers/sample_urdfs/forklift/part18.stl new file mode 100644 index 0000000..6d4f7da --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part18.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d8509d2d7e34a92f42cd11a7ec8fec2f16de2e4402f8dbd3ed2f8b02c975b936 +size 50884 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part2.stl b/pkgs/node_helpers/sample_urdfs/forklift/part2.stl new file mode 100644 index 0000000..a66d177 --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part2.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:839455c27d53420c887c44aab16d81517418dde4c4aa4efada2be817ec434d32 +size 316784 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part3.stl b/pkgs/node_helpers/sample_urdfs/forklift/part3.stl new file mode 100644 index 0000000..92ac89d --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part3.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3933913fe0a2116add5ac2c6f945b53190e12a7c072acf43fffec392942aa977 +size 52784 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part4.stl b/pkgs/node_helpers/sample_urdfs/forklift/part4.stl new file mode 100644 index 0000000..734fa65 --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part4.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5598f7263c87e5b9e3e778d616744942d893c4b71a02d636b58db32825123396 +size 1484 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part5.stl b/pkgs/node_helpers/sample_urdfs/forklift/part5.stl new file mode 100644 index 0000000..da2cc38 --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part5.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:557f1f93eb65f9c949f99a2b959e325045b201d40d1f43d411f37af705e40e3a +size 670484 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part6.stl b/pkgs/node_helpers/sample_urdfs/forklift/part6.stl new file mode 100644 index 0000000..f58ed77 --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part6.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f66ae624c02fced42e65384977855763913fba87d23897cfa033269dc3a759b7 +size 43234 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part7.stl b/pkgs/node_helpers/sample_urdfs/forklift/part7.stl new file mode 100644 index 0000000..35bcc8d --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part7.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2343e367f1ee83752de2cf487ad088f748708ec14c0b419ec8e7b879523ca3e7 +size 464084 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part8.stl b/pkgs/node_helpers/sample_urdfs/forklift/part8.stl new file mode 100644 index 0000000..0a29257 --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part8.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a1ecf720eec465502179cecdba043e0d6a0904926852815eab86226508133636 +size 653684 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part9.stl b/pkgs/node_helpers/sample_urdfs/forklift/part9.stl new file mode 100644 index 0000000..00d834e --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part9.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:fdbc9f4e25eda76e257f41097878abc3a4ff790e39983632cf5fdc7d45fe8c51 +size 5684 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_16.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_16.stl new file mode 100644 index 0000000..7e1644d --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_16.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ac6e94ebc418d72f860bad18029aca2046eb621c0f98440525739c0840d6398f +size 35484 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_18.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_18.stl new file mode 100644 index 0000000..e521a8f --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_18.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:26d6e928956a52aea379a6ff25862251ff7e5d6eef32ffab472e76b2113dc411 +size 2284 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_19.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_19.stl new file mode 100644 index 0000000..d11700b --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_19.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2b654a01a19ca1360c320f9f3ce2f85f11a2ac19dfffb34973d58f6cbf300cc3 +size 1484 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_20.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_20.stl new file mode 100644 index 0000000..e418742 --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_20.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:674e55358d95bf6520946db8f2f8aaa73cd2be929d3266e509a0f7dfb72b19b8 +size 5484 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_22.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_22.stl new file mode 100644 index 0000000..ed16e77 --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_22.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:40ae99e37cc29b6ad8de5bcde31cf49894cc75aa700645cbca531e16fa644554 +size 1484 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_25.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_25.stl new file mode 100644 index 0000000..19d0548 --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_25.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:645532cd37ca126a8e3f3ba5d16f2907aa0df9e94028eaa6312a6b9611a91095 +size 12884 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_26.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_26.stl new file mode 100644 index 0000000..69cd1f9 --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_26.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:dfc06f0b11fe3eb9a04a081cb6647a731ad7331179168188b0861c816c1fb89b +size 92884 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_28.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_28.stl new file mode 100644 index 0000000..4d9a27f --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_28.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0111967eb85936958bee791c47aed5e3eaf6a00904390c409e81f6ddf306a913 +size 43284 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_29.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_29.stl new file mode 100644 index 0000000..2a792a3 --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_29.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1ed09ef88b15b59608ab91af68a4cb60910dd0db89fce03569d144c6251910ed +size 16284 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_30.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_30.stl new file mode 100644 index 0000000..6db807d --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_30.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d316f08a8231a0c5bf3b50aa25afb161f6f280c14168caa66ae7c9ebf5f18bf0 +size 31884 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_31.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_31.stl new file mode 100644 index 0000000..e5459b9 --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_31.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d70f7398ed4865c901b582d9afd5fc4d40eb388e6eef5895d7e63fce9300b361 +size 17684 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_32.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_32.stl new file mode 100644 index 0000000..e4bd895 --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_32.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:89670eed0ec4200c0ae72fc72f4e7965f608598e1b9cbc209f96e6db290f7814 +size 346884 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_33.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_33.stl new file mode 100644 index 0000000..e4d39b2 --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_33.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:279cf353f6bd9f6fb158c38026a7af7c28d0d747b498cbe13ffc406eeec6100b +size 3784 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_34.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_34.stl new file mode 100644 index 0000000..4ab82e9 --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_34.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:974a64a164feafcedf4d8f8ebfc09f2e4dd1eecfbc168ef11e3434743a15dd2f +size 3784 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_35.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_35.stl new file mode 100644 index 0000000..32341cd --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_35.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f7742e295c363871dac6ab8eba9d8421a4cca2da8da1a90b37aa73c6056e5fba +size 12484 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_36.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_36.stl new file mode 100644 index 0000000..2027687 --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_36.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0b975614c71a2fbb6de4443194a28174f507f8efbb41fd9719f1f05c3168e218 +size 12484 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_37.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_37.stl new file mode 100644 index 0000000..8ea7c62 --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_37.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9ba48290b9cb426a66e44d8fa496847ec5a7945de46287ea0ba4819aa29b60e4 +size 12484 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_38.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_38.stl new file mode 100644 index 0000000..d7493d8 --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_38.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1d2064f845ea9bc5ef80c9151954a87e9ba8ef87f49a0965ae1b8f3b6bdcfe96 +size 12484 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_39.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_39.stl new file mode 100644 index 0000000..d0eebf8 --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_39.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7b5a228c71313882e322cba9647783bb803088899a9883c62330ccc75bed9e48 +size 11084 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_40.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_40.stl new file mode 100644 index 0000000..f7ffae4 --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_40.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:51c2afff14d5c79e4f6ddacaf3bed3855f0d0f4b942501f74b70d20ed95ea1c1 +size 11084 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_41.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_41.stl new file mode 100644 index 0000000..0628bce --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_41.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7340e5033ecde3fcea2ea242d767f79f5626684739a73c185e07f0971d6e6fd5 +size 11284 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_42.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_42.stl new file mode 100644 index 0000000..2f04c98 --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_42.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5b9c94e5677a5b317333e485ede0704e932c6ce69a8f3e7227fa0005b8b6d100 +size 11284 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_43.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_43.stl new file mode 100644 index 0000000..e3dac56 --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_43.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7bba62180b48f9fde038d5f9392ec434b7dd68ce6d70749f7f07796640cf357c +size 732684 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_44.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_44.stl new file mode 100644 index 0000000..1528ced --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_44.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:48cdc662a1f20005351653803080513b3a6a2f9bd86dff42b1e0f678f7eb9fad +size 61984 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_45.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_45.stl new file mode 100644 index 0000000..78029fe --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_45.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f6ada22c2e9a75188973ad2cd02522e9bf2db913a86343261e7f449f199ff1d6 +size 4084 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_46.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_46.stl new file mode 100644 index 0000000..fdc5155 --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_46.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:275b95e3c589f36146f2972ef191c3f98930114591ba0ba3bbb9459d704ec3c1 +size 6084 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_47.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_47.stl new file mode 100644 index 0000000..c01acfc --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_47.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f439b7409e8c4038e6f2b6de087dc45dc94b6ec7dc7df25db242a833b70585ea +size 684 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_48.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_48.stl new file mode 100644 index 0000000..73ec81c --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_48.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b39bc85313489484faca724d79324dfac4cafeae7c9279162317338bd7066f23 +size 53084 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_49.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_49.stl new file mode 100644 index 0000000..0018a61 --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_49.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:64bb22db672f3d440e020af9840056d50914792dc21e01de1e9680eb192b611b +size 15484 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_50.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_50.stl new file mode 100644 index 0000000..fd92a3c --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_50.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:34932739315dfe6297df83cd62b3e50f20954ac9cf33217935d24976d56b34ea +size 15484 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_51.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_51.stl new file mode 100644 index 0000000..801075d --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_51.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ab7e36432172b42efad5d1fa2cc38bb2b8b78d9241bc69cc0b846f8b72de407f +size 261484 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_52.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_52.stl new file mode 100644 index 0000000..7ff5183 --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_52.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:09297d084f97165a159c86db6267408e805a50b11be26c8c65ac73a4677102d4 +size 28884 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_53.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_53.stl new file mode 100644 index 0000000..4ad31fa --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_53.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4f7d6047a2ae4240101f25bce7a49297bffa7cb28cd6fef9375c5890a39a45dc +size 13084 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_54.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_54.stl new file mode 100644 index 0000000..561b19c --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_54.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:828e820fe0d787317e65468f98543ee9bdc8957069687ba3afc35b11c14c3fec +size 13084 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_56.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_56.stl new file mode 100644 index 0000000..57230e5 --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_56.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:94f64592eedcc1dc551c725e1976538e3bd6c695ee9d9a9377751a456303040e +size 5684 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/part_57.stl b/pkgs/node_helpers/sample_urdfs/forklift/part_57.stl new file mode 100644 index 0000000..6162f8c --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/part_57.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:eaf05f849981e708e72abff5907e92ee4cca0b1633290792307450abd4f29670 +size 55884 diff --git a/pkgs/node_helpers/sample_urdfs/forklift/robot.urdf b/pkgs/node_helpers/sample_urdfs/forklift/robot.urdf new file mode 100644 index 0000000..4cb28d5 --- /dev/null +++ b/pkgs/node_helpers/sample_urdfs/forklift/robot.urdf @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8e27b39b189b6b8c668e4af073e0a3a170a457804ccd3087f55e5f098561385a +size 49916