From 939345dac846bf8fccc8d90868fdf62c72909aa5 Mon Sep 17 00:00:00 2001 From: Zeerek Date: Wed, 19 Aug 2026 17:15:17 -0700 Subject: [PATCH 1/2] Add very very basic polymath_kinematics ros2 node that we can fill with projections --- polymath_kinematics_ros2/CMakeLists.txt | 110 ++++++++++++++++++ polymath_kinematics_ros2/README.md | 15 +++ .../kinematics_node.hpp | 54 +++++++++ polymath_kinematics_ros2/package.xml | 24 ++++ .../src/kinematics_node.cpp | 56 +++++++++ .../test/catch2_compat.hpp | 25 ++++ .../test/test_kinematics_node.cpp | 75 ++++++++++++ 7 files changed, 359 insertions(+) create mode 100644 polymath_kinematics_ros2/CMakeLists.txt create mode 100644 polymath_kinematics_ros2/README.md create mode 100644 polymath_kinematics_ros2/include/polymath_kinematics_ros2/kinematics_node.hpp create mode 100644 polymath_kinematics_ros2/package.xml create mode 100644 polymath_kinematics_ros2/src/kinematics_node.cpp create mode 100644 polymath_kinematics_ros2/test/catch2_compat.hpp create mode 100644 polymath_kinematics_ros2/test/test_kinematics_node.cpp diff --git a/polymath_kinematics_ros2/CMakeLists.txt b/polymath_kinematics_ros2/CMakeLists.txt new file mode 100644 index 0000000..d9ba14f --- /dev/null +++ b/polymath_kinematics_ros2/CMakeLists.txt @@ -0,0 +1,110 @@ +# Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +cmake_minimum_required(VERSION 3.8) +project(polymath_kinematics_ros2) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) + add_link_options(-Wl,-no-undefined) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(polymath_kinematics REQUIRED) + +add_library(${PROJECT_NAME} SHARED + src/kinematics_node.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +target_link_libraries( + ${PROJECT_NAME} + PUBLIC + polymath_kinematics::polymath_kinematics + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + PRIVATE + rclcpp_components::component +) + +# Upstream rclcpp_components, not polymath_core's rclcpp_lifecycle_components wrapper: that +# package lives in polymath_core and is unavailable when this repo builds standalone in its own CI. +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "polymath::kinematics_ros2::KinematicsNode" + EXECUTABLE kinematics_node +) + +install( + TARGETS ${PROJECT_NAME} kinematics_node + EXPORT ${PROJECT_NAME}_TARGETS + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) +install( + EXPORT ${PROJECT_NAME}_TARGETS + NAMESPACE ${PROJECT_NAME}:: + DESTINATION share/${PROJECT_NAME}/cmake +) +install( + DIRECTORY include/ + DESTINATION include/ +) + +if(BUILD_TESTING) + include(CTest) + + # Jammy (22.04) ships Catch2 v2; every later Ubuntu ships v3. Override with -DBUILD_JAMMY=ON/OFF. + if(NOT DEFINED BUILD_JAMMY) + set(BUILD_JAMMY OFF) + if(EXISTS "/etc/os-release") + file(READ "/etc/os-release" OS_RELEASE) + string(REGEX MATCH "VERSION_CODENAME=([^\n\r]+)" MATCHED "${OS_RELEASE}") + if(CMAKE_MATCH_1) + string(TOLOWER "${CMAKE_MATCH_1}" UBUNTU_CODENAME) + if(UBUNTU_CODENAME STREQUAL "jammy") + set(BUILD_JAMMY ON) + endif() + endif() + endif() + endif() + + if(BUILD_JAMMY) + find_package(Catch2 2 REQUIRED) + else() + find_package(Catch2 3 REQUIRED) + endif() + include(Catch OPTIONAL) + + # test/catch2_compat.hpp bridges the v2/v3 header and Approx differences. + add_executable(test_kinematics_node test/test_kinematics_node.cpp) + target_link_libraries(test_kinematics_node PRIVATE ${PROJECT_NAME} Catch2::Catch2WithMain) + target_include_directories(test_kinematics_node PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/test) + if(COMMAND catch_discover_tests) + # PRE_TEST enumerates at ctest time, not during the build where a stale installed .so can win. + catch_discover_tests(test_kinematics_node DISCOVERY_MODE PRE_TEST) + else() + add_test(NAME test_kinematics_node COMMAND test_kinematics_node) + endif() +endif() + +ament_export_targets(${PROJECT_NAME}_TARGETS HAS_LIBRARY_TARGET) +ament_package() diff --git a/polymath_kinematics_ros2/README.md b/polymath_kinematics_ros2/README.md new file mode 100644 index 0000000..355f310 --- /dev/null +++ b/polymath_kinematics_ros2/README.md @@ -0,0 +1,15 @@ +# polymath_kinematics_ros2 + +ROS 2 layer over [polymath_kinematics](../polymath_kinematics/). + +**Placeholder.** `KinematicsNode` is a `LifecycleNode` whose transition callbacks are no-ops. It +declares no parameters, topics, or services — it exists so the build target, component +registration, and link against the models are already in place. + +```bash +ros2 run polymath_kinematics_ros2 kinematics_node +``` + +Registration uses upstream `rclcpp_components_register_node`, and the test plain Catch2, rather +than polymath_core's `rclcpp_lifecycle_components_register_node` and `polymath_test`. Both of +those live in polymath_core and are unavailable when this repository builds standalone in CI. diff --git a/polymath_kinematics_ros2/include/polymath_kinematics_ros2/kinematics_node.hpp b/polymath_kinematics_ros2/include/polymath_kinematics_ros2/kinematics_node.hpp new file mode 100644 index 0000000..468e848 --- /dev/null +++ b/polymath_kinematics_ros2/include/polymath_kinematics_ros2/kinematics_node.hpp @@ -0,0 +1,54 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +namespace polymath::kinematics_ros2 +{ + +/// Placeholder ROS 2 lifecycle wrapper around the polymath_kinematics models. +/// +/// The node declares no parameters, topics, or services yet. It exists so the ROS 2 layer has a +/// build target, a component registration, and a lifecycle contract to grow into; every transition +/// callback is currently a no-op that reports SUCCESS. +class KinematicsNode : public rclcpp_lifecycle::LifecycleNode +{ +public: + using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + + /// Construct the node. + /// \param options Node options supplied by rclcpp or by a component container. + explicit KinematicsNode(const rclcpp::NodeOptions & options); + + /// Allocate resources. No-op until the node gains interfaces. + /// \param state The lifecycle state being transitioned from. + CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + + /// Begin publishing. No-op until the node gains interfaces. + /// \param state The lifecycle state being transitioned from. + CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + + /// Stop publishing. No-op until the node gains interfaces. + /// \param state The lifecycle state being transitioned from. + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + + /// Release resources. No-op until the node gains interfaces. + /// \param state The lifecycle state being transitioned from. + CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; +}; + +} // namespace polymath::kinematics_ros2 diff --git a/polymath_kinematics_ros2/package.xml b/polymath_kinematics_ros2/package.xml new file mode 100644 index 0000000..ca30496 --- /dev/null +++ b/polymath_kinematics_ros2/package.xml @@ -0,0 +1,24 @@ + + + + polymath_kinematics_ros2 + 0.3.0 + ROS 2 layer over polymath_kinematics. Placeholder — the node is a lifecycle skeleton with no interfaces yet. + Polymath Engineering + Apache-2.0 + Zeerek Ahmad + + ament_cmake_auto + + lifecycle_msgs + polymath_kinematics + rclcpp + rclcpp_components + rclcpp_lifecycle + + catch2 + + + ament_cmake + + diff --git a/polymath_kinematics_ros2/src/kinematics_node.cpp b/polymath_kinematics_ros2/src/kinematics_node.cpp new file mode 100644 index 0000000..320fd17 --- /dev/null +++ b/polymath_kinematics_ros2/src/kinematics_node.cpp @@ -0,0 +1,56 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "polymath_kinematics_ros2/kinematics_node.hpp" + +#include "rclcpp_components/register_node_macro.hpp" + +namespace polymath::kinematics_ros2 +{ + +KinematicsNode::KinematicsNode(const rclcpp::NodeOptions & options) +: rclcpp_lifecycle::LifecycleNode("kinematics_node", options) +{} + +KinematicsNode::CallbackReturn KinematicsNode::on_configure(const rclcpp_lifecycle::State & state) +{ + (void)state; + RCLCPP_INFO(get_logger(), "configured (placeholder: nothing to set up yet)"); + return CallbackReturn::SUCCESS; +} + +KinematicsNode::CallbackReturn KinematicsNode::on_activate(const rclcpp_lifecycle::State & state) +{ + (void)state; + RCLCPP_INFO(get_logger(), "activated (placeholder: nothing to publish yet)"); + return CallbackReturn::SUCCESS; +} + +KinematicsNode::CallbackReturn KinematicsNode::on_deactivate(const rclcpp_lifecycle::State & state) +{ + (void)state; + RCLCPP_INFO(get_logger(), "deactivated"); + return CallbackReturn::SUCCESS; +} + +KinematicsNode::CallbackReturn KinematicsNode::on_cleanup(const rclcpp_lifecycle::State & state) +{ + (void)state; + RCLCPP_INFO(get_logger(), "cleaned up"); + return CallbackReturn::SUCCESS; +} + +} // namespace polymath::kinematics_ros2 + +RCLCPP_COMPONENTS_REGISTER_NODE(polymath::kinematics_ros2::KinematicsNode) diff --git a/polymath_kinematics_ros2/test/catch2_compat.hpp b/polymath_kinematics_ros2/test/catch2_compat.hpp new file mode 100644 index 0000000..5ab33b0 --- /dev/null +++ b/polymath_kinematics_ros2/test/catch2_compat.hpp @@ -0,0 +1,25 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#if __has_include() + #include + #include +using Catch::Approx; +#elif __has_include() + #include +#else + #error "Catch2 headers not found. Please install Catch2 (v2 or v3)." +#endif diff --git a/polymath_kinematics_ros2/test/test_kinematics_node.cpp b/polymath_kinematics_ros2/test/test_kinematics_node.cpp new file mode 100644 index 0000000..292fc44 --- /dev/null +++ b/polymath_kinematics_ros2/test/test_kinematics_node.cpp @@ -0,0 +1,75 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "catch2_compat.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "polymath_kinematics_ros2/kinematics_node.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace +{ + +using polymath::kinematics_ros2::KinematicsNode; + +/// Brings rclcpp up for the duration of a test case and tears it down again, so the suite can be +/// run repeatedly in one process without leaking context state. +class RclcppFixture +{ +public: + RclcppFixture() + { + rclcpp::init(0, nullptr); + } + + ~RclcppFixture() + { + rclcpp::shutdown(); + } + + RclcppFixture(const RclcppFixture &) = delete; + RclcppFixture & operator=(const RclcppFixture &) = delete; +}; + +} // namespace + +TEST_CASE("KinematicsNode walks the full lifecycle", "[kinematics_node]") +{ + const RclcppFixture fixture; + auto node = std::make_shared(rclcpp::NodeOptions()); + + REQUIRE(std::string("kinematics_node") == std::string(node->get_name())); + + const rclcpp_lifecycle::State unconfigured = node->get_current_state(); + REQUIRE(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED == unconfigured.id()); + + REQUIRE(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE == node->configure().id()); + REQUIRE(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE == node->activate().id()); + REQUIRE(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE == node->deactivate().id()); + REQUIRE(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED == node->cleanup().id()); +} + +TEST_CASE("KinematicsNode transition callbacks report success", "[kinematics_node]") +{ + const RclcppFixture fixture; + auto node = std::make_shared(rclcpp::NodeOptions()); + + const rclcpp_lifecycle::State state = node->get_current_state(); + REQUIRE(KinematicsNode::CallbackReturn::SUCCESS == node->on_configure(state)); + REQUIRE(KinematicsNode::CallbackReturn::SUCCESS == node->on_activate(state)); + REQUIRE(KinematicsNode::CallbackReturn::SUCCESS == node->on_deactivate(state)); + REQUIRE(KinematicsNode::CallbackReturn::SUCCESS == node->on_cleanup(state)); +} From 92e01706b310da739070d707af86a44f1b5a8fe4 Mon Sep 17 00:00:00 2001 From: Zeerek Date: Thu, 20 Aug 2026 09:41:45 -0700 Subject: [PATCH 2/2] Semi-functional initial system --- polymath_kinematics_ros2/CMakeLists.txt | 18 +- .../articulated_projector_node.hpp | 105 +++++++++ .../kinematics_node.hpp | 54 ----- polymath_kinematics_ros2/package.xml | 6 +- .../src/articulated_projector.yaml | 125 +++++++++++ .../src/articulated_projector_node.cpp | 206 ++++++++++++++++++ .../src/kinematics_node.cpp | 56 ----- .../test/test_kinematics_node.cpp | 140 +++++++++++- 8 files changed, 585 insertions(+), 125 deletions(-) create mode 100644 polymath_kinematics_ros2/include/polymath_kinematics_ros2/articulated_projector_node.hpp delete mode 100644 polymath_kinematics_ros2/include/polymath_kinematics_ros2/kinematics_node.hpp create mode 100644 polymath_kinematics_ros2/src/articulated_projector.yaml create mode 100644 polymath_kinematics_ros2/src/articulated_projector_node.cpp delete mode 100644 polymath_kinematics_ros2/src/kinematics_node.cpp diff --git a/polymath_kinematics_ros2/CMakeLists.txt b/polymath_kinematics_ros2/CMakeLists.txt index d9ba14f..8265f58 100644 --- a/polymath_kinematics_ros2/CMakeLists.txt +++ b/polymath_kinematics_ros2/CMakeLists.txt @@ -26,10 +26,13 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -find_package(polymath_kinematics REQUIRED) +generate_parameter_library( + articulated_projector_params + src/articulated_projector.yaml +) add_library(${PROJECT_NAME} SHARED - src/kinematics_node.cpp + src/articulated_projector_node.cpp ) target_include_directories(${PROJECT_NAME} PUBLIC $ @@ -38,22 +41,27 @@ target_include_directories(${PROJECT_NAME} PUBLIC target_link_libraries( ${PROJECT_NAME} PUBLIC + articulated_projector_params polymath_kinematics::polymath_kinematics rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle + ${geometry_msgs_TARGETS} + ${lifecycle_msgs_TARGETS} + ${sensor_msgs_TARGETS} PRIVATE + magic_enum::magic_enum rclcpp_components::component ) # Upstream rclcpp_components, not polymath_core's rclcpp_lifecycle_components wrapper: that # package lives in polymath_core and is unavailable when this repo builds standalone in its own CI. rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "polymath::kinematics_ros2::KinematicsNode" - EXECUTABLE kinematics_node + PLUGIN "polymath::kinematics::ros2::ArticulatedProjector" + EXECUTABLE articulated_projector ) install( - TARGETS ${PROJECT_NAME} kinematics_node + TARGETS ${PROJECT_NAME} articulated_projector articulated_projector_params EXPORT ${PROJECT_NAME}_TARGETS ARCHIVE DESTINATION lib LIBRARY DESTINATION lib diff --git a/polymath_kinematics_ros2/include/polymath_kinematics_ros2/articulated_projector_node.hpp b/polymath_kinematics_ros2/include/polymath_kinematics_ros2/articulated_projector_node.hpp new file mode 100644 index 0000000..f80ff67 --- /dev/null +++ b/polymath_kinematics_ros2/include/polymath_kinematics_ros2/articulated_projector_node.hpp @@ -0,0 +1,105 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include + +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "polymath_kinematics/articulated_projector.hpp" +#include "polymath_kinematics_ros2/articulated_projector_params.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "sensor_msgs/msg/joint_state.hpp" + +namespace polymath::kinematics::ros2 +{ + +/// ROS 2 lifecycle wrapper around polymath_kinematics::ArticulatedProjector. +/// +/// The node tracks the vehicle's measured articulation angle from a JointState topic and the +/// commanded body velocity from a cmd_vel topic. Every command produces a fresh forward projection +/// over `projection.horizon_s` at `projection.time_step_s` steps, starting from the identity pose +/// and the measured articulation angle, and ramping toward the articulation angle the command asks +/// for. The result is held on the node and read back with getLastProjection(); nothing is published +/// yet. +class ArticulatedProjectorNode : public rclcpp_lifecycle::LifecycleNode +{ +public: + using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + + /// Construct the node. + /// \param options Node options supplied by rclcpp or by a component container. + explicit ArticulatedProjectorNode(const rclcpp::NodeOptions & options); + + /// Build the kinematic model and projector from parameters, and create the subscriptions. + /// \param state The lifecycle state being transitioned from. + CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + + /// Begin projecting on incoming commands. + /// \param state The lifecycle state being transitioned from. + CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + + /// Stop projecting on incoming commands. + /// \param state The lifecycle state being transitioned from. + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + + /// Tear down the subscriptions, the projector, and any cached projection. + /// \param state The lifecycle state being transitioned from. + CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; + + /// \return A copy of the most recent projection, or an empty vector if none has been computed. + std::vector getLastProjection() const; + + /// \return The most recently measured articulation angle in radians (0.0 before the first + /// JointState message naming the configured joint arrives). + double getArticulationAngleRad() const; + +private: + /// Latch the articulation angle from the joint named by the `articulation_joint_name` parameter. + /// Messages that do not carry that joint (or carry no position for it) are ignored. + /// \param msg The incoming joint state. + void onJointState(const sensor_msgs::msg::JointState & msg); + + /// Project the trajectory the command implies from the measured articulation angle. + /// \param msg The incoming velocity command. + void onCmdVel(const geometry_msgs::msg::TwistStamped & msg); + + /// The underlying polymath_kinematics projector. Null until on_configure() succeeds. + std::unique_ptr projector_; + + /// Subscriptions + rclcpp::Subscription::SharedPtr joint_state_sub_; + rclcpp::Subscription::SharedPtr cmd_vel_sub_; + + /// Publishers + + /// Guards the state shared between the two subscription callbacks and the accessors, so the node + /// stays correct under a multi-threaded executor. + mutable std::mutex state_mutex_; + + /// Most recent measured articulation angle (gamma) in radians. + double articulation_angle_rad_{0.0}; + + /// Most recent projection, one entry per time step including the initial state. + std::vector last_projection_; + + /// Parameters + std::shared_ptr param_listener_; + articulated_projector::Params params_; +}; + +} // namespace polymath::kinematics::ros2 diff --git a/polymath_kinematics_ros2/include/polymath_kinematics_ros2/kinematics_node.hpp b/polymath_kinematics_ros2/include/polymath_kinematics_ros2/kinematics_node.hpp deleted file mode 100644 index 468e848..0000000 --- a/polymath_kinematics_ros2/include/polymath_kinematics_ros2/kinematics_node.hpp +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#pragma once - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" - -namespace polymath::kinematics_ros2 -{ - -/// Placeholder ROS 2 lifecycle wrapper around the polymath_kinematics models. -/// -/// The node declares no parameters, topics, or services yet. It exists so the ROS 2 layer has a -/// build target, a component registration, and a lifecycle contract to grow into; every transition -/// callback is currently a no-op that reports SUCCESS. -class KinematicsNode : public rclcpp_lifecycle::LifecycleNode -{ -public: - using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - - /// Construct the node. - /// \param options Node options supplied by rclcpp or by a component container. - explicit KinematicsNode(const rclcpp::NodeOptions & options); - - /// Allocate resources. No-op until the node gains interfaces. - /// \param state The lifecycle state being transitioned from. - CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; - - /// Begin publishing. No-op until the node gains interfaces. - /// \param state The lifecycle state being transitioned from. - CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; - - /// Stop publishing. No-op until the node gains interfaces. - /// \param state The lifecycle state being transitioned from. - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; - - /// Release resources. No-op until the node gains interfaces. - /// \param state The lifecycle state being transitioned from. - CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; -}; - -} // namespace polymath::kinematics_ros2 diff --git a/polymath_kinematics_ros2/package.xml b/polymath_kinematics_ros2/package.xml index ca30496..5dda0ee 100644 --- a/polymath_kinematics_ros2/package.xml +++ b/polymath_kinematics_ros2/package.xml @@ -3,18 +3,22 @@ polymath_kinematics_ros2 0.3.0 - ROS 2 layer over polymath_kinematics. Placeholder — the node is a lifecycle skeleton with no interfaces yet. + ROS 2 layer over polymath_kinematics. Projects articulated-vehicle trajectories forward in time from the measured articulation angle and a commanded body velocity. Polymath Engineering Apache-2.0 Zeerek Ahmad ament_cmake_auto + generate_parameter_library + geometry_msgs lifecycle_msgs polymath_kinematics rclcpp rclcpp_components rclcpp_lifecycle + sensor_msgs + magic_enum catch2 diff --git a/polymath_kinematics_ros2/src/articulated_projector.yaml b/polymath_kinematics_ros2/src/articulated_projector.yaml new file mode 100644 index 0000000..05e36bf --- /dev/null +++ b/polymath_kinematics_ros2/src/articulated_projector.yaml @@ -0,0 +1,125 @@ +--- +articulated_projector: + articulation_joint_name: + type: string + default_value: articulation_joint + description: Name of the joint in the subscribed JointState message carrying the articulation angle (gamma) in radians. + read_only: true + validation: + not_empty<>: + + model: + articulation_to_front_axle_m: + type: double + default_value: 1.65 + description: Distance from the articulation joint to the front axle centre [m]. + read_only: true + validation: + gt<>: [0.0] + + articulation_to_rear_axle_m: + type: double + default_value: 1.65 + description: Distance from the articulation joint to the rear axle centre [m]. + read_only: true + validation: + gt<>: [0.0] + + front_track_width_m: + type: double + default_value: 2.0 + description: Lateral distance between the front wheel contact centres (track width) [m]. + read_only: true + validation: + gt<>: [0.0] + + rear_track_width_m: + type: double + default_value: 2.0 + description: Lateral distance between the rear wheel contact centres (track width) [m]. + read_only: true + validation: + gt<>: [0.0] + + front_wheel_radius_m: + type: double + default_value: 0.723 + description: Rolling radius of the front wheels [m]. + read_only: true + validation: + gt<>: [0.0] + + rear_wheel_radius_m: + type: double + default_value: 0.723 + description: Rolling radius of the rear wheels [m]. + read_only: true + validation: + gt<>: [0.0] + + projector: + minimum_articulation_angle_rad: + type: double + default_value: -0.7853981633974483 + description: Minimum articulation angle (radians) reported by the vehicle's articulation encoder. + read_only: false + validation: + bounds<>: [-1.57, 0.0] + + maximum_articulation_angle_rad: + type: double + default_value: 0.7853981633974483 + description: Maximum articulation angle (radians) reported by the vehicle's articulation encoder. + read_only: false + validation: + bounds<>: [0.0, 1.57] + + axle_reference: + type: string + default_value: rear + description: Which axle is used as the reference for the articulation angle (rear or front). + read_only: false + validation: + one_of<>: [[rear, front]] + + # TODO: (zeerekahmad) Do we want to be able to subscribe to these? + front_footprint: + type: double_array + default_value: [] + description: The front footprint polygon, in the front-axle frame, as a flat list of x,y pairs. If empty, the front footprint is left unset. + read_only: false + validation: + element_bounds<>: [-100.0, 100.0] + + rear_footprint: + type: double_array + default_value: [] + description: The rear footprint polygon, in the rear-axle frame, as a flat list of x,y pairs. If empty, the rear footprint is left unset. + read_only: false + validation: + element_bounds<>: [-100.0, 100.0] + + articulation_rate_rad_s: + type: double + default_value: 0.5 + description: Maximum articulation rate (radians per second) the articulation joint can slew at. + read_only: false + validation: + gt<>: [0.0] + + projection: + horizon_s: + type: double + default_value: 3.0 + description: How far into the future each trajectory is projected [s]. + read_only: false + validation: + gt<>: [0.0] + + time_step_s: + type: double + default_value: 0.1 + description: Integration step used while projecting [s]. Must be no larger than horizon_s. + read_only: false + validation: + gt<>: [0.0] diff --git a/polymath_kinematics_ros2/src/articulated_projector_node.cpp b/polymath_kinematics_ros2/src/articulated_projector_node.cpp new file mode 100644 index 0000000..9b50159 --- /dev/null +++ b/polymath_kinematics_ros2/src/articulated_projector_node.cpp @@ -0,0 +1,206 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "polymath_kinematics_ros2/articulated_projector_node.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include "lifecycle_msgs/msg/state.hpp" +#include "magic_enum/magic_enum.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(polymath::kinematics::ros2::ArticulatedProjectorNode) + +namespace polymath::kinematics::ros2 +{ + +namespace +{ + +/// Depth of the two command/feedback subscriptions. Both carry the latest sample only, so a short +/// queue is enough and keeps a backlog from projecting stale commands. +constexpr int SUBSCRIPTION_QUEUE_DEPTH = 1; + +/// Reinterpret a flat [x0, y0, x1, y1, ...] parameter as a footprint polygon. +/// \param flat_xy Flat list of alternating x and y coordinates; an odd length is rejected. +/// \return The polygon, or an empty optional if `flat_xy` does not hold whole x,y pairs. +std::optional footprintFromFlatArray(const std::vector & flat_xy) +{ + if (0 != flat_xy.size() % 2) { + return std::nullopt; + } + Footprint footprint; + footprint.reserve(flat_xy.size() / 2); + for (size_t index = 0; index < flat_xy.size(); index += 2) { + footprint.push_back(Point2D{flat_xy[index], flat_xy[index + 1]}); + } + return footprint; +} + +} // namespace + +ArticulatedProjectorNode::ArticulatedProjectorNode(const rclcpp::NodeOptions & options) +: rclcpp_lifecycle::LifecycleNode("articulated_projector", options) +{ + param_listener_ = std::make_shared(get_node_parameters_interface()); + params_ = param_listener_->get_params(); +} + +ArticulatedProjectorNode::CallbackReturn ArticulatedProjectorNode::on_configure(const rclcpp_lifecycle::State & state) +{ + (void)state; + params_ = param_listener_->get_params(); + + const std::optional front_footprint = footprintFromFlatArray(params_.projector.front_footprint); + const std::optional rear_footprint = footprintFromFlatArray(params_.projector.rear_footprint); + if (!front_footprint.has_value() || !rear_footprint.has_value()) { + RCLCPP_ERROR(get_logger(), "footprint parameters must hold an even number of entries (flat x,y pairs)"); + return CallbackReturn::FAILURE; + } + + const std::optional axle_reference = + magic_enum::enum_cast(params_.projector.axle_reference, magic_enum::case_insensitive); + if (!axle_reference.has_value()) { + RCLCPP_ERROR(get_logger(), "axle_reference '%s' is not a known axle", params_.projector.axle_reference.c_str()); + return CallbackReturn::FAILURE; + } + + if (params_.projection.time_step_s > params_.projection.horizon_s) { + RCLCPP_ERROR( + get_logger(), + "projection.time_step_s (%f) exceeds projection.horizon_s (%f)", + params_.projection.time_step_s, + params_.projection.horizon_s); + return CallbackReturn::FAILURE; + } + + const ArticulatedModel model = ArticulatedModel( + params_.model.articulation_to_front_axle_m, + params_.model.articulation_to_rear_axle_m, + params_.model.front_track_width_m, + params_.model.rear_track_width_m, + params_.model.front_wheel_radius_m, + params_.model.rear_wheel_radius_m); + + projector_ = std::make_unique( + model, + params_.projector.minimum_articulation_angle_rad, + params_.projector.maximum_articulation_angle_rad, + axle_reference.value(), + front_footprint.value(), + rear_footprint.value()); + + joint_state_sub_ = create_subscription( + "joint_states", SUBSCRIPTION_QUEUE_DEPTH, [this](const sensor_msgs::msg::JointState & msg) { onJointState(msg); }); + cmd_vel_sub_ = create_subscription( + "cmd_vel", SUBSCRIPTION_QUEUE_DEPTH, [this](const geometry_msgs::msg::TwistStamped & msg) { onCmdVel(msg); }); + + RCLCPP_INFO( + get_logger(), + "configured: tracking joint '%s', projecting %.2fs ahead in %.3fs steps", + params_.articulation_joint_name.c_str(), + params_.projection.horizon_s, + params_.projection.time_step_s); + return CallbackReturn::SUCCESS; +} + +ArticulatedProjectorNode::CallbackReturn ArticulatedProjectorNode::on_activate(const rclcpp_lifecycle::State & state) +{ + (void)state; + RCLCPP_INFO(get_logger(), "activated"); + return CallbackReturn::SUCCESS; +} + +ArticulatedProjectorNode::CallbackReturn ArticulatedProjectorNode::on_deactivate(const rclcpp_lifecycle::State & state) +{ + (void)state; + RCLCPP_INFO(get_logger(), "deactivated"); + return CallbackReturn::SUCCESS; +} + +ArticulatedProjectorNode::CallbackReturn ArticulatedProjectorNode::on_cleanup(const rclcpp_lifecycle::State & state) +{ + (void)state; + joint_state_sub_.reset(); + cmd_vel_sub_.reset(); + projector_.reset(); + { + const std::lock_guard lock(state_mutex_); + articulation_angle_rad_ = 0.0; + last_projection_.clear(); + } + RCLCPP_INFO(get_logger(), "cleaned up"); + return CallbackReturn::SUCCESS; +} + +std::vector ArticulatedProjectorNode::getLastProjection() const +{ + const std::lock_guard lock(state_mutex_); + return last_projection_; +} + +double ArticulatedProjectorNode::getArticulationAngleRad() const +{ + const std::lock_guard lock(state_mutex_); + return articulation_angle_rad_; +} + +void ArticulatedProjectorNode::onJointState(const sensor_msgs::msg::JointState & msg) +{ + const auto joint = std::find(msg.name.begin(), msg.name.end(), params_.articulation_joint_name); + if (msg.name.end() == joint) { + return; + } + + // position[] is allowed to be shorter than name[]: a joint can be reported with velocity/effort only. + const size_t index = static_cast(std::distance(msg.name.begin(), joint)); + if (index >= msg.position.size()) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 5000, "joint '%s' carries no position", params_.articulation_joint_name.c_str()); + return; + } + + const std::lock_guard lock(state_mutex_); + articulation_angle_rad_ = msg.position[index]; +} + +void ArticulatedProjectorNode::onCmdVel(const geometry_msgs::msg::TwistStamped & msg) +{ + if (lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE != get_current_state().id()) { + return; + } + + // The command is a body velocity; the projector steers by articulation angle, so ask the model + // which articulation angle sustains that (v, omega) pair and ramp toward it. + ArticulatedModel model = projector_->get_model(); + const ArticulatedVehicleState commanded = model.bodyVelocityToVehicleState(msg.twist.linear.x, msg.twist.angular.z); + + const std::lock_guard lock(state_mutex_); + last_projection_ = projector_->project( + params_.projection.horizon_s, + params_.projection.time_step_s, + Pose2D{0.0, 0.0, 0.0}, + articulation_angle_rad_, + commanded.articulation_angle_rad, + params_.projector.articulation_rate_rad_s, + msg.twist.linear.x); +} + +} // namespace polymath::kinematics::ros2 diff --git a/polymath_kinematics_ros2/src/kinematics_node.cpp b/polymath_kinematics_ros2/src/kinematics_node.cpp deleted file mode 100644 index 320fd17..0000000 --- a/polymath_kinematics_ros2/src/kinematics_node.cpp +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "polymath_kinematics_ros2/kinematics_node.hpp" - -#include "rclcpp_components/register_node_macro.hpp" - -namespace polymath::kinematics_ros2 -{ - -KinematicsNode::KinematicsNode(const rclcpp::NodeOptions & options) -: rclcpp_lifecycle::LifecycleNode("kinematics_node", options) -{} - -KinematicsNode::CallbackReturn KinematicsNode::on_configure(const rclcpp_lifecycle::State & state) -{ - (void)state; - RCLCPP_INFO(get_logger(), "configured (placeholder: nothing to set up yet)"); - return CallbackReturn::SUCCESS; -} - -KinematicsNode::CallbackReturn KinematicsNode::on_activate(const rclcpp_lifecycle::State & state) -{ - (void)state; - RCLCPP_INFO(get_logger(), "activated (placeholder: nothing to publish yet)"); - return CallbackReturn::SUCCESS; -} - -KinematicsNode::CallbackReturn KinematicsNode::on_deactivate(const rclcpp_lifecycle::State & state) -{ - (void)state; - RCLCPP_INFO(get_logger(), "deactivated"); - return CallbackReturn::SUCCESS; -} - -KinematicsNode::CallbackReturn KinematicsNode::on_cleanup(const rclcpp_lifecycle::State & state) -{ - (void)state; - RCLCPP_INFO(get_logger(), "cleaned up"); - return CallbackReturn::SUCCESS; -} - -} // namespace polymath::kinematics_ros2 - -RCLCPP_COMPONENTS_REGISTER_NODE(polymath::kinematics_ros2::KinematicsNode) diff --git a/polymath_kinematics_ros2/test/test_kinematics_node.cpp b/polymath_kinematics_ros2/test/test_kinematics_node.cpp index 292fc44..4b0519a 100644 --- a/polymath_kinematics_ros2/test/test_kinematics_node.cpp +++ b/polymath_kinematics_ros2/test/test_kinematics_node.cpp @@ -12,18 +12,24 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include #include #include +#include +#include #include "catch2_compat.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" #include "lifecycle_msgs/msg/state.hpp" -#include "polymath_kinematics_ros2/kinematics_node.hpp" +#include "polymath_kinematics_ros2/articulated_projector_node.hpp" #include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/joint_state.hpp" namespace { -using polymath::kinematics_ros2::KinematicsNode; +using polymath::kinematics::ros2::ArticulatedProjectorNode; /// Brings rclcpp up for the duration of a test case and tears it down again, so the suite can be /// run repeatedly in one process without leaking context state. @@ -44,14 +50,41 @@ class RclcppFixture RclcppFixture & operator=(const RclcppFixture &) = delete; }; +/// Spin `node` until `predicate` holds or the budget runs out, so a test never blocks forever on a +/// message that is not coming. +/// \return True if the predicate held before the budget expired. +template +bool spinUntil(const std::shared_ptr & node, PredicateT predicate) +{ + constexpr int MAX_SPINS = 200; + for (int spin = 0; spin < MAX_SPINS; ++spin) { + if (predicate()) { + return true; + } + rclcpp::spin_some(node->get_node_base_interface()); + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + } + return predicate(); +} + +/// Build a JointState naming a decoy joint ahead of the articulation joint, so a test that passes +/// cannot be passing by reading index 0. +sensor_msgs::msg::JointState makeJointState(const std::string & articulation_joint_name, double angle_rad) +{ + sensor_msgs::msg::JointState msg; + msg.name = {"some_other_joint", articulation_joint_name}; + msg.position = {0.1, angle_rad}; + return msg; +} + } // namespace TEST_CASE("KinematicsNode walks the full lifecycle", "[kinematics_node]") { const RclcppFixture fixture; - auto node = std::make_shared(rclcpp::NodeOptions()); + auto node = std::make_shared(rclcpp::NodeOptions()); - REQUIRE(std::string("kinematics_node") == std::string(node->get_name())); + REQUIRE(std::string("articulated_projector") == std::string(node->get_name())); const rclcpp_lifecycle::State unconfigured = node->get_current_state(); REQUIRE(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED == unconfigured.id()); @@ -65,11 +98,100 @@ TEST_CASE("KinematicsNode walks the full lifecycle", "[kinematics_node]") TEST_CASE("KinematicsNode transition callbacks report success", "[kinematics_node]") { const RclcppFixture fixture; - auto node = std::make_shared(rclcpp::NodeOptions()); + auto node = std::make_shared(rclcpp::NodeOptions()); const rclcpp_lifecycle::State state = node->get_current_state(); - REQUIRE(KinematicsNode::CallbackReturn::SUCCESS == node->on_configure(state)); - REQUIRE(KinematicsNode::CallbackReturn::SUCCESS == node->on_activate(state)); - REQUIRE(KinematicsNode::CallbackReturn::SUCCESS == node->on_deactivate(state)); - REQUIRE(KinematicsNode::CallbackReturn::SUCCESS == node->on_cleanup(state)); + REQUIRE(ArticulatedProjectorNode::CallbackReturn::SUCCESS == node->on_configure(state)); + REQUIRE(ArticulatedProjectorNode::CallbackReturn::SUCCESS == node->on_activate(state)); + REQUIRE(ArticulatedProjectorNode::CallbackReturn::SUCCESS == node->on_deactivate(state)); + REQUIRE(ArticulatedProjectorNode::CallbackReturn::SUCCESS == node->on_cleanup(state)); +} + +TEST_CASE("KinematicsNode latches the articulation angle from the named joint", "[kinematics_node]") +{ + const RclcppFixture fixture; + auto node = std::make_shared(rclcpp::NodeOptions()); + REQUIRE(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE == node->configure().id()); + REQUIRE(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE == node->activate().id()); + + const std::string joint_name = node->get_parameter("articulation_joint_name").as_string(); + auto publisher_node = std::make_shared("joint_state_publisher"); + auto publisher = publisher_node->create_publisher("joint_states", 1); + + publisher->publish(makeJointState(joint_name, 0.25)); + REQUIRE(spinUntil(node, [&node] { return 0.0 != node->getArticulationAngleRad(); })); + CHECK(node->getArticulationAngleRad() == Approx(0.25)); + + // A message that does not name the articulation joint must leave the latched angle alone. + sensor_msgs::msg::JointState unrelated; + unrelated.name = {"some_other_joint"}; + unrelated.position = {1.0}; + publisher->publish(unrelated); + spinUntil(node, [] { return false; }); + CHECK(node->getArticulationAngleRad() == Approx(0.25)); +} + +TEST_CASE("KinematicsNode projects a trajectory from the latched angle and cmd_vel", "[kinematics_node]") +{ + const RclcppFixture fixture; + auto node = std::make_shared(rclcpp::NodeOptions()); + REQUIRE(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE == node->configure().id()); + REQUIRE(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE == node->activate().id()); + + const double horizon_s = node->get_parameter("projection.horizon_s").as_double(); + const double time_step_s = node->get_parameter("projection.time_step_s").as_double(); + const std::string joint_name = node->get_parameter("articulation_joint_name").as_string(); + + auto publisher_node = std::make_shared("command_publisher"); + auto joint_publisher = publisher_node->create_publisher("joint_states", 1); + auto cmd_vel_publisher = publisher_node->create_publisher("cmd_vel", 1); + + joint_publisher->publish(makeJointState(joint_name, 0.2)); + REQUIRE(spinUntil(node, [&node] { return 0.0 != node->getArticulationAngleRad(); })); + + geometry_msgs::msg::TwistStamped command; + command.twist.linear.x = 1.0; + command.twist.angular.z = 0.0; + cmd_vel_publisher->publish(command); + REQUIRE(spinUntil(node, [&node] { return !node->getLastProjection().empty(); })); + + const std::vector projection = node->getLastProjection(); + const size_t expected_samples = static_cast(std::ceil(horizon_s / time_step_s)) + 1; + CHECK(expected_samples == projection.size()); + + // Element 0 is the initial state: t=0, identity pose, and the angle the joint reported. + CHECK(projection.front().time_s == Approx(0.0)); + CHECK(projection.front().pose.x == Approx(0.0)); + CHECK(projection.front().articulation_angle_rad == Approx(0.2)); + + // A straight-ahead command ramps the articulation back to zero and carries the vehicle forward. + CHECK(projection.back().time_s == Approx(horizon_s)); + CHECK(projection.back().articulation_angle_rad == Approx(0.0).margin(1e-9)); + CHECK(projection.back().pose.x > projection.front().pose.x); +} + +TEST_CASE("KinematicsNode ignores cmd_vel while inactive", "[kinematics_node]") +{ + const RclcppFixture fixture; + auto node = std::make_shared(rclcpp::NodeOptions()); + REQUIRE(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE == node->configure().id()); + + auto publisher_node = std::make_shared("command_publisher"); + auto cmd_vel_publisher = publisher_node->create_publisher("cmd_vel", 1); + + geometry_msgs::msg::TwistStamped command; + command.twist.linear.x = 1.0; + cmd_vel_publisher->publish(command); + spinUntil(node, [] { return false; }); + + CHECK(node->getLastProjection().empty()); +} + +TEST_CASE("KinematicsNode rejects an empty articulation joint name", "[kinematics_node]") +{ + const RclcppFixture fixture; + rclcpp::NodeOptions options; + options.parameter_overrides({rclcpp::Parameter("articulation_joint_name", std::string(""))}); + + REQUIRE_THROWS(std::make_shared(options)); }