From 494182b5d2f3ac4ff1bcf94e2b0bf12ada61e906 Mon Sep 17 00:00:00 2001 From: Alex Thiele Date: Fri, 6 Dec 2024 08:32:20 -0800 Subject: [PATCH 01/24] Set a strict timeout for tests --- docker/Dockerfile | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/docker/Dockerfile b/docker/Dockerfile index 6e46e0a..e02bf7f 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -41,6 +41,8 @@ RUN --mount=type=cache,target="${APT_CACHE}" \ # ROS2 ros-${ROS2_DISTRO}-ros-base \ ros-${ROS2_DISTRO}-rosbridge-suite \ + # TODO: Remove cyclone + ros-${ROS2_DISTRO}-rmw-cyclonedds-cpp \ # Build tools build-essential \ git \ @@ -101,13 +103,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 From 0c263c12edd4edfdec83be7fff95ad15b1db5c09 Mon Sep 17 00:00:00 2001 From: Alex Thiele Date: Fri, 6 Dec 2024 08:53:46 -0800 Subject: [PATCH 02/24] Add cyclonedds to system but don't set it as the default --- docker/Dockerfile | 2 -- 1 file changed, 2 deletions(-) diff --git a/docker/Dockerfile b/docker/Dockerfile index e02bf7f..83e850f 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -41,8 +41,6 @@ RUN --mount=type=cache,target="${APT_CACHE}" \ # ROS2 ros-${ROS2_DISTRO}-ros-base \ ros-${ROS2_DISTRO}-rosbridge-suite \ - # TODO: Remove cyclone - ros-${ROS2_DISTRO}-rmw-cyclonedds-cpp \ # Build tools build-essential \ git \ From 82d0d63a396cdb723239106e9aa2228cc8717ebb Mon Sep 17 00:00:00 2001 From: Alex Thiele Date: Fri, 6 Dec 2024 08:54:57 -0800 Subject: [PATCH 03/24] Update template --- .github/workflows/test.yaml | 2 +- docker/Dockerfile | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) 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 83e850f..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 \ From 0e4d8cd46c75a2e83a0e8465f28ecf0818d5e1bc Mon Sep 17 00:00:00 2001 From: Alex Thiele Date: Fri, 6 Dec 2024 08:55:10 -0800 Subject: [PATCH 04/24] Update template --- .cruft.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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": { From 689f6a82522c3f93c1d3fe52c5ae65365a4a4b27 Mon Sep 17 00:00:00 2001 From: Alex Thiele Date: Fri, 6 Dec 2024 14:23:24 -0800 Subject: [PATCH 05/24] Add URDF docs --- docs/launching.rst | 5 +- docs/urdfs.rst | 125 +++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 129 insertions(+), 1 deletion(-) create mode 100644 docs/urdfs.rst 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..dee5c23 --- /dev/null +++ b/docs/urdfs.rst @@ -0,0 +1,125 @@ +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(), + ) + + + +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 + ) + ) From 6a3a6fa96031f3d99f109db5a5bdb10259520933 Mon Sep 17 00:00:00 2001 From: Alex Thiele Date: Fri, 6 Dec 2024 15:23:53 -0800 Subject: [PATCH 06/24] Update URDFs docs --- docs/urdfs.rst | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/docs/urdfs.rst b/docs/urdfs.rst index dee5c23..653314b 100644 --- a/docs/urdfs.rst +++ b/docs/urdfs.rst @@ -75,7 +75,7 @@ specifies what URDF the prepended names refer to. frames=BigBirdFrames(), ) - +Note that an example URDF constant can be found in ``pkgs/node_helpers_test/integration/urdfs/example_urdf_constants.py`` URDFModule ---------- @@ -123,3 +123,8 @@ and ensure that all TF frames are published correctly before yielding the fixtur 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 From a583867ccc9433471f303ff639dac1457c8354d9 Mon Sep 17 00:00:00 2001 From: Alex Thiele Date: Fri, 6 Dec 2024 15:24:02 -0800 Subject: [PATCH 07/24] Add forklift URDF --- pkgs/node_helpers/sample_urdfs/forklift/cube.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part1.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part10.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part12.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part13a.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part14b.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part15a.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part16b.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part17.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part18.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part2.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part3.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part4.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part5.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part6.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part7.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part8.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part9.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_16.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_18.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_19.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_20.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_22.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_25.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_26.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_28.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_29.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_30.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_31.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_32.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_33.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_34.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_35.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_36.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_37.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_38.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_39.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_40.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_41.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_42.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_43.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_44.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_45.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_46.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_47.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_48.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_49.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_50.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_51.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_52.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_53.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_54.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_56.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/part_57.stl | 3 +++ pkgs/node_helpers/sample_urdfs/forklift/robot.urdf | 3 +++ 55 files changed, 165 insertions(+) create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/cube.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part1.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part10.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part12.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part13a.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part14b.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part15a.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part16b.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part17.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part18.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part2.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part3.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part4.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part5.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part6.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part7.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part8.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part9.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_16.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_18.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_19.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_20.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_22.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_25.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_26.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_28.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_29.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_30.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_31.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_32.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_33.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_34.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_35.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_36.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_37.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_38.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_39.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_40.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_41.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_42.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_43.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_44.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_45.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_46.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_47.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_48.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_49.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_50.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_51.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_52.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_53.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_54.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_56.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/part_57.stl create mode 100644 pkgs/node_helpers/sample_urdfs/forklift/robot.urdf 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 From 1dd58df837ccfb5a68025a2cd973f41671eff2c0 Mon Sep 17 00:00:00 2001 From: Alex Thiele Date: Fri, 6 Dec 2024 15:24:12 -0800 Subject: [PATCH 08/24] Add urdf dependencies --- pkgs/node_helpers/package.xml | 4 ++++ pkgs/node_helpers/pyproject.toml | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) 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"] From 326bb82468d71a63eac56d9d9f6659e828184594 Mon Sep 17 00:00:00 2001 From: Alex Thiele Date: Fri, 6 Dec 2024 15:24:26 -0800 Subject: [PATCH 09/24] Add urdf tests --- .../integration/urdfs/__init__.py | 0 .../urdfs/example_urdf_constants.py | 25 ++ .../integration/urdfs/test_forklift.py | 58 ++++ .../unit/urdfs/test_loading.py | 120 ++++++++ .../unit/urdfs/test_urdf_constants.py | 271 ++++++++++++++++++ 5 files changed, 474 insertions(+) create mode 100644 pkgs/node_helpers/node_helpers_test/integration/urdfs/__init__.py create mode 100644 pkgs/node_helpers/node_helpers_test/integration/urdfs/example_urdf_constants.py create mode 100644 pkgs/node_helpers/node_helpers_test/integration/urdfs/test_forklift.py create mode 100644 pkgs/node_helpers/node_helpers_test/unit/urdfs/test_loading.py create mode 100644 pkgs/node_helpers/node_helpers_test/unit/urdfs/test_urdf_constants.py 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..a2fe3c2 --- /dev/null +++ b/pkgs/node_helpers/node_helpers_test/integration/urdfs/test_forklift.py @@ -0,0 +1,58 @@ +from collections.abc import Generator + +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=_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(), + ) From 776f36e978770ffa86bd31af7cf16527c52d4954 Mon Sep 17 00:00:00 2001 From: Alex Thiele Date: Fri, 6 Dec 2024 15:24:45 -0800 Subject: [PATCH 10/24] Add urdf module fixture --- .../node_helpers/testing/__init__.py | 14 ++ .../testing/urdf_frame_validation.py | 95 ++++++++++ .../testing/urdf_module_fixture.py | 175 ++++++++++++++++++ 3 files changed, 284 insertions(+) create mode 100644 pkgs/node_helpers/node_helpers/testing/urdf_frame_validation.py create mode 100644 pkgs/node_helpers/node_helpers/testing/urdf_module_fixture.py 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) From 5cb512158a6e9b4365d954a73f8d09662cb04223 Mon Sep 17 00:00:00 2001 From: Alex Thiele Date: Fri, 6 Dec 2024 15:25:22 -0800 Subject: [PATCH 11/24] Add `URDFModuleNodeFactory` --- .../node_helpers/launching/__init__.py | 1 + .../launching/urdf_module_launching.py | 124 ++++++++++++++++++ .../integration/urdfs/test_forklift.py | 3 +- 3 files changed, 127 insertions(+), 1 deletion(-) create mode 100644 pkgs/node_helpers/node_helpers/launching/urdf_module_launching.py 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_test/integration/urdfs/test_forklift.py b/pkgs/node_helpers/node_helpers_test/integration/urdfs/test_forklift.py index a2fe3c2..a098a9f 100644 --- a/pkgs/node_helpers/node_helpers_test/integration/urdfs/test_forklift.py +++ b/pkgs/node_helpers/node_helpers_test/integration/urdfs/test_forklift.py @@ -1,4 +1,5 @@ from collections.abc import Generator +from typing import cast import pytest from node_helpers.launching import URDFModuleNodeFactory @@ -25,7 +26,7 @@ def tf_client() -> Generator[TFClient, None, None]: def urdf_module() -> Generator[URDFModuleFixture, None, None]: yield from URDFModuleFixture.set_up( URDFModuleNodeFactory.Parameters( - namespace=_NAMESPACED_CONSTANTS.namespace, + namespace=cast(str, _NAMESPACED_CONSTANTS.namespace), urdf_constant_name=ForkliftURDF.registration_name, ), static_transforms=[], From 5e64a15bb6148b33a3a6dcb33e54b73c9f62532f Mon Sep 17 00:00:00 2001 From: Alex Thiele Date: Fri, 6 Dec 2024 15:25:47 -0800 Subject: [PATCH 12/24] Migrate URDFConstants --- .../node_helpers/node_helpers/urdfs/README.md | 13 ++ .../node_helpers/urdfs/__init__.py | 1 + .../node_helpers/urdfs/loading.py | 137 +++++++++++++++ .../node_helpers/urdfs/urdf_constants.py | 166 ++++++++++++++++++ 4 files changed, 317 insertions(+) create mode 100644 pkgs/node_helpers/node_helpers/urdfs/README.md create mode 100644 pkgs/node_helpers/node_helpers/urdfs/__init__.py create mode 100644 pkgs/node_helpers/node_helpers/urdfs/loading.py create mode 100644 pkgs/node_helpers/node_helpers/urdfs/urdf_constants.py 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("/", ""), + ) From aed183d455195ba772660cd7e9828b948e913401 Mon Sep 17 00:00:00 2001 From: Alex Thiele Date: Fri, 6 Dec 2024 16:52:54 -0800 Subject: [PATCH 13/24] Prevent a circular dependency --- .cruft.json | 2 +- docker/Dockerfile | 1 + .../node_helpers_showcase/launcher.py | 61 ++++++++++++- .../parameters/parameters.yaml | 23 +++++ .../node_helpers_showcase/rviz-config.rviz | 89 +++++++++++++++---- .../node_helpers/urdfs/loading.py | 7 +- 6 files changed, 162 insertions(+), 21 deletions(-) create mode 100644 launch-profiles/node_helpers_showcase/parameters/parameters.yaml diff --git a/.cruft.json b/.cruft.json index 3e06d3e..6ff1684 100644 --- a/.cruft.json +++ b/.cruft.json @@ -1,6 +1,6 @@ { "template": "https://github.com/UrbanMachine/create-ros-app.git", - "commit": "6a85fd934e7c6297d5fd717c545fad443cc4dfcf", + "commit": "5d14b64b8a4c2ac85f57a19dd962216de9c7a28a", "checkout": null, "context": { "cookiecutter": { diff --git a/docker/Dockerfile b/docker/Dockerfile index 6273fd3..2168b24 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -45,6 +45,7 @@ RUN --mount=type=cache,target="${APT_CACHE}" \ # Build tools build-essential \ git \ + git-lfs \ # Complementary ROS2 Python tools python3-colcon-common-extensions \ python3-colcon-mixin \ diff --git a/launch-profiles/node_helpers_showcase/launcher.py b/launch-profiles/node_helpers_showcase/launcher.py index 4e80f01..a6ea41b 100644 --- a/launch-profiles/node_helpers_showcase/launcher.py +++ b/launch-profiles/node_helpers_showcase/launcher.py @@ -1,12 +1,69 @@ """Launch nodes for this launch profile.""" +from pathlib import Path + from launch import LaunchDescription from launch_ros.actions import Node +# Import the forklift URDF module so it register itself with the URDFConstants +from node_helpers import launching +from node_helpers.parameters import ParameterLoader +from pydantic import BaseModel + + +class MetaParameters(BaseModel): + """This is a great place to put parameters that affect the generation of the launch + file. Don't put node specific configuration in here, rather, put configuration for + what nodes you want to be created in the first place. + + Read more about this functionality under docs/parameters.rst + """ + + urdf_modules_to_load: list[launching.URDFModuleNodeFactory.Parameters] + """This is an example of dynamically loading an arbitrary number of URDFs. + + This is set in the `/robot/launch-profile/parameters.yaml` under `meta_parameters`. + """ + def generate_launch_description() -> LaunchDescription: - rviz_config = "/robot/launch-profile/rviz-config.rviz" + # Create a parameter loader to parse all yaml files in the launch-profile/parameters + # directory, and then apply overrides from the override file, if one exists. + param_loader: ParameterLoader[MetaParameters] = ParameterLoader( + parameters_directory=Path("/robot/launch-profile/parameters/"), + override_file=Path("/robot/launch-profile/parameters.override.yaml"), + meta_parameters_schema=MetaParameters, + ) + + rviz_config = launching.required_file("/robot/launch-profile/rviz-config.rviz") + + urdf_node_factories = ( + launching.URDFModuleNodeFactory(parameters=node_factory_params) + for node_factory_params in param_loader.meta_parameters.urdf_modules_to_load + ) + urdf_nodes = [] + for urdf_node_factory in urdf_node_factories: + urdf_nodes += urdf_node_factory.create_nodes() + launch_description = [ - Node(package="rviz2", executable="rviz2", arguments=["-d", [rviz_config]]), + *urdf_nodes, + Node( + package="node_helpers", + executable="run_ExampleNode", + name="ExampleNode", + parameters=[param_loader.ros_parameters_file], + namespace="example_node_namespace", + ), + Node( + package="rviz2", + executable="rviz2", + arguments=["-d", [str(rviz_config)]], + ), + Node( + namespace="urdf_arrangement", + package="node_helpers", + executable="interactive_transform_publisher", + parameters=[param_loader.ros_parameters_file], + ), ] return LaunchDescription(launch_description) diff --git a/launch-profiles/node_helpers_showcase/parameters/parameters.yaml b/launch-profiles/node_helpers_showcase/parameters/parameters.yaml new file mode 100644 index 0000000..c939132 --- /dev/null +++ b/launch-profiles/node_helpers_showcase/parameters/parameters.yaml @@ -0,0 +1,23 @@ +meta_parameters: + urdf_modules_to_load: + # URDF's are attached to base_link down below in the + - namespace: "example_node_namespace" + urdf_constant_name: "forklift" + + +example_node_namespace: + ExampleNode: + ros__parameters: + root_config: + publish_value: "hello" + publish_hz: 10.0 + +urdf_arrangement: + interactive_transform_publisher: + ros__parameters: + static_transforms_file: /robot/persistent/interactive_transforms.json + scale_factor: 1.0 + tf_publish_frequency: 1.0 + transforms: + # Mounts the forklift urdf to the base_link so it can be visualized in rviz + - "base_link:example_node_namespace.forklift_body" \ No newline at end of file diff --git a/launch-profiles/node_helpers_showcase/rviz-config.rviz b/launch-profiles/node_helpers_showcase/rviz-config.rviz index 5da66cd..7a6847a 100644 --- a/launch-profiles/node_helpers_showcase/rviz-config.rviz +++ b/launch-profiles/node_helpers_showcase/rviz-config.rviz @@ -4,10 +4,10 @@ Panels: Name: Displays Property Tree Widget: Expanded: - - /Global Options1 - - /Status1 + - /InteractiveMarkers1 + - /InteractiveTransforms1 Splitter Ratio: 0.5 - Tree Height: 1064 + Tree Height: 1108 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -47,10 +47,69 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /example_node_namespace/urdf_0/robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + example_node_namespace.forklift_body: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + example_node_namespace.forks: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + example_node_namespace.forks_origin: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: ExampleURDF + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/InteractiveMarkers + Enable Transparency: true + Enabled: true + Interactive Markers Namespace: "" + Name: InteractiveMarkers + Show Axes: false + Show Descriptions: true + Show Visual Aids: false + Value: true + - Class: rviz_default_plugins/InteractiveMarkers + Enable Transparency: true + Enabled: true + Interactive Markers Namespace: /urdf_arrangement + Name: InteractiveTransforms + Show Axes: false + Show Descriptions: true + Show Visual Aids: false + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: map + Fixed Frame: base_link Frame Rate: 30 Name: root Tools: @@ -93,33 +152,33 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 10 + Distance: 8.649819374084473 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0 - Y: 0 - Z: 0 + X: -0.4261273145675659 + Y: -0.3684071898460388 + Z: 0.5727261900901794 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.785398006439209 + Pitch: 0.5403984189033508 Target Frame: Value: Orbit (rviz) - Yaw: 0.785398006439209 + Yaw: 1.0753980875015259 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1355 + Height: 1331 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000156000004b1fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000004b1000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004b1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000004b1000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f60000003efc0100000002fb0000000800540069006d00650100000000000004f60000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000285000004b100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001c5000004ddfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000004dd000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000499fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000499000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009f40000003efc0100000002fb0000000800540069006d00650000000000000009f40000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000829000004dd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -128,6 +187,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1270 - X: 3842 - Y: 40 + Width: 2548 + X: 8 + Y: 64 diff --git a/pkgs/node_helpers/node_helpers/urdfs/loading.py b/pkgs/node_helpers/node_helpers/urdfs/loading.py index e75ac09..349b6bb 100644 --- a/pkgs/node_helpers/node_helpers/urdfs/loading.py +++ b/pkgs/node_helpers/node_helpers/urdfs/loading.py @@ -4,8 +4,6 @@ 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" @@ -20,7 +18,10 @@ def load_urdf(package: str, relative_urdf_path: Path | str) -> str: relative_urdf_path = Path(relative_urdf_path) package_root = get_package_share_directory(package) - urdf_file = required_file(package_root, relative_urdf_path) + urdf_file = Path(package_root, relative_urdf_path) + + if not urdf_file.is_file(): + raise FileNotFoundError(f"URDF file '{urdf_file!s}' not found") return urdf_file.read_text() From d8d4af59d4d4c39c536c73ee4ded57871c9346f1 Mon Sep 17 00:00:00 2001 From: Alex Thiele Date: Sun, 8 Dec 2024 15:12:07 -0800 Subject: [PATCH 14/24] Add mimic tag --- pkgs/node_helpers/node_helpers/urdfs/loading.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/pkgs/node_helpers/node_helpers/urdfs/loading.py b/pkgs/node_helpers/node_helpers/urdfs/loading.py index 349b6bb..5227dce 100644 --- a/pkgs/node_helpers/node_helpers/urdfs/loading.py +++ b/pkgs/node_helpers/node_helpers/urdfs/loading.py @@ -11,6 +11,7 @@ _CHILD_TAG = "child" _NAME_KEY = "name" _LINK_KEY = "link" +_MIMIC_TAG = "mimic" def load_urdf(package: str, relative_urdf_path: Path | str) -> str: @@ -134,5 +135,9 @@ def prepend_namespace(urdf_str: str, namespace: str) -> str: "link", NAMESPACE_FMT.format(namespace=namespace, name=node.get(_LINK_KEY)), ) - + elif node.tag == _MIMIC_TAG: + node.set( + "joint", + NAMESPACE_FMT.format(namespace=namespace, name=node.get(_NAME_KEY)), + ) return ElementTree.tostring(urdf, encoding="unicode") From 261a0c1b0963ecb9d0b3860fd35962bd51ab7c5c Mon Sep 17 00:00:00 2001 From: Alex Thiele Date: Sun, 8 Dec 2024 15:22:18 -0800 Subject: [PATCH 15/24] Fix lint --- .../node_helpers/node_helpers_test/resources/urdfs/robot.urdf | 4 ++-- .../node_helpers/node_helpers_test/unit/urdfs/test_loading.py | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/pkgs/node_helpers/node_helpers_test/resources/urdfs/robot.urdf b/pkgs/node_helpers/node_helpers_test/resources/urdfs/robot.urdf index 37744a1..77d883a 100644 --- a/pkgs/node_helpers/node_helpers_test/resources/urdfs/robot.urdf +++ b/pkgs/node_helpers/node_helpers_test/resources/urdfs/robot.urdf @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:df4ea50d48402ad056343e980fb0ce717b644fbee6f1f2a3fc20952a253870e4 -size 3191 +oid sha256:a491f06f81ec4d696b9a1ea2eb8297bcd04ffcb8c9878a70cfeee3c261518d0b +size 4452 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 index a8d8e86..14d9b97 100644 --- a/pkgs/node_helpers/node_helpers_test/unit/urdfs/test_loading.py +++ b/pkgs/node_helpers/node_helpers_test/unit/urdfs/test_loading.py @@ -3,8 +3,8 @@ 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"] +EXPECTED_JOINT_NAMES = ["shuttle1-joint", "clamp1-joint", "clamp-mimic-joint"] +EXPECTED_LINK_NAMES = ["base_link", "shuttle1", "clamp1", "clamp-mimic"] def test_fix_urdf_paths_makes_path_replacements() -> None: From e72f23ea8d6cef1cf9bb4d876967f560b08c5d9e Mon Sep 17 00:00:00 2001 From: Alex Thiele Date: Sun, 8 Dec 2024 15:24:58 -0800 Subject: [PATCH 16/24] Update cruft --- .cruft.json | 2 +- .gitattributes | 1 + docker/Dockerfile | 1 + .../node_helpers_showcase/launcher.py | 9 ++++-- .../parameters/parameters.yaml | 4 +-- .../node_helpers/node_helpers/example_urdf.py | 31 +++++++++++++++++++ 6 files changed, 42 insertions(+), 6 deletions(-) create mode 100644 pkgs/node_helpers/node_helpers/example_urdf.py diff --git a/.cruft.json b/.cruft.json index 6ff1684..518b195 100644 --- a/.cruft.json +++ b/.cruft.json @@ -1,6 +1,6 @@ { "template": "https://github.com/UrbanMachine/create-ros-app.git", - "commit": "5d14b64b8a4c2ac85f57a19dd962216de9c7a28a", + "commit": "f6732cb66da0ffbdd3dafde95de077954ffcc63c", "checkout": null, "context": { "cookiecutter": { diff --git a/.gitattributes b/.gitattributes index dc0463f..eea2fd9 100644 --- a/.gitattributes +++ b/.gitattributes @@ -17,6 +17,7 @@ *.mp3 filter=lfs diff=lfs merge=lfs -binary *.mp4 filter=lfs diff=lfs merge=lfs -binary *.ogg filter=lfs diff=lfs merge=lfs -binary +*.dae filter=lfs diff=lfs merge=lfs -binary # ReadTheDocs does not support Git LFS docs/** !filter !diff !merge binary diff --git a/docker/Dockerfile b/docker/Dockerfile index 2168b24..6eeabf7 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -105,6 +105,7 @@ COPY pkgs/node_helpers/pyproject.toml pkgs/node_helpers/pyproject.toml WORKDIR /ros-git-deps/ ##################### 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/launch-profiles/node_helpers_showcase/launcher.py b/launch-profiles/node_helpers_showcase/launcher.py index a6ea41b..c485fc0 100644 --- a/launch-profiles/node_helpers_showcase/launcher.py +++ b/launch-profiles/node_helpers_showcase/launcher.py @@ -4,12 +4,13 @@ from launch import LaunchDescription from launch_ros.actions import Node +from pydantic import BaseModel -# Import the forklift URDF module so it register itself with the URDFConstants from node_helpers import launching from node_helpers.parameters import ParameterLoader -from pydantic import BaseModel +# Import the forklift URDF module so it register itself with the URDFConstants +from node_helpers import example_urdf class MetaParameters(BaseModel): """This is a great place to put parameters that affect the generation of the launch @@ -38,7 +39,9 @@ def generate_launch_description() -> LaunchDescription: rviz_config = launching.required_file("/robot/launch-profile/rviz-config.rviz") urdf_node_factories = ( - launching.URDFModuleNodeFactory(parameters=node_factory_params) + launching.URDFModuleNodeFactory( + parameters=node_factory_params + ) for node_factory_params in param_loader.meta_parameters.urdf_modules_to_load ) urdf_nodes = [] diff --git a/launch-profiles/node_helpers_showcase/parameters/parameters.yaml b/launch-profiles/node_helpers_showcase/parameters/parameters.yaml index c939132..d05ba29 100644 --- a/launch-profiles/node_helpers_showcase/parameters/parameters.yaml +++ b/launch-profiles/node_helpers_showcase/parameters/parameters.yaml @@ -9,8 +9,8 @@ example_node_namespace: ExampleNode: ros__parameters: root_config: - publish_value: "hello" - publish_hz: 10.0 + forklift_speed: 0.25 + forklift_max_extent: 0.5 urdf_arrangement: interactive_transform_publisher: diff --git a/pkgs/node_helpers/node_helpers/example_urdf.py b/pkgs/node_helpers/node_helpers/example_urdf.py new file mode 100644 index 0000000..24e870e --- /dev/null +++ b/pkgs/node_helpers/node_helpers/example_urdf.py @@ -0,0 +1,31 @@ +"""This module demonstrates how to define a `node_helpers` URDFConstants object for a +basic URDF, specify necessary joint and frame names, and register the URDF with the +`node_helpers` package. + +By registering it, the URDF can be accessed _by name_ in configuration files. +""" + +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(), +) From 6f50797e1e646e2efca0224f2a5b11c754e1cedb Mon Sep 17 00:00:00 2001 From: Alex Thiele Date: Sun, 8 Dec 2024 15:26:26 -0800 Subject: [PATCH 17/24] Remove unnecessary `echo` --- docker/Dockerfile | 1 - 1 file changed, 1 deletion(-) diff --git a/docker/Dockerfile b/docker/Dockerfile index 6eeabf7..2168b24 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -105,7 +105,6 @@ COPY pkgs/node_helpers/pyproject.toml pkgs/node_helpers/pyproject.toml WORKDIR /ros-git-deps/ ##################### 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 From 5b84222c6c18b63347788c73849d2a1c41a32fcf Mon Sep 17 00:00:00 2001 From: Alex Thiele Date: Sun, 8 Dec 2024 15:32:21 -0800 Subject: [PATCH 18/24] Have node_helpers more closely follow the template --- .../node_helpers/nodes/__init__.py | 2 + .../node_helpers/nodes/node_helpers_node.py | 65 +++++++++++++++++++ pkgs/node_helpers/pyproject.toml | 1 + 3 files changed, 68 insertions(+) create mode 100644 pkgs/node_helpers/node_helpers/nodes/node_helpers_node.py diff --git a/pkgs/node_helpers/node_helpers/nodes/__init__.py b/pkgs/node_helpers/node_helpers/nodes/__init__.py index bc073e5..9494dc0 100644 --- a/pkgs/node_helpers/node_helpers/nodes/__init__.py +++ b/pkgs/node_helpers/node_helpers/nodes/__init__.py @@ -6,9 +6,11 @@ TransformsFile, ) from .sound_player import SoundPlayer +from .node_helpers_node import ExampleNode __all__ = [ "HelpfulNode", + "ExampleNode", "InteractiveTransformPublisher", "TransformModel", "TransformsFile", diff --git a/pkgs/node_helpers/node_helpers/nodes/node_helpers_node.py b/pkgs/node_helpers/node_helpers/nodes/node_helpers_node.py new file mode 100644 index 0000000..f9a8fb8 --- /dev/null +++ b/pkgs/node_helpers/node_helpers/nodes/node_helpers_node.py @@ -0,0 +1,65 @@ +"""This is a total example node to show off some simple node_helpers features. It's not +meant to be a comprehensive example, but rather a simple one to show off some of the +features of the node_helpers package. +""" + +from typing import Any + +from sensor_msgs.msg import JointState + +from node_helpers.nodes import HelpfulNode +from pydantic import BaseModel +from node_helpers.spinning import create_spin_function +from rclpy.qos import qos_profile_services_default + +from node_helpers.example_urdf import ForkliftURDF + +class ExampleNode(HelpfulNode): + + class Parameters(BaseModel): + # Define your ROS parameters here + forklift_speed: float # m/s + forklift_max_extent: float + + def __init__(self, **kwargs: Any): + super().__init__("ExampleNode", **kwargs) + # Load parameters from the ROS parameter server + self.params = self.declare_from_pydantic_model(self.Parameters, "root_config") + self.urdf = ForkliftURDF.with_namespace(self.get_namespace()) + + # Track the forks position and direction, so we can move them up and down + self.forklift_position = 0.0 + self.forklift_direction = 1 + + # Create publishers + self.joint_state_publisher = self.create_publisher( + JointState, + "desired_joint_states", + qos_profile_services_default + ) + + # Create a timer to publish joint values + self._publish_hz = 20 + self.create_timer(1 / self._publish_hz, self.on_publish_joints) + + + def on_publish_joints(self) -> None: + if self.forklift_position >= self.params.forklift_max_extent: + self.forklift_direction = -1 + elif self.forklift_position <= 0: + self.forklift_direction = 1 + + self.forklift_position += self.forklift_direction * self.params.forklift_speed / self._publish_hz + + joint_positions = { + self.urdf.joints.FORKS: self.forklift_position + } + + self.joint_state_publisher.publish( + JointState( + name=list(joint_positions.keys()), + position = list(joint_positions.values()) + ) + ) + +main = create_spin_function(ExampleNode) \ No newline at end of file diff --git a/pkgs/node_helpers/pyproject.toml b/pkgs/node_helpers/pyproject.toml index ccd7c53..5d21dea 100644 --- a/pkgs/node_helpers/pyproject.toml +++ b/pkgs/node_helpers/pyproject.toml @@ -19,6 +19,7 @@ transforms3d = "^0.4.2" interactive_transform_publisher = "node_helpers.nodes.interactive_transform_publisher:main" sound_player = "node_helpers.nodes.sound_player:main" placeholder = "node_helpers.nodes.placeholder:main" +run_ExampleNode = "node_helpers.nodes.node_helpers_node:main" [tool.colcon-poetry-ros.data-files] "share/ament_index/resource_index/packages" = ["resource/node_helpers"] From 2c882a8074ce73811ff70c9ef6e26b8eede11b28 Mon Sep 17 00:00:00 2001 From: Alex Thiele Date: Sun, 8 Dec 2024 15:32:51 -0800 Subject: [PATCH 19/24] Lint fixes --- .../node_helpers_showcase/launcher.py | 9 +++---- .../node_helpers/node_helpers/example_urdf.py | 1 + .../node_helpers/nodes/__init__.py | 2 +- .../node_helpers/nodes/node_helpers_node.py | 25 ++++++++----------- 4 files changed, 16 insertions(+), 21 deletions(-) diff --git a/launch-profiles/node_helpers_showcase/launcher.py b/launch-profiles/node_helpers_showcase/launcher.py index c485fc0..a6ea41b 100644 --- a/launch-profiles/node_helpers_showcase/launcher.py +++ b/launch-profiles/node_helpers_showcase/launcher.py @@ -4,13 +4,12 @@ from launch import LaunchDescription from launch_ros.actions import Node -from pydantic import BaseModel +# Import the forklift URDF module so it register itself with the URDFConstants from node_helpers import launching from node_helpers.parameters import ParameterLoader +from pydantic import BaseModel -# Import the forklift URDF module so it register itself with the URDFConstants -from node_helpers import example_urdf class MetaParameters(BaseModel): """This is a great place to put parameters that affect the generation of the launch @@ -39,9 +38,7 @@ def generate_launch_description() -> LaunchDescription: rviz_config = launching.required_file("/robot/launch-profile/rviz-config.rviz") urdf_node_factories = ( - launching.URDFModuleNodeFactory( - parameters=node_factory_params - ) + launching.URDFModuleNodeFactory(parameters=node_factory_params) for node_factory_params in param_loader.meta_parameters.urdf_modules_to_load ) urdf_nodes = [] diff --git a/pkgs/node_helpers/node_helpers/example_urdf.py b/pkgs/node_helpers/node_helpers/example_urdf.py index 24e870e..493ad8a 100644 --- a/pkgs/node_helpers/node_helpers/example_urdf.py +++ b/pkgs/node_helpers/node_helpers/example_urdf.py @@ -22,6 +22,7 @@ class ForkliftFrames(NamedTuple): FORKS_ORIGIN: str = "forks_origin" FORKS: str = "forks" + ForkliftURDF = URDFConstants[ForkliftJoints, ForkliftFrames]( from_package="node_helpers", registration_name="forklift", diff --git a/pkgs/node_helpers/node_helpers/nodes/__init__.py b/pkgs/node_helpers/node_helpers/nodes/__init__.py index 9494dc0..75e1c30 100644 --- a/pkgs/node_helpers/node_helpers/nodes/__init__.py +++ b/pkgs/node_helpers/node_helpers/nodes/__init__.py @@ -5,8 +5,8 @@ TransformModel, TransformsFile, ) -from .sound_player import SoundPlayer from .node_helpers_node import ExampleNode +from .sound_player import SoundPlayer __all__ = [ "HelpfulNode", diff --git a/pkgs/node_helpers/node_helpers/nodes/node_helpers_node.py b/pkgs/node_helpers/node_helpers/nodes/node_helpers_node.py index f9a8fb8..dbcaba1 100644 --- a/pkgs/node_helpers/node_helpers/nodes/node_helpers_node.py +++ b/pkgs/node_helpers/node_helpers/nodes/node_helpers_node.py @@ -5,17 +5,16 @@ from typing import Any +from pydantic import BaseModel +from rclpy.qos import qos_profile_services_default from sensor_msgs.msg import JointState +from node_helpers.example_urdf import ForkliftURDF from node_helpers.nodes import HelpfulNode -from pydantic import BaseModel from node_helpers.spinning import create_spin_function -from rclpy.qos import qos_profile_services_default -from node_helpers.example_urdf import ForkliftURDF class ExampleNode(HelpfulNode): - class Parameters(BaseModel): # Define your ROS parameters here forklift_speed: float # m/s @@ -33,33 +32,31 @@ def __init__(self, **kwargs: Any): # Create publishers self.joint_state_publisher = self.create_publisher( - JointState, - "desired_joint_states", - qos_profile_services_default + JointState, "desired_joint_states", qos_profile_services_default ) # Create a timer to publish joint values self._publish_hz = 20 self.create_timer(1 / self._publish_hz, self.on_publish_joints) - def on_publish_joints(self) -> None: if self.forklift_position >= self.params.forklift_max_extent: self.forklift_direction = -1 elif self.forklift_position <= 0: self.forklift_direction = 1 - self.forklift_position += self.forklift_direction * self.params.forklift_speed / self._publish_hz + self.forklift_position += ( + self.forklift_direction * self.params.forklift_speed / self._publish_hz + ) - joint_positions = { - self.urdf.joints.FORKS: self.forklift_position - } + joint_positions = {self.urdf.joints.FORKS: self.forklift_position} self.joint_state_publisher.publish( JointState( name=list(joint_positions.keys()), - position = list(joint_positions.values()) + position=list(joint_positions.values()), ) ) -main = create_spin_function(ExampleNode) \ No newline at end of file + +main = create_spin_function(ExampleNode) From aec79e7ee526a3307dd0fe567abddd8206260712 Mon Sep 17 00:00:00 2001 From: Alex Thiele Date: Sun, 8 Dec 2024 22:36:44 -0800 Subject: [PATCH 20/24] Fix tests --- .../integration/urdfs/example_urdf_constants.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index 753fc0d..cc3d1ec 100644 --- 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 @@ -18,7 +18,7 @@ class ForkliftFrames(NamedTuple): ForkliftURDF = URDFConstants[ForkliftJoints, ForkliftFrames]( from_package="node_helpers", - registration_name="forklift", + registration_name="test_forklift", urdf_paths=[(None, "sample_urdfs/forklift/robot.urdf")], joints=ForkliftJoints(), frames=ForkliftFrames(), From 319d41d904fb96e59c034e6691f097250cc45164 Mon Sep 17 00:00:00 2001 From: Alex Thiele Date: Sun, 8 Dec 2024 22:39:17 -0800 Subject: [PATCH 21/24] Update template --- .cruft.json | 2 +- .gitattributes | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/.cruft.json b/.cruft.json index 518b195..cde929e 100644 --- a/.cruft.json +++ b/.cruft.json @@ -1,6 +1,6 @@ { "template": "https://github.com/UrbanMachine/create-ros-app.git", - "commit": "f6732cb66da0ffbdd3dafde95de077954ffcc63c", + "commit": "3d4731e5661e8cbac11d71c60c8e925a989c150c", "checkout": null, "context": { "cookiecutter": { diff --git a/.gitattributes b/.gitattributes index eea2fd9..2f8fe14 100644 --- a/.gitattributes +++ b/.gitattributes @@ -18,6 +18,8 @@ *.mp4 filter=lfs diff=lfs merge=lfs -binary *.ogg filter=lfs diff=lfs merge=lfs -binary *.dae filter=lfs diff=lfs merge=lfs -binary +*.usd filter=lfs diff=lfs merge=lfs -binary + # ReadTheDocs does not support Git LFS docs/** !filter !diff !merge binary From 612ed60bb454da353ade51a71884bb562350ce6f Mon Sep 17 00:00:00 2001 From: Alex Thiele Date: Mon, 9 Dec 2024 09:36:30 -0800 Subject: [PATCH 22/24] Fix bug with `mimic` joint namespacing implementation --- pkgs/node_helpers/node_helpers/urdfs/loading.py | 3 ++- .../node_helpers_test/unit/urdfs/test_loading.py | 11 +++++++++++ 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/pkgs/node_helpers/node_helpers/urdfs/loading.py b/pkgs/node_helpers/node_helpers/urdfs/loading.py index 5227dce..2c961f7 100644 --- a/pkgs/node_helpers/node_helpers/urdfs/loading.py +++ b/pkgs/node_helpers/node_helpers/urdfs/loading.py @@ -11,6 +11,7 @@ _CHILD_TAG = "child" _NAME_KEY = "name" _LINK_KEY = "link" +_JOINT_KEY = "joint" _MIMIC_TAG = "mimic" @@ -138,6 +139,6 @@ def prepend_namespace(urdf_str: str, namespace: str) -> str: elif node.tag == _MIMIC_TAG: node.set( "joint", - NAMESPACE_FMT.format(namespace=namespace, name=node.get(_NAME_KEY)), + NAMESPACE_FMT.format(namespace=namespace, name=node.get(_JOINT_KEY)), ) return ElementTree.tostring(urdf, encoding="unicode") 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 index 14d9b97..f20be78 100644 --- a/pkgs/node_helpers/node_helpers_test/unit/urdfs/test_loading.py +++ b/pkgs/node_helpers/node_helpers_test/unit/urdfs/test_loading.py @@ -118,3 +118,14 @@ def test_prepend_namespace() -> None: ) ) assert len(expected_changes) == len(actual_changes) + +def test_mimics_are_prepended() -> None: + """Validate that 'mimic' style joints are prepended with the namespace as well.""" + + urdf_text: str = GENERIC_URDF.read_text() + namespace = "cool_namespace" + + mimic_joint = "clamp1-joint" + assert f"{namespace}.{mimic_joint}" not in urdf_text + modified = urdf_helpers.prepend_namespace(urdf_text, namespace=namespace) + assert modified.count(f'mimic joint="{namespace}.{mimic_joint}"') == 1 From a41f3d25509ec16f470e722662f8e3fdd4317ca9 Mon Sep 17 00:00:00 2001 From: Alex Thiele Date: Mon, 9 Dec 2024 09:37:57 -0800 Subject: [PATCH 23/24] Lint --- pkgs/node_helpers/node_helpers_test/unit/urdfs/test_loading.py | 1 + 1 file changed, 1 insertion(+) 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 index f20be78..257e1e5 100644 --- a/pkgs/node_helpers/node_helpers_test/unit/urdfs/test_loading.py +++ b/pkgs/node_helpers/node_helpers_test/unit/urdfs/test_loading.py @@ -119,6 +119,7 @@ def test_prepend_namespace() -> None: ) assert len(expected_changes) == len(actual_changes) + def test_mimics_are_prepended() -> None: """Validate that 'mimic' style joints are prepended with the namespace as well.""" From 2b62189d9be13e04125cca5aa89ae2a455e0c0c9 Mon Sep 17 00:00:00 2001 From: Alex Thiele Date: Thu, 12 Dec 2024 08:51:37 -0800 Subject: [PATCH 24/24] Add template changes --- .cruft.json | 2 +- docker-compose.yaml | 2 ++ docker/_shared.sh | 23 +++++++++++++++++++++++ docker/launch | 13 ++++--------- docker/reload-ros-nodes | 37 +++++++++++++++++++++++++++++++++++++ docs/about_template.md | 3 +++ 6 files changed, 70 insertions(+), 10 deletions(-) create mode 100755 docker/reload-ros-nodes diff --git a/.cruft.json b/.cruft.json index cde929e..1b0bce9 100644 --- a/.cruft.json +++ b/.cruft.json @@ -1,6 +1,6 @@ { "template": "https://github.com/UrbanMachine/create-ros-app.git", - "commit": "3d4731e5661e8cbac11d71c60c8e925a989c150c", + "commit": "93e541631403e2c2c091c65d6583d61bd85a472c", "checkout": null, "context": { "cookiecutter": { diff --git a/docker-compose.yaml b/docker-compose.yaml index 299260b..2e408ab 100644 --- a/docker-compose.yaml +++ b/docker-compose.yaml @@ -38,6 +38,8 @@ services: DISPLAY: $DISPLAY # Necessary for PulseAudio passthrough PULSE_SERVER: "unix:/pulse-socket" + # Enable serial passthrough + privileged: true # Gives the container access to kernel capabilities, useful for most robots network_mode: host cap_add: diff --git a/docker/_shared.sh b/docker/_shared.sh index d4e6ad8..8314171 100644 --- a/docker/_shared.sh +++ b/docker/_shared.sh @@ -71,4 +71,27 @@ function deploy_and_wait { done echo "Another process brought down the stack." >&2 return 1 +} + +# Shows the user the available launch profiles +function launch_profiles_helper_msg { + echo "Available launch profiles are:" >&2 + # shellcheck disable=SC2012 + ls -1 launch-profiles/ | sed 's/^/ - /' >&2 + echo "" >&2 + echo "Read more about 'launch-profiles' under 'docs/about_template.md'" >&2 + exit 1 +} + +# Inform the user that the chosen launch profile is invalid if it is not a directory +function validate_launch_profile { + local chosen_profile + chosen_profile="$1" + + # Check if the chosen profile is a directory under 'launch-profiles' + if [[ ! -d "launch-profiles/${chosen_profile}" ]]; then + echo "Error: '${chosen_profile}' is not a valid launch profile." >&2 + echo "It should be a directory under 'launch-profiles/'." >&2 + launch_profiles_helper_msg + fi } \ No newline at end of file diff --git a/docker/launch b/docker/launch index 828fcc5..d48b1b7 100755 --- a/docker/launch +++ b/docker/launch @@ -24,14 +24,11 @@ set -o errexit set -o pipefail set -o nounset +source docker/_shared.sh + function usage { echo "Usage: docker/launch [--no-pull] " >&2 - echo "Available launch profiles are:" >&2 - # shellcheck disable=SC2012 - ls -1 launch-profiles/ | sed 's/^/ - /' >&2 - echo "" >&2 - echo "Read more about 'launch-profiles' under 'docs/about_template.md'" >&2 - exit 1 + launch_profiles_helper_msg } function main { @@ -64,9 +61,7 @@ function main { echo "Missing argument, specify a directory under 'launch-profiles/'" >&2 usage fi - - source docker/_shared.sh - + validate_launch_profile "${launch_profile}" # To reduce downtime, build the latest images before stopping any existing stacks. if [[ "${pull_upstream_images}" = true ]]; then diff --git a/docker/reload-ros-nodes b/docker/reload-ros-nodes new file mode 100755 index 0000000..3934d5a --- /dev/null +++ b/docker/reload-ros-nodes @@ -0,0 +1,37 @@ +#!/usr/bin/env bash + +# Builds and re-runs the ROS nodes container. This is useful if you want to restart +# the container over and over, oftentimes when developing on you ROS code. +# The container is only re-run if the image has changed. +# +# Usage: +# docker/reload-ros-nodes +# +# Examples: +# docker/reload-ros-nodes node_helpers_showcase + +set -o errexit +set -o pipefail +set -o nounset + +source docker/_shared.sh + +function main { + local launch_profile + launch_profile="${1:-}" + + if [[ -z "${launch_profile}" ]]; then + echo "Missing argument, specify a directory under 'launch-profiles/'" >&2 + echo "Usage: docker/reload-ros-nodes " >&2 + launch_profiles_helper_msg + fi + validate_launch_profile "${launch_profile}" + + build_images # Build any images that need to be built + enable_display_passthrough # Enable passthrough for the stack + + export LAUNCH_PROFILE="${launch_profile}" + docker compose up -d --force-recreate ros-nodes +} + +main "${@}" diff --git a/docs/about_template.md b/docs/about_template.md index 493951b..3c0c3f1 100644 --- a/docs/about_template.md +++ b/docs/about_template.md @@ -31,6 +31,9 @@ Here's a quick guide on the features of this template # Enter a currently running ROS container to poke around docker/exec + + # Rebuild and restart the ROS nodes in the container, useful for fast development + docker/reload-ros-nodes ``` More usage examples for the above scripts are documented at the top of the script files.