Add a ros2 node that projects the vehicle's path into the future

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
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+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()

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.

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 <atomic>
+#include <memory>
+#include <mutex>
+#include <vector>
+
+#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<polymath::kinematics::ArticulatedProjectedState> 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<visualization_msgs::msg::MarkerArray> produceProjectedFootprintMarkers(
+    const std::vector<polymath::kinematics::ArticulatedProjectedState> & 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<nav_msgs::msg::Path> produceProjectedPath(
+    const std::vector<polymath::kinematics::ArticulatedProjectedState> & projection) const;
+
+  /// The underlying polymath_kinematics projector. Null until on_configure() succeeds.
+  std::unique_ptr<polymath::kinematics::ArticulatedProjector> projector_;
+
+  /// Subscriptions
+  rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_sub_;
+  rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr cmd_vel_sub_;
+
+  /// Publishers
+  rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr footprint_marker_pub_;
+  rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::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<bool> articulation_angle_seen_{false};
+
+  /// Most recent projection, one entry per time step including the initial state.
+  std::vector<polymath::kinematics::ArticulatedProjectedState> last_projection_;
+
+  /// Parameters
+  std::shared_ptr<articulated_projector::ParamListener> param_listener_;
+  articulated_projector::Params params_;
+};
+
+}  // namespace polymath::kinematics::ros2

polymath_kinematics_ros2/package.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>polymath_kinematics_ros2</name>
+  <version>0.3.0</version>
+  <description>ROS 2 layer over polymath_kinematics. Projects articulated-vehicle trajectories forward in time from the measured articulation angle and a commanded body velocity.</description>
+  <maintainer email="engineering@polymathrobotics.com">Polymath Engineering</maintainer>
+  <license>Apache-2.0</license>
+  <author email="zeerek@polymathrobotics.com">Zeerek Ahmad</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>generate_parameter_library</buildtool_depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>lifecycle_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>polymath_kinematics</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>magic_enum</depend>
+
+  <test_depend>catch2</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>