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

polymath_kinematics_ros2/CMakeLists.txt

@@ -0,0 +1,118 @@
+# 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}
+    ${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::ArticulatedProjector"
+  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,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 <memory>
+#include <mutex>
+#include <vector>
+
+#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<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.
+  /// \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<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
+
+  /// 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<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,28 @@
+<?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>polymath_kinematics</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>sensor_msgs</depend>
+  <depend>magic_enum</depend>
+
+  <test_depend>catch2</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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]