Add support for mimic tags in URDFs (#9)

Author: apockill

.cruft.json

@@ -1,6 +1,6 @@
 {
   "template": "https://github.com/UrbanMachine/create-ros-app.git",
-  "commit": "5d14b64b8a4c2ac85f57a19dd962216de9c7a28a",
+  "commit": "3d4731e5661e8cbac11d71c60c8e925a989c150c",
   "checkout": null,
   "context": {
     "cookiecutter": {

.gitattributes

@@ -17,6 +17,9 @@
 *.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
+*.usd filter=lfs diff=lfs merge=lfs -binary
+
 # ReadTheDocs does not support Git LFS
 docs/** !filter !diff !merge binary
 

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:

pkgs/node_helpers/node_helpers/example_urdf.py

@@ -0,0 +1,32 @@
+"""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(),
+)

pkgs/node_helpers/node_helpers/nodes/__init__.py

@@ -5,10 +5,12 @@
     TransformModel,
     TransformsFile,
 )
+from .node_helpers_node import ExampleNode
 from .sound_player import SoundPlayer
 
 __all__ = [
     "HelpfulNode",
+    "ExampleNode",
     "InteractiveTransformPublisher",
     "TransformModel",
     "TransformsFile",

pkgs/node_helpers/node_helpers/nodes/node_helpers_node.py

@@ -0,0 +1,62 @@
+"""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 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 node_helpers.spinning import create_spin_function
+
+
+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)

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

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(),

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

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: