Add urdf tooling! (#7)

Author: apockill

.cruft.json

@@ -1,6 +1,6 @@
 {
   "template": "https://github.com/UrbanMachine/create-ros-app.git",
-  "commit": "7697b9d1edf4d2b27d2bc9d3095fa893a8d92497",
+  "commit": "6a85fd934e7c6297d5fd717c545fad443cc4dfcf",
   "checkout": null,
   "context": {
     "cookiecutter": {

.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:

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

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

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``

pkgs/node_helpers/node_helpers/launching/__init__.py

@@ -5,3 +5,4 @@
     SwappableNode,
     apply_node_swaps,
 )
+from .urdf_module_launching import URDFModuleNodeFactory

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}"

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",
 ]