diff --git a/.cruft.json b/.cruft.json index 6ff1684..cde929e 100644 --- a/.cruft.json +++ b/.cruft.json @@ -1,6 +1,6 @@ { "template": "https://github.com/UrbanMachine/create-ros-app.git", - "commit": "5d14b64b8a4c2ac85f57a19dd962216de9c7a28a", + "commit": "3d4731e5661e8cbac11d71c60c8e925a989c150c", "checkout": null, "context": { "cookiecutter": { diff --git a/.gitattributes b/.gitattributes index dc0463f..2f8fe14 100644 --- a/.gitattributes +++ b/.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 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..493ad8a --- /dev/null +++ b/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(), +) diff --git a/pkgs/node_helpers/node_helpers/nodes/__init__.py b/pkgs/node_helpers/node_helpers/nodes/__init__.py index bc073e5..75e1c30 100644 --- a/pkgs/node_helpers/node_helpers/nodes/__init__.py +++ b/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", 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..dbcaba1 --- /dev/null +++ b/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) 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") 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(), 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: 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"]