diff --git a/polymath_kinematics_ros2/CMakeLists.txt b/polymath_kinematics_ros2/CMakeLists.txt new file mode 100644 index 0000000..a4de058 --- /dev/null +++ b/polymath_kinematics_ros2/CMakeLists.txt @@ -0,0 +1,121 @@ +# 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() + +generate_parameter_library( + articulated_projector_params + src/articulated_projector.yaml +) + +add_library(${PROJECT_NAME} SHARED + src/articulated_projector_node.cpp +) +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} + ${nav_msgs_TARGETS} + ${sensor_msgs_TARGETS} + ${std_msgs_TARGETS} + ${visualization_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::ArticulatedProjectorNode" + EXECUTABLE articulated_projector +) + +install( + TARGETS ${PROJECT_NAME} articulated_projector articulated_projector_params + 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/articulated_projector_node.hpp b/polymath_kinematics_ros2/include/polymath_kinematics_ros2/articulated_projector_node.hpp new file mode 100644 index 0000000..f5c53e2 --- /dev/null +++ b/polymath_kinematics_ros2/include/polymath_kinematics_ros2/articulated_projector_node.hpp @@ -0,0 +1,132 @@ +// 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 + +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/path.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 "rclcpp_lifecycle/lifecycle_publisher.hpp" +#include "sensor_msgs/msg/joint_state.hpp" +#include "visualization_msgs/msg/marker_array.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 for getLastProjection() and published two ways: as a +/// MarkerArray on `projected_footprints` outlining both bodies at every sample, and as a Path on +/// `projected_path` tracing the reference axle through those same samples. +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, and publish + /// the footprint markers for it. + /// \param msg The incoming velocity command. + void onCmdVel(const geometry_msgs::msg::TwistStamped & msg); + + /// Outline the front and rear body footprints at every sample of `projection`. Samples whose + /// footprint is unset contribute no marker. + /// \param projection Projection to draw, in the frame named by `visualization.frame_id`. + /// \return Markers led by a DELETEALL that clears the previous publication. + std::unique_ptr produceProjectedFootprintMarkers( + const std::vector & projection) const; + + /// Trace the reference axle through `projection`, one pose per sample including the initial one. + /// The axle is the one named by `projector.axle_reference`, and each pose's yaw is the heading of + /// the body that axle belongs to. + /// \param projection Projection to trace, in the frame named by `visualization.frame_id`. + /// \return A Path holding every sample, undecimated. + std::unique_ptr produceProjectedPath( + const std::vector & projection) const; + + /// 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 + rclcpp_lifecycle::LifecyclePublisher::SharedPtr footprint_marker_pub_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr path_pub_; + + /// 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}; + + /// Set once a JointState naming the configured joint has supplied an angle. Read outside + /// state_mutex_ by the logging in both callbacks. + std::atomic articulation_angle_seen_{false}; + + /// 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/package.xml b/polymath_kinematics_ros2/package.xml new file mode 100644 index 0000000..d983aef --- /dev/null +++ b/polymath_kinematics_ros2/package.xml @@ -0,0 +1,31 @@ + + + + polymath_kinematics_ros2 + 0.3.0 + 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 + nav_msgs + polymath_kinematics + rclcpp + rclcpp_components + rclcpp_lifecycle + sensor_msgs + std_msgs + visualization_msgs + magic_enum + + catch2 + + + ament_cmake + + diff --git a/polymath_kinematics_ros2/src/articulated_projector.yaml b/polymath_kinematics_ros2/src/articulated_projector.yaml new file mode 100644 index 0000000..d8cad1d --- /dev/null +++ b/polymath_kinematics_ros2/src/articulated_projector.yaml @@ -0,0 +1,168 @@ +--- +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] + + visualization: + frame_id: + type: string + default_value: base_link + description: TF frame the projection markers are stamped with. Projections start at the reference axle, so this must name the frame of the axle selected by projector.axle_reference. + read_only: true + validation: + not_empty<>: + + front_color: + type: double_array + default_value: [0.0, 0.6, 1.0, 0.8] + description: Front-body footprint outline color as r,g,b,a, each in [0.0, 1.0]. + read_only: true + validation: + fixed_size<>: [4] + element_bounds<>: [0.0, 1.0] + + rear_color: + type: double_array + default_value: [1.0, 0.6, 0.0, 0.8] + description: Rear-body footprint outline color as r,g,b,a, each in [0.0, 1.0]. + read_only: true + validation: + fixed_size<>: [4] + element_bounds<>: [0.0, 1.0] + + line_width_m: + type: double + default_value: 0.05 + description: Width of the footprint outline strokes [m]. + read_only: true + validation: + bounds<>: [0.005, 0.5] + + marker_lifetime_s: + type: double + default_value: 1.0 + description: How long a published marker survives without being republished [s]. 0.0 keeps it forever. + read_only: true + validation: + bounds<>: [0.0, 20.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..c2bb40b --- /dev/null +++ b/polymath_kinematics_ros2/src/articulated_projector_node.cpp @@ -0,0 +1,426 @@ +// 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 +#include +#include + +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "magic_enum/magic_enum.hpp" +#include "rclcpp_components/register_node_macro.hpp" +#include "std_msgs/msg/color_rgba.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; + +/// Depth of the marker and path publishers; each publication supersedes the last. +constexpr int MARKER_QUEUE_DEPTH = 1; + +/// Height above the ground plane the outlines are drawn at. +constexpr double MARKER_Z_OFFSET_M = 0.001; + +/// Marker namespace holding the front-body outlines. +constexpr const char * FRONT_MARKER_NAMESPACE = "projected_front_footprint"; + +/// Marker namespace holding the rear-body outlines. +constexpr const char * REAR_MARKER_NAMESPACE = "projected_rear_footprint"; + +/// Throttle period for the per-message diagnostic logs. +constexpr int LOG_THROTTLE_MS = 2000; + +constexpr double RAD_TO_DEG = 57.29577951308232; + +/// Render a JointState's name list for a log line. +std::string joinNames(const std::vector & names) +{ + std::string joined; + for (const std::string & name : names) { + if (!joined.empty()) { + joined += ", "; + } + joined += name; + } + return joined; +} + +/// 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; +} + +/// Reinterpret an [r, g, b, a] parameter as a color. The parameter is validated to hold exactly +/// four entries in [0.0, 1.0]. +std_msgs::msg::ColorRGBA colorFromArray(const std::vector & rgba) +{ + std_msgs::msg::ColorRGBA color; + color.r = static_cast(rgba[0]); + color.g = static_cast(rgba[1]); + color.b = static_cast(rgba[2]); + color.a = static_cast(rgba[3]); + return color; +} + +/// Append a closed LINE_STRIP tracing `footprint` to `markers`, taking every shared field from +/// `prototype`. An empty footprint appends nothing. +/// \param prototype Marker carrying the header, type, scale, and lifetime shared by all outlines. +/// \param footprint Open polygon in the frame of `prototype`'s header. +/// \param marker_namespace Namespace to file the outline under. +/// \param color Stroke color. +/// \param id Marker id, unique within `marker_namespace`. +/// \param markers Array the outline is appended to. +void appendFootprintOutline( + const visualization_msgs::msg::Marker & prototype, + const Footprint & footprint, + const char * marker_namespace, + const std_msgs::msg::ColorRGBA & color, + int id, + visualization_msgs::msg::MarkerArray & markers) +{ + if (footprint.empty()) { + return; + } + + visualization_msgs::msg::Marker outline = prototype; + outline.ns = marker_namespace; + outline.id = id; + outline.color = color; + outline.points.reserve(footprint.size() + 1); + for (const Point2D & vertex : footprint) { + geometry_msgs::msg::Point point; + point.x = vertex.x; + point.y = vertex.y; + outline.points.push_back(point); + } + // Footprints arrive open; repeating the first vertex closes the outline. + outline.points.push_back(outline.points.front()); + + markers.markers.push_back(std::move(outline)); +} + +} // 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); }); + + footprint_marker_pub_ = + create_publisher("projected_footprints", MARKER_QUEUE_DEPTH); + path_pub_ = create_publisher("projected_path", MARKER_QUEUE_DEPTH); + + 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; + footprint_marker_pub_->on_activate(); + path_pub_->on_activate(); + RCLCPP_INFO(get_logger(), "activated"); + return CallbackReturn::SUCCESS; +} + +ArticulatedProjectorNode::CallbackReturn ArticulatedProjectorNode::on_deactivate(const rclcpp_lifecycle::State & state) +{ + (void)state; + footprint_marker_pub_->on_deactivate(); + path_pub_->on_deactivate(); + 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(); + footprint_marker_pub_.reset(); + path_pub_.reset(); + projector_.reset(); + { + const std::lock_guard lock(state_mutex_); + articulation_angle_rad_ = 0.0; + last_projection_.clear(); + articulation_angle_seen_.store(false); + } + 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) { + RCLCPP_INFO_THROTTLE( + get_logger(), + *get_clock(), + LOG_THROTTLE_MS, + "joint '%s' is not in this JointState; it names [%s]", + params_.articulation_joint_name.c_str(), + joinNames(msg.name).c_str()); + 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 double angle_rad = msg.position[index]; + { + const std::lock_guard lock(state_mutex_); + articulation_angle_rad_ = angle_rad; + } + + if (!articulation_angle_seen_.exchange(true)) { + RCLCPP_INFO( + get_logger(), + "first articulation angle from joint '%s' (index %zu of %zu): %.4f rad (%.2f deg)", + params_.articulation_joint_name.c_str(), + index, + msg.name.size(), + angle_rad, + angle_rad * RAD_TO_DEG); + } + RCLCPP_INFO_THROTTLE( + get_logger(), + *get_clock(), + LOG_THROTTLE_MS, + "articulation angle: %.4f rad (%.2f deg)", + angle_rad, + angle_rad * RAD_TO_DEG); +} + +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); + + std::unique_ptr markers; + std::unique_ptr path; + double measured_angle_rad = 0.0; + double final_angle_rad = 0.0; + double final_yaw_rate_rad_s = 0.0; + { + 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); + markers = produceProjectedFootprintMarkers(last_projection_); + path = produceProjectedPath(last_projection_); + measured_angle_rad = articulation_angle_rad_; + if (!last_projection_.empty()) { + final_angle_rad = last_projection_.back().articulation_angle_rad; + final_yaw_rate_rad_s = last_projection_.back().angular_velocity_rad_s; + } + } + + if (!articulation_angle_seen_.load()) { + RCLCPP_INFO_THROTTLE( + get_logger(), + *get_clock(), + LOG_THROTTLE_MS, + "projecting from articulation angle 0.0 rad: no JointState naming '%s' has arrived yet", + params_.articulation_joint_name.c_str()); + } + RCLCPP_INFO_THROTTLE( + get_logger(), + *get_clock(), + LOG_THROTTLE_MS, + "cmd_vel v=%.3f m/s w=%.3f rad/s -> articulation measured %.4f, target %.4f, reached %.4f rad " + "after %.2fs at %.3f rad/s (final yaw rate %.4f rad/s)", + msg.twist.linear.x, + msg.twist.angular.z, + measured_angle_rad, + commanded.articulation_angle_rad, + final_angle_rad, + params_.projection.horizon_s, + params_.projector.articulation_rate_rad_s, + final_yaw_rate_rad_s); + + footprint_marker_pub_->publish(std::move(markers)); + path_pub_->publish(std::move(path)); +} + +std::unique_ptr ArticulatedProjectorNode::produceProjectedFootprintMarkers( + const std::vector & projection) const +{ + auto markers = std::make_unique(); + markers->markers.reserve(2 * projection.size() + 1); + + visualization_msgs::msg::Marker clear_previous; + clear_previous.action = visualization_msgs::msg::Marker::DELETEALL; + markers->markers.push_back(clear_previous); + + visualization_msgs::msg::Marker prototype; + prototype.header.frame_id = params_.visualization.frame_id; + prototype.header.stamp = now(); + prototype.type = visualization_msgs::msg::Marker::LINE_STRIP; + prototype.action = visualization_msgs::msg::Marker::ADD; + prototype.pose.position.z = MARKER_Z_OFFSET_M; + prototype.pose.orientation.w = 1.0; + prototype.scale.x = params_.visualization.line_width_m; + prototype.lifetime = rclcpp::Duration::from_seconds(params_.visualization.marker_lifetime_s); + + const std_msgs::msg::ColorRGBA front_color = colorFromArray(params_.visualization.front_color); + const std_msgs::msg::ColorRGBA rear_color = colorFromArray(params_.visualization.rear_color); + + // The projector emits footprints already in the projection frame, so each outline carries those + // points directly and its marker pose stays at the origin. + int marker_id = 0; + for (const ArticulatedProjectedState & sample : projection) { + appendFootprintOutline(prototype, sample.front_footprint, FRONT_MARKER_NAMESPACE, front_color, marker_id, *markers); + appendFootprintOutline(prototype, sample.rear_footprint, REAR_MARKER_NAMESPACE, rear_color, marker_id, *markers); + ++marker_id; + } + + return markers; +} + +std::unique_ptr ArticulatedProjectorNode::produceProjectedPath( + const std::vector & projection) const +{ + auto path = std::make_unique(); + path->header.frame_id = params_.visualization.frame_id; + path->header.stamp = now(); + path->poses.reserve(projection.size()); + + for (const ArticulatedProjectedState & sample : projection) { + geometry_msgs::msg::PoseStamped pose; + pose.header = path->header; + pose.pose.position.x = sample.pose.x; + pose.pose.position.y = sample.pose.y; + pose.pose.orientation.z = std::sin(sample.pose.theta / 2.0); + pose.pose.orientation.w = std::cos(sample.pose.theta / 2.0); + path->poses.push_back(pose); + } + + return path; +} + +} // namespace polymath::kinematics::ros2 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..3cdeff4 --- /dev/null +++ b/polymath_kinematics_ros2/test/test_kinematics_node.cpp @@ -0,0 +1,365 @@ +// 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 +#include +#include +#include + +#include "catch2_compat.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "nav_msgs/msg/path.hpp" +#include "polymath_kinematics_ros2/articulated_projector_node.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/joint_state.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +namespace +{ + +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. +class RclcppFixture +{ +public: + RclcppFixture() + { + rclcpp::init(0, nullptr); + } + + ~RclcppFixture() + { + rclcpp::shutdown(); + } + + RclcppFixture(const RclcppFixture &) = delete; + RclcppFixture & operator=(const RclcppFixture &) = delete; +}; + +/// Spin every node in `nodes` 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::vector & nodes, PredicateT predicate) +{ + constexpr int MAX_SPINS = 200; + for (int spin = 0; spin < MAX_SPINS; ++spin) { + if (predicate()) { + return true; + } + for (const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr & base : nodes) { + rclcpp::spin_some(base); + } + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + } + return predicate(); +} + +/// Spin the node under test alone. +/// \return True if the predicate held before the budget expired. +template +bool spinUntil(const std::shared_ptr & node, PredicateT predicate) +{ + return spinUntil({node->get_node_base_interface()}, 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()); + + 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()); + + 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(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 publishes the projected footprints as markers", "[kinematics_node]") +{ + const RclcppFixture fixture; + + // A 2 m x 2 m square about each axle, so every sample contributes a four-vertex outline. + const std::vector square = {-1.0, -1.0, 1.0, -1.0, 1.0, 1.0, -1.0, 1.0}; + rclcpp::NodeOptions options; + options.parameter_overrides( + {rclcpp::Parameter("projector.front_footprint", square), + rclcpp::Parameter("projector.rear_footprint", square), + rclcpp::Parameter("visualization.frame_id", std::string("rear_axle"))}); + + auto node = std::make_shared(options); + REQUIRE(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE == node->configure().id()); + REQUIRE(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE == node->activate().id()); + + auto peer_node = std::make_shared("marker_listener"); + visualization_msgs::msg::MarkerArray received; + auto marker_subscription = peer_node->create_subscription( + "projected_footprints", 1, [&received](const visualization_msgs::msg::MarkerArray & msg) { received = msg; }); + auto cmd_vel_publisher = peer_node->create_publisher("cmd_vel", 1); + + const std::vector nodes = { + node->get_node_base_interface(), peer_node->get_node_base_interface()}; + + // Publishing before discovery completes drops the command, and with it the only marker array. + REQUIRE(spinUntil(nodes, [&marker_subscription, &cmd_vel_publisher] { + return 0 < marker_subscription->get_publisher_count() && 0 < cmd_vel_publisher->get_subscription_count(); + })); + + geometry_msgs::msg::TwistStamped command; + command.twist.linear.x = 1.0; + command.twist.angular.z = 0.2; + cmd_vel_publisher->publish(command); + REQUIRE(spinUntil(nodes, [&received] { return !received.markers.empty(); })); + + // One DELETEALL, then a front and a rear outline per projected sample. + const size_t sample_count = node->getLastProjection().size(); + REQUIRE(0 < sample_count); + CHECK(2 * sample_count + 1 == received.markers.size()); + + CHECK(visualization_msgs::msg::Marker::DELETEALL == received.markers.front().action); + + const double line_width_m = node->get_parameter("visualization.line_width_m").as_double(); + size_t front_outlines = 0; + size_t rear_outlines = 0; + for (size_t index = 1; index < received.markers.size(); ++index) { + const visualization_msgs::msg::Marker & outline = received.markers[index]; + CHECK(visualization_msgs::msg::Marker::ADD == outline.action); + CHECK(visualization_msgs::msg::Marker::LINE_STRIP == outline.type); + CHECK(std::string("rear_axle") == outline.header.frame_id); + CHECK(outline.scale.x == Approx(line_width_m)); + + // Four vertices plus the repeat that closes the outline. + REQUIRE(5 == outline.points.size()); + CHECK(outline.points.front().x == Approx(outline.points.back().x)); + CHECK(outline.points.front().y == Approx(outline.points.back().y)); + + if ("projected_front_footprint" == outline.ns) { + ++front_outlines; + } else if ("projected_rear_footprint" == outline.ns) { + ++rear_outlines; + } + } + CHECK(sample_count == front_outlines); + CHECK(sample_count == rear_outlines); + + // The outlines carry projection-frame points, so a turning command spreads them out. + const visualization_msgs::msg::Marker & first_rear = received.markers[2]; + const visualization_msgs::msg::Marker & last_rear = received.markers.back(); + CHECK(std::string("projected_rear_footprint") == first_rear.ns); + CHECK(std::string("projected_rear_footprint") == last_rear.ns); + CHECK(last_rear.points.front().x > first_rear.points.front().x); +} + +TEST_CASE("KinematicsNode publishes the projected reference-axle path", "[kinematics_node]") +{ + const RclcppFixture fixture; + rclcpp::NodeOptions options; + options.parameter_overrides({rclcpp::Parameter("visualization.frame_id", std::string("rear_axle"))}); + + auto node = std::make_shared(options); + REQUIRE(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE == node->configure().id()); + REQUIRE(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE == node->activate().id()); + + auto peer_node = std::make_shared("path_listener"); + nav_msgs::msg::Path received; + auto path_subscription = peer_node->create_subscription( + "projected_path", 1, [&received](const nav_msgs::msg::Path & msg) { received = msg; }); + auto cmd_vel_publisher = peer_node->create_publisher("cmd_vel", 1); + + const std::vector nodes = { + node->get_node_base_interface(), peer_node->get_node_base_interface()}; + + REQUIRE(spinUntil(nodes, [&path_subscription, &cmd_vel_publisher] { + return 0 < path_subscription->get_publisher_count() && 0 < cmd_vel_publisher->get_subscription_count(); + })); + + geometry_msgs::msg::TwistStamped command; + command.twist.linear.x = 1.0; + command.twist.angular.z = 0.3; + cmd_vel_publisher->publish(command); + REQUIRE(spinUntil(nodes, [&received] { return !received.poses.empty(); })); + + // The path is undecimated: one pose per projected sample, unlike the markers, which only carry a + // pose for samples whose footprint is set. + const std::vector projection = node->getLastProjection(); + REQUIRE(projection.size() == received.poses.size()); + CHECK(std::string("rear_axle") == received.header.frame_id); + + for (size_t index = 0; index < projection.size(); ++index) { + const geometry_msgs::msg::PoseStamped & pose = received.poses[index]; + CHECK(std::string("rear_axle") == pose.header.frame_id); + CHECK(pose.pose.position.x == Approx(projection[index].pose.x)); + CHECK(pose.pose.position.y == Approx(projection[index].pose.y)); + // Yaw-only orientation, so the quaternion is a unit half-angle pair about z. + CHECK(pose.pose.orientation.z == Approx(std::sin(projection[index].pose.theta / 2.0))); + CHECK(pose.pose.orientation.w == Approx(std::cos(projection[index].pose.theta / 2.0))); + CHECK(pose.pose.orientation.x == Approx(0.0)); + CHECK(pose.pose.orientation.y == Approx(0.0)); + } + + // A turning command must actually bend the path, not emit a straight line. + CHECK(std::abs(received.poses.back().pose.position.y) > 0.1); +} + +TEST_CASE("KinematicsNode publishes no markers for an unset footprint", "[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()); + + auto peer_node = std::make_shared("marker_listener"); + visualization_msgs::msg::MarkerArray received; + auto marker_subscription = peer_node->create_subscription( + "projected_footprints", 1, [&received](const visualization_msgs::msg::MarkerArray & msg) { received = msg; }); + auto cmd_vel_publisher = peer_node->create_publisher("cmd_vel", 1); + + const std::vector nodes = { + node->get_node_base_interface(), peer_node->get_node_base_interface()}; + + REQUIRE(spinUntil(nodes, [&marker_subscription, &cmd_vel_publisher] { + return 0 < marker_subscription->get_publisher_count() && 0 < cmd_vel_publisher->get_subscription_count(); + })); + + geometry_msgs::msg::TwistStamped command; + command.twist.linear.x = 1.0; + cmd_vel_publisher->publish(command); + REQUIRE(spinUntil(nodes, [&received] { return !received.markers.empty(); })); + + // The footprint parameters default to empty, leaving only the DELETEALL. + CHECK(1 == received.markers.size()); + CHECK(visualization_msgs::msg::Marker::DELETEALL == received.markers.front().action); +} + +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)); +}